classes.cpp

#include <iostream>
#include "classes.h"
#include <cstdint>
#include <array>
#include <memory>
using namespace std;

//ControlInput
ControlInput::ControlInput(){
        cout << "Input is working.\n";
    }
int ControlInput::getter() {
        return input;
    }
void ControlInput::setter(int set) {
        input = set;
    }

//ControlOutput
ControlOutput::ControlOutput() {
        cout << "OutPut is working.\n";
    }
int ControlOutput::getter() {
        return Output;
    }
void ControlOutput::setter(int set) {
        Output = set;
    }

//VehicleState
VehicleState::VehicleState(): throttle(0), brake(0), steering(0) {
        cout << "VehicleState is working.\n";
    }

void VehicleState::setThrottle(float t){
    throttle = t;
}
void VehicleState::setBrake(float b){
    brake = b;
}
void VehicleState::setSteering(float s){
    steering = s;
}
float VehicleState::getBrake() {
    return brake;
}
float VehicleState::getThrottle() {
    return throttle;
}
float VehicleState::getSteering() {
    return steering;
}
//VehicleController
VehicleController::VehicleController() {
    
}
void VehicleController::update(){

}
VehicleState VehicleController::getVehicleState() {
    return state;
}
//PID Controller
PIDController::PIDController(double Kp, double Ki, double Kd, double Dt): Kp(Kp),Ki(Ki),Kd(Kd),Dt(Dt),integral(0.0),previous_error(0.0) {

}
double PIDController::compute(double target, double current) {

//error ammount
    double error = target - current;
//Proportional term
    double p_term = Kp *error;
//integral term
    integral += error * Dt;
    double i_term = integral * Ki; 
//Derivative term
    double derivative = (error - previous_error) / Dt;
    double d_term = Kd*derivative; 
//updating the state of the error;
    previous_error = error;
//Output storing to variable
    double output = p_term + i_term + d_term;
// Define your limits
    const double output_min = 0.0;    
    const double output_max = 200.0; 

// Clamp the output
    if (output > output_max) {
        output = output_max;
        integral = (output - p_term - d_term) / Ki;  // Force integral to value that would produce max output
    }
    else if (output < output_min) {
        output = output_min;
        integral = (output - p_term - d_term) / Ki;
    }
//output
    return output;

}

//CANBUS
uint8_t CanManeger::calculateChecksum(uint8_t* data, uint32_t len) {
    uint8_t checkSum = 0;
    for(int i =0 ; i < len ;i++) {
        checkSum ^= data[i];
    }
    return checkSum;
}

void CanManeger::packingCommandControl(float& throttle, float& brake, float& steering, CanFrame& frame) {
    //Data infos
    frame.dlc = 8;
    frame.id = 0x201;// exp: 0x201
    uint32_t packed_data = 0;

    //clamping
    if (throttle > 100){
        throttle = 100;
    } else if(throttle < 0) {
        throttle = 0;
    }
    if(brake > 100){
        brake = 100;
    } else if(brake < 0) {
        brake = 0;
    }
    if(steering > 45){
        steering = 45;
    } else if(steering < -45) {
        steering = -45;
    }
    //packing-bit the data 
    packed_data = packed_data | ((uint8_t)throttle & 0x7F);
    packed_data = packed_data | ((uint8_t)brake & 0x7F) << 7;
    packed_data = packed_data | (((uint8_t)(steering+45.0f)) & 0x7F) << 14;
    //clearing the data[]
    for(int i = 0 ; i < 8  ; i++){
        frame.data[i] = 0;
    }
    
    //Maping the data
    frame.data[0] = (uint8_t)(packed_data & 0xFF) ; 
    frame.data[1] = (uint8_t)((packed_data >> 7) & 0xFF);
    frame.data[2] = (uint8_t)((packed_data >> 14) & 0xFF) ;
    //Rolling attack
    frame.data[6] = msg_counter & 0xF;
    msg_counter++;
    if(msg_counter > 15) msg_counter = 0;
    //Checksum for the msg 0-6 bytes
    frame.data[7] = calculateChecksum(frame.data, 7);
}

//SafetyManeging

void SafetyManeger::setChecksum_fail(bool status) {
    CHECKSUM_FAIL = status;
}
void SafetyManeger::setCounter_fail(bool status) {
    COUNTER_FAIL = status;
}
void SafetyManeger::setTimeout_fail(bool status) {
    TIMEOUT_FAIL = status;
}
SafetyManeger::SafetyManeger(){
    currentState = SystemStates::STATE_NORMAL; //at the beggining of the program
}
SystemStates SafetyManeger::getCurrentState() {
    return currentState;
}
void SafetyManeger::update() {
    if(CHECKSUM_FAIL || COUNTER_FAIL || TIMEOUT_FAIL) {
        currentState = SystemStates::STATE_FAILSAFE;
        V_state.setBrake(MAX_BRAKE); // we created an object of the vehicleState  
    } else {
        currentState = SystemStates::STATE_NORMAL;
    }
}