AMiRo-OS

#include <cmath>

void LineFollow::followLine(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){

    chprintf((BaseSequentialStream*) &SD1, "SP: %d,\n", SetPoint);

    // chprintf((BaseSequentialStream*) &SD1, "Proximity: WL:0x%04X FL:0x%04X FR:0x%04X WR:0x%04X\n",

    //                 vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],

    //                 vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],

    //                 vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],

    //                 vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);

    // global->motorcontrol.printGains();

    // chprintf((BaseSequentialStream*) &SD1, "Speed -- Left: %d, Right: %d\n", global->motorcontrol.getCurrentRPMLeft(), global->motorcontrol.getCurrentRPMRight());

    // float speedL = global->motorcontrol.getCurrentRPMLeft();

    // float speedR = global->motorcontrol.getCurrentRPMRight();

    // chprintf((BaseSequentialStream*) &SD1, "After motor request SP: %f,\n", SetPoint);

    // Process value

    float processV = static_cast< float >((vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] + vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]));

    // chprintf((BaseSequentialStream*) &SD1, "PV: %f,\n", processV);

    // chprintf((BaseSequentialStream*) &SD1, "After PV SP: %f,\n", SetPoint);

    float error = SetPoint - processV;

    float d_term = old_error - error;

    // chprintf((BaseSequentialStream*) &SD1, "After Error SP: %f,\n", SetPoint);

    // chprintf((BaseSequentialStream*) &SD1, "Error: %f,\n", error);

    acc_sum = 0.5 * acc_sum + error;

    int new_speed = static_cast< int >(Kp * error + Ki*acc_sum + Kd*d_term);

    old_error = error;

    chprintf((BaseSequentialStream*) &SD1, "Error: %f,\n", error);

    chprintf((BaseSequentialStream*) &SD1, "Dterm: %f,\n", d_term);

    chprintf((BaseSequentialStream*) &SD1, "Iterm: %f,\n", acc_sum);

    chprintf((BaseSequentialStream*) &SD1, "New Speed: %d,\n", new_speed);

    // chprintf((BaseSequentialStream*) &SD1, "New Speed: %f, Sum: %f, SP: %f, processV: %f, K_p: %f, K_i: %f \n", new_speed, acc_sum, SetPoint, processV, Kp, Ki);

    // int forward = 15;

    // int l_speed = forward - new_speed;

    // int r_speed = forward + new_speed;

    // if (l_speed )

    rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = 15  + new_speed;

    rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = 15  - new_speed;

    chprintf((BaseSequentialStream*) &SD1, "Speed L: %d, R: %d\n", rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL], rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL]);