AMiRo-OS

#include "global.hpp"

#include "linefollow2.hpp" 

#include <cmath>

// Trash

void LineFollow::printSensorData(){

    chprintf((BaseSequentialStream*) &SD1, "Test!");

}

LineFollow::LineFollow(Global *global){

    this->global = global;

}

// trash

int LineFollow::delta(){

    int delta = 0;

    int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();

    int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();

    delta = abs(abs(global->threshProxyL-global->threshProxyR) - abs(FL-FR));

    if (FR > global->threshProxyR && FL > global->threshProxyL ){

        return delta ;

    }else {

        return delta* -1;

    }

    return  delta;

}

// old and trash

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

    int targetSensor = 0x38;

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

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

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

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

    int diff = actualSensorR - actualSensorL; 

    int error = targetSensor - (actualSensorL + actualSensorR);

    accSum += error;

    int dTerm = error - oldError;

    if (diff > biggestDiff){

        biggestDiff = diff;

    }

    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;

    chprintf((BaseSequentialStream*) &SD1, "Diff: %d, Biggest: %d\n", correctionSpeed, biggestDiff);

}

/**

 * Calculate the error from front proxi sensors and fixed threshold values for those sensors.

 */

int LineFollow::getError(){

    int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();

    int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();

    int targetL = global->threshProxyL;

    int targetR = global->threshProxyR;

    int error = FL -targetL + FR - targetR;

    if (FL+FR > global->threshWhite){

        whiteFlag = 1;

    }else{

        whiteFlag = 0;

    }

    return error;

}

/**

 * Follow strategy for left edge. 

 */

int LineFollow::followLeftEdge(int rpmSpeed[2]){

    int correctionSpeed = getPidCorrectionSpeed();

    chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);

    rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + correctionSpeed;

    rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed;

    return whiteFlag;

}

/**

 * Follow strategy for right edge. 

 */

int LineFollow::followRightEdge(int rpmSpeed[2]){

    int correctionSpeed = getPidCorrectionSpeed();

    chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);

    rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed - correctionSpeed;

    rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed + correctionSpeed;

    return whiteFlag;

}

/**

 * Pid controller which returns a corrections speed.

 */

int LineFollow::getPidCorrectionSpeed(){

    int error = getError();

    int sloap = error - global->oldError;

    int correctionSpeed = (int) (global->K_p*error + global->K_i*global->accumHist + global->K_d*sloap);

    global->oldError = error;

    global->accumHist += error;

    if (abs(error) > global->maxDist.error){

        global->maxDist.error = error;

    }

    return correctionSpeed;

}

// trash

// void LineFollow::calibrateZiegler(float KCrit, int rpmSpeed[2]){

//     int targetSpeedL = 5;

//     int targetSpeedR = 5;

//     int delta_ = error();

//     int correctionSpeed = (int) (KCrit * delta_);   

//     if (global->enableRecord){

//         global->senseRec[global->sensSamples].error = delta_;

//         global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();

//         global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();

//         global->sensSamples++;

//         }

//     if (abs(delta_) > global->maxDist.error){

//         global->maxDist.error = delta_;

//     }

//     rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + -1*correctionSpeed;

//     rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed + correctionSpeed;

//     chprintf((BaseSequentialStream*) &SD1, "CS:%d,LW:%d,RW:%d\n", correctionSpeed, rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]);

// }