Revision e0c5d17a devices/DiWheelDrive/linefollow.cpp
| devices/DiWheelDrive/linefollow.cpp | ||
|---|---|---|
| 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 |
} |
Also available in: Unified diff