PID-NEO

m3508neo.cpp

#include "InterfaceCAN.h"
#include "PinNames.h"
#include "mbed.h"
#include <cstdint>
#include <cstdio>
#include <string>

#if !DEVICE_CAN
#error [NOT_SUPPORTED] CAN not supported for this target
#endif

double prev_input_UsinginputDelay;
double prev_output_UsinginputDelay;

double inputDelay(double input){
    double output = 0.04761904761904762*input + 0.04761904761904762* prev_input_UsinginputDelay + 0.9047619047619048* prev_output_UsinginputDelay;

    prev_input_UsinginputDelay = input;
    prev_output_UsinginputDelay = output;

    return output;
}

double deltaT = 4.0 / 1000;
double prev_error_speed;
double error_Integral_speed;
double Kp_speed = 6.5;
double Ki_speed = 0.0;
double Kd_speed = 0.0;

double PIDControl( double error){
    error_Integral_speed = (error + prev_error_speed)/2 * deltaT;

    double output = error*Kp_speed + error_Integral_speed*Ki_speed + (error - prev_error_speed)*Kd_speed;
    prev_error_speed = error;

    return output;
}

const double rpm2ampare = 0.29411764705882354;

double plantModel;
double prev_ampare;
double prev_output;

double Pm( double wantAmpare){
    plantModel = 0.085 * wantAmpare + 0.085 * prev_ampare + 0.95 * prev_output;

    prev_ampare = wantAmpare;
    prev_output = plantModel;

    return plantModel;
}

double prev_error_mec;
double Kp_mec = 1.0;
double Ki_mec = 0.0;
double Kd_mec = 0.0;

double Compensator( double error){
    double output = Kp_mec*error + Kd_mec*(error - prev_error_mec);

    prev_error_mec = error;

    return output;
}

CAN can(PB_8, PB_9, 1000 * 1000);

namespace motor_param{
    int16_t SET_SPEED;
    int16_t speed;
    int16_t power;
    int16_t p_gain;
    int16_t d_gain;
    int16_t err_last;
}

// Constants and Variables
constexpr int16_t MAX_POWER = 16000;
constexpr int16_t MIN_POWER = -16000;

Ticker flipper;
volatile bool newCanMessage = false;
uint8_t motor_send_count = 0;
volatile bool motor_data_send_flag = false;


void calculatePID() {
    double filteredRPM = inputDelay(500.0); //100.0 = target rpm
    double needRPM = filteredRPM + PIDControl(filteredRPM - motor_param::speed);
    double wantAmpare = needRPM * rpm2ampare;
    double modelValue = Pm( wantAmpare);
    double inputAmpare = wantAmpare + Compensator( modelValue - motor_param::speed);
    motor_param::power = inputAmpare;
    
    // // Saturation Logic
    if (motor_param::power > MAX_POWER) motor_param::power = MAX_POWER;
    if (motor_param::power < MIN_POWER) motor_param::power = MIN_POWER;
}

int counter_val = 0;

void flip() {
    calculatePID();
    newCanMessage = true;
    counter_val ++;
}


int main() {
    flipper.attach(&flip, 4ms);

    while (1) {
         CANMessage rcv_msg;
        if (can.read(rcv_msg) == 1) {
            if (rcv_msg.id == 0x201) {
                motor_param::speed  = static_cast<int16_t>((rcv_msg.data[2] << 8) | rcv_msg.data[3]);
            }
        }

        if (newCanMessage) {
            auto msg = CANMessage();
            msg.id = 0x200;
            msg.len = 8;
            msg.data[0] = (motor_param::power >> 8) & 0xff;
            msg.data[1] = motor_param::power & 0xff;
            can.write(msg);
            newCanMessage = false;
        }

        // if (counter_val > 3) {
        //     counter_val = 0;
        //     printf("%d",motor_param::speed);
        // }
    }
}