AMiRo-OS

#include "global.hpp"

#include "linefollow2.hpp" 

#include <cmath>

void LineFollow::printSensorData(){

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

}

LineFollow::LineFollow(Global *global){

    this->global = global;

}

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;

}

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);

}

int error(){

    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;

    return FL -targetL + FR - targetR;

}

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

    int targetSpeedL = 5;

    int targetSpeedR = 5;

    int delta_ = error();

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

    // global->senseRec[global->sensSamples].FL = FL;

    // global->senseRec[global->sensSamples].FR = FR;

    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++

    rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   -1*correctionSpeed;

    rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] =  correctionSpeed;

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

}