Revision c9fa414d devices/DiWheelDrive/linefollow.cpp
| devices/DiWheelDrive/linefollow.cpp | ||
|---|---|---|
| 120 | 120 |
|
| 121 | 121 |
case LineFollowStrategy::REVERSE: |
| 122 | 122 |
correctionSpeed = -getPidCorrectionSpeed(); |
| 123 |
rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -global->forwardSpeed; |
|
| 123 |
rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -1000000*global->forwardSpeed;
|
|
| 124 | 124 |
|
| 125 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -global->forwardSpeed; |
|
| 125 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -1000000*global->forwardSpeed;
|
|
| 126 | 126 |
|
| 127 | 127 |
break; |
| 128 | 128 |
|
| ... | ... | |
| 130 | 130 |
correctionSpeed = getPidCorrectionSpeed(); |
| 131 | 131 |
// chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed, global->threshWhite); |
| 132 | 132 |
|
| 133 |
rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = global->forwardSpeed + correctionSpeed; |
|
| 133 |
rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = 1000000*global->forwardSpeed + correctionSpeed;
|
|
| 134 | 134 |
|
| 135 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed; |
|
| 135 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = 1000000*global->forwardSpeed - correctionSpeed;
|
|
| 136 | 136 |
break; |
| 137 | 137 |
} |
| 138 | 138 |
return whiteFlag; |
| ... | ... | |
| 143 | 143 |
* Pid controller which returns a corrections speed. |
| 144 | 144 |
*/ |
| 145 | 145 |
int LineFollow::getPidCorrectionSpeed(){
|
| 146 |
int error = getError(); |
|
| 147 |
int sloap = oldError - error ; |
|
| 146 |
int32_t error = getError();
|
|
| 147 |
int32_t sloap = oldError - error ;
|
|
| 148 | 148 |
// int correctionSpeed = (int) (global->K_p*error + Ki*accumHist - global->K_d*sloap); |
| 149 |
int correctionSpeed = (int) (K_p*error + K_i*accumHist - K_d*sloap);
|
|
| 149 |
int32_t correctionSpeed = (int32_t) (K_p*error + K_i*accumHist + K_d*sloap);
|
|
| 150 | 150 |
oldError = error; |
| 151 |
// accumHist += (int) (0.01 * error);
|
|
| 151 |
accumHist += error;
|
|
| 152 | 152 |
if (abs(error) > global->maxDist.error){
|
| 153 | 153 |
global->maxDist.error = error; |
| 154 | 154 |
} |
Also available in: Unified diff