amiro-os / devices / DiWheelDrive / linefollow2.cpp @ 88afb834
History | View | Annotate | Download (3.113 KB)
| 1 | c76baf23 | Georg Alberding | #include "global.hpp" |
|---|---|---|---|
| 2 | #include "linefollow2.hpp" |
||
| 3 | 2330e415 | Georg Alberding | #include <cmath> |
| 4 | c76baf23 | Georg Alberding | |
| 5 | |||
| 6 | void LineFollow::printSensorData(){
|
||
| 7 | chprintf((BaseSequentialStream*) &SD1, "Test!");
|
||
| 8 | } |
||
| 9 | |||
| 10 | 12463563 | galberding | LineFollow::LineFollow(Global *global){
|
| 11 | this->global = global;
|
||
| 12 | } |
||
| 13 | |||
| 14 | af93a91c | galberding | int LineFollow::delta(){
|
| 15 | int delta = 0; |
||
| 16 | int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
||
| 17 | int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
||
| 18 | |||
| 19 | delta = abs(abs(global->threshProxyL-global->threshProxyR) - abs(FL-FR)); |
||
| 20 | |||
| 21 | if (FR > global->threshProxyR && FL > global->threshProxyL ){
|
||
| 22 | 88afb834 | galberding | return delta ;
|
| 23 | af93a91c | galberding | }else {
|
| 24 | 88afb834 | galberding | return delta* -1; |
| 25 | af93a91c | galberding | } |
| 26 | return delta;
|
||
| 27 | } |
||
| 28 | 25388c2f | Georg Alberding | |
| 29 | |||
| 30 | void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){ |
||
| 31 | int targetSensor = 0x38; |
||
| 32 | b8085493 | Georg Alberding | int actualSensorL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] ;
|
| 33 | int actualSensorR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] ;
|
||
| 34 | 25388c2f | Georg Alberding | int targetSpeedL = global->rpmForward[constants::DiWheelDrive::LEFT_WHEEL];
|
| 35 | int targetSpeedR = global->rpmForward[constants::DiWheelDrive::RIGHT_WHEEL];
|
||
| 36 | |||
| 37 | b8085493 | Georg Alberding | |
| 38 | int diff = actualSensorR - actualSensorL;
|
||
| 39 | 25388c2f | Georg Alberding | int error = targetSensor - (actualSensorL + actualSensorR);
|
| 40 | |||
| 41 | accSum += error; |
||
| 42 | int dTerm = error - oldError;
|
||
| 43 | |||
| 44 | b8085493 | Georg Alberding | if (diff > biggestDiff){
|
| 45 | biggestDiff = diff; |
||
| 46 | } |
||
| 47 | 25388c2f | Georg Alberding | int correctionSpeed = (int) (Kp * error + Ki * accSum + Kd * dTerm); |
| 48 | chprintf((BaseSequentialStream*) &SD1, "Correction Speed: %d\n", correctionSpeed);
|
||
| 49 | rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = targetSpeedL + correctionSpeed; |
||
| 50 | rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR - correctionSpeed; |
||
| 51 | 2330e415 | Georg Alberding | |
| 52 | b8085493 | Georg Alberding | chprintf((BaseSequentialStream*) &SD1, "Diff: %d, Biggest: %d\n", correctionSpeed, biggestDiff);
|
| 53 | |||
| 54 | c76baf23 | Georg Alberding | } |
| 55 | 25388c2f | Georg Alberding | |
| 56 | 88afb834 | galberding | int error(){
|
| 57 | int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
||
| 58 | int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
||
| 59 | int targetL = global->threshProxyL;
|
||
| 60 | int targetR = global->threshProxyR;
|
||
| 61 | return FL -targetL + FR - targetR;
|
||
| 62 | } |
||
| 63 | 12463563 | galberding | |
| 64 | af93a91c | galberding | void LineFollow::calibrateZiegler(float KCrit, int rpmSpeed[2]){ |
| 65 | int targetSpeedL = 5; |
||
| 66 | int targetSpeedR = 5; |
||
| 67 | 88afb834 | galberding | 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 | af93a91c | galberding | } |