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