| 1 |
|
#include "global.hpp"
|
|
1 |
// #include "global.hpp"
|
| 2 |
2 |
#include "linefollow.hpp"
|
| 3 |
3 |
#include <cmath>
|
| 4 |
|
|
|
4 |
#include <limits>
|
| 5 |
5 |
|
| 6 |
6 |
|
| 7 |
7 |
LineFollow::LineFollow(Global *global){
|
| ... | ... | |
| 57 |
57 |
// Get actual sensor data of both front sensors
|
| 58 |
58 |
int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
| 59 |
59 |
int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
| 60 |
|
int targetL = global->threshProxyL;
|
| 61 |
|
int targetR = global->threshProxyR;
|
|
60 |
// int targetL = global->linePID.threshProxyL;
|
|
61 |
// int targetR = global->linePID.threshProxyR;
|
|
62 |
int targetL = global->linePID.BThresh;
|
|
63 |
int targetR = global->linePID.WThresh;
|
| 62 |
64 |
int error = 0;
|
| 63 |
65 |
switch (this->strategy)
|
| 64 |
66 |
{
|
| ... | ... | |
| 70 |
72 |
break;
|
| 71 |
73 |
case LineFollowStrategy::MIDDLE:
|
| 72 |
74 |
// Assume that the smallest value means driving in the middle
|
| 73 |
|
targetL = targetR = !(targetL<targetR)?targetR:targetL;
|
| 74 |
|
error = (FL -targetL + FR - targetR);
|
|
75 |
// targetL = targetR = !(targetL<targetR)?targetR:targetL;
|
|
76 |
error = (FL -targetL + FR - targetL);
|
| 75 |
77 |
break;
|
| 76 |
78 |
case LineFollowStrategy::TRANSITION_L_R: case LineFollowStrategy::TRANSITION_R_L:
|
| 77 |
79 |
error = transitionError(FL, FR, targetL, targetR);
|
| ... | ... | |
| 135 |
137 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = 1000000*global->forwardSpeed - correctionSpeed;
|
| 136 |
138 |
break;
|
| 137 |
139 |
}
|
|
140 |
|
|
141 |
// Limit Speed
|
|
142 |
if ((rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] > MAX_CORRECTED_SPEED) || (rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] > MAX_CORRECTED_SPEED)){
|
|
143 |
rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] /= 2;
|
|
144 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] /= 2;
|
|
145 |
}
|
|
146 |
|
|
147 |
|
| 138 |
148 |
return whiteFlag;
|
| 139 |
149 |
}
|
| 140 |
150 |
|
| ... | ... | |
| 148 |
158 |
// int correctionSpeed = (int) (global->K_p*error + Ki*accumHist - global->K_d*sloap);
|
| 149 |
159 |
int32_t correctionSpeed = (int32_t) (K_p*error + K_i*accumHist + K_d*sloap);
|
| 150 |
160 |
oldError = error;
|
| 151 |
|
accumHist += error;
|
|
161 |
|
|
162 |
// Overflow Protection
|
|
163 |
if ((error > 0) && (accumHist > std::numeric_limits<int32_t>::max() - error)){
|
|
164 |
// Overflow detected
|
|
165 |
accumHist = std::numeric_limits<int32_t>::max();
|
|
166 |
} else if ((error < 0) && (accumHist < std::numeric_limits<int32_t>::min() - error)) {
|
|
167 |
// Underflow detected
|
|
168 |
accumHist = std::numeric_limits<int32_t>::min();
|
|
169 |
|
|
170 |
}else {
|
|
171 |
accumHist += error;
|
|
172 |
}
|
| 152 |
173 |
if (abs(error) > global->maxDist.error){
|
| 153 |
174 |
global->maxDist.error = error;
|
| 154 |
175 |
}
|