Revision 22b85da1 devices/DiWheelDrive/linefollow2.cpp

View differences:

devices/DiWheelDrive/linefollow2.cpp
2 2 
#include "linefollow2.hpp"
3 3 
#include <cmath>
4 4 

  		




  5
  
  
    

  		




  
  5
  
    
// Trash


  		




  6
  6
  
    
void LineFollow::printSensorData(){


  		




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


  		




  8
  8
  
    
}


  		




  9
  9
  
    

  		




  
  10
  
    

  		




  10
  11
  
    
LineFollow::LineFollow(Global *global){


  		




  11
  12
  
    
    this->global = global;


  		




  12
  13
  
    
}


  		




  13
  14
  
    

  		




  
  15
  
    
// trash


  		




  14
  16
  
    
int LineFollow::delta(){


  		




  15
  17
  
    
    int delta = 0;


  		




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


  		




  ......




  26
  28
  
    
    return  delta;


  		




  27
  29
  
    
}


  		




  28
  30
  
    

  		




  29
  
  
    

  		




  
  31
  
    
// old and trash


  		




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


  		




  31
  33
  
    
    int targetSensor = 0x38;


  		




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


  		




  ......




  53
  55
  
    

  		




  54
  56
  
    
}


  		




  55
  57
  
    

  		




  56
  
  
    
int error(){


  		




  
  58
  
    
/**


  		




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


  		




  
  60
  
    
 */


  		




  
  61
  
    
int LineFollow::getError(){


  		




  
  62
  
    
    


  		




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


  		




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


  		




  59
  65
  
    
    int targetL = global->threshProxyL;


  		




  60
  66
  
    
    int targetR = global->threshProxyR;


  		




  61
  
  
    
    return FL -targetL + FR - targetR;


  		




  
  67
  
    
    int error = FL -targetL + FR - targetR;


  		




  
  68
  
    

  		




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


  		




  
  70
  
    
        whiteFlag = 1;


  		




  
  71
  
    
    }else{


  		




  
  72
  
    
        whiteFlag = 0;


  		




  
  73
  
    
    }


  		




  
  74
  
    
    return error;


  		




  
  75
  
    
}


  		




  
  76
  
    

  		




  
  77
  
    
/**


  		




  
  78
  
    
 * Follow strategy for left edge. 


  		




  
  79
  
    
 */


  		




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


  		




  
  81
  
    

  		




  
  82
  
    
    int correctionSpeed = getPidCorrectionSpeed();


  		




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


  		




  
  84
  
    

  		




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


  		




  
  86
  
    

  		




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


  		




  
  88
  
    
    return whiteFlag;


  		




  
  89
  
    
}


  		




  
  90
  
    

  		




  
  91
  
    
/**


  		




  
  92
  
    
 * Follow strategy for right edge. 


  		




  
  93
  
    
 */


  		




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


  		




  
  95
  
    

  		




  
  96
  
    
    int correctionSpeed = getPidCorrectionSpeed();


  		




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


  		




  
  98
  
    

  		




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


  		




  
  100
  
    

  		




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


  		




  
  102
  
    
    return whiteFlag;


  		




  
  103
  
    
}


  		




  
  104
  
    

  		




  
  105
  
    
/**


  		




  
  106
  
    
 * Pid controller which returns a corrections speed.


  		




  
  107
  
    
 */


  		




  
  108
  
    
int LineFollow::getPidCorrectionSpeed(){


  		




  
  109
  
    
    int error = getError();


  		




  
  110
  
    
    int sloap = error - global->oldError;


  		




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


  		




  
  112
  
    
    global->oldError = error;


  		




  
  113
  
    
    global->accumHist += error;


  		




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


  		




  
  115
  
    
        global->maxDist.error = error;


  		




  
  116
  
    
    }


  		




  
  117
  
    
    return correctionSpeed;


  		




  62
  118
  
    
}


  		




  63
  119
  
    

  		




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


  		




  65
  
  
    
    int targetSpeedL = 5;


  		




  66
  
  
    
    int targetSpeedR = 5;


  		




  67
  
  
    
    int delta_ = error();


  		




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


  		




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


  		




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


  		




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


  		




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


  		




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


  		




  74
  
  
    
    global->sensSamples++


  		




  75
  
  
    

  		




  76
  
  
    

  		




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


  		




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


  		




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


  		




  80
  
  
    
}


  		




  
  120
  
    
// trash


  		




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


  		




  
  122
  
    
//     int targetSpeedL = 5;


  		




  
  123
  
    
//     int targetSpeedR = 5;


  		




  
  124
  
    
//     int delta_ = error();


  		




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


  		




  
  126
  
    
//     if (global->enableRecord){


  		




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


  		




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


  		




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


  		




  
  130
  
    
//         global->sensSamples++;


  		




  
  131
  
    
//         }


  		




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


  		




  
  133
  
    
//         global->maxDist.error = delta_;


  		




  
  134
  
    
//     }


  		




  
  135
  
    

  		




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


  		




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


  		




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


  		




  
  139
  
    
// }


  		












Also available in:  Unified diff