AMiRo-OS

// 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 correctionSpeed = 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", correctionSpeed);

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

//     // int forward = 15;

//     int speedL = global->rpmForward[constants::DiWheelDrive::LEFT_WHEEL] - correctionSpeed;

//     int speedR = global->rpmForward[constants::DiWheelDrive::RIGHT_WHEEL] + correctionSpeed;

//     // if (l_speed )

//     rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = speedL;

//     rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = speedR;

//     chprintf((BaseSequentialStream*) &SD1, "Speed L: %d, R: %d\n", speedL, speedR);

// }

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

    int targetSensor = 0x38;

    int actualSensorL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] >> 8;

    int actualSensorR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] >> 8;

    int targetSpeedL = global->rpmForward[constants::DiWheelDrive::LEFT_WHEEL];

    int targetSpeedR = global->rpmForward[constants::DiWheelDrive::RIGHT_WHEEL];

    int error = targetSensor - (actualSensorL + actualSensorR);

    accSum += error;

    int dTerm = error - oldError;

    int correctionSpeed = (int) (Kp * error + Ki * accSum + Kd * dTerm);   

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

    rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = targetSpeedL + correctionSpeed;

    rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR - correctionSpeed;

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

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

//     int targetSensorL = 0x10;

//     int targetSensorR = 0x28;

//     float actualSpeedL = 20;

//     float actualSpeedR = 20;

//     // if(actualSpeedL == 0){

//     //     actualSpeedL = 1;

//     // }

//     // if(actualSpeedR == 0){

//     //     actualSpeedR = 1;

//     // }

//     // Shift sensor values to prevent overflow in following calculation

//     int actualSensorL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] >> 8;

//     int actualSensorR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] >> 8;

//     chprintf((BaseSequentialStream*) &SD1, "Sensor L: %d, R: %d\n", actualSensorL, actualSensorR);

//     int targetSpeedL = global->rpmForward[constants::DiWheelDrive::LEFT_WHEEL];

//     int targetSpeedR = global->rpmForward[constants::DiWheelDrive::RIGHT_WHEEL];

//     int setPointL = targetSensorL;

//     int setPointR = targetSensorR;

//     chprintf((BaseSequentialStream*) &SD1, "SetPoint L: %d, R: %d\n",setPointL, setPointR );

//     int processValueL =  actualSensorL;

//     int processValueR =  actualSensorR;

//     chprintf((BaseSequentialStream*) &SD1, "ProcessValue L: %d, R: %d\n",processValueL, processValueR );

//     int errorL = setPointL - processValueL;

//     int errorR = setPointR - processValueR;

//     // This will howfully decrease the overall speed when sensors deviate much 

//         // errorL /= targetSensorL+actualSensorL;

//         // errorR /= targetSensorR+actualSensorR;

//     chprintf((BaseSequentialStream*) &SD1, "Error L: %d, R: %d\n",errorL, errorR);

//     // int newSpeedL =  

//     rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = errorL;

//     rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = errorR;

//     int correction_speedL = (int) (Kp * errorL);   

//     int correction_speedR = (int) (Kp * errorR);   

//     chprintf((BaseSequentialStream*) &SD1, "Speed L: %d, R: %d\n",correction_speedL, correction_speedR);

//     // // 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] = targetSpeedL + correction_speedL;

//     rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR + correction_speedR;

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

// }

  void stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global);

  float Kd = 0.5

  ;

  int accSum = 0;

  float oldError = 0;

			lf.stableFollow(vcnl4020Proximity, rpmFuzzyCtrl, &global);

            setRpmSpeed(rpmFuzzyCtrl);

  const uint32_t SET_LINE_FOLLOW_SPEED     = 0x23;