Revision c9fa414d
| 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 |
} |
| devices/DiWheelDrive/linefollow.hpp | ||
|---|---|---|
| 116 | 116 |
int trans = 0; |
| 117 | 117 |
|
| 118 | 118 |
// PID controller components --------------- |
| 119 |
float K_p = 0.002;
|
|
| 120 |
float K_i = 0; |
|
| 121 |
float K_d = 0; |
|
| 122 |
int accumHist = 0; |
|
| 119 |
int32_t K_p = 1400;
|
|
| 120 |
float K_i = 0.06;
|
|
| 121 |
float K_d = 0.2;
|
|
| 122 |
int32_t accumHist = 0;
|
|
| 123 | 123 |
float oldError = 0; |
| 124 | 124 |
// ---------------------------------------- |
| 125 | 125 |
}; |
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 16 | 16 |
* |
| 17 | 17 |
* @param rpmSpeed speed for left and right wheel in rounds/min |
| 18 | 18 |
*/ |
| 19 |
void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) {
|
|
| 19 |
void UserThread::setRpmSpeedFuzzy(const int (&rpmSpeed)[2]) {
|
|
| 20 | 20 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
| 21 | 21 |
} |
| 22 | 22 |
|
| 23 |
void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) {
|
|
| 24 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]); |
|
| 25 |
} |
|
| 26 |
|
|
| 23 | 27 |
void UserThread::lightOneLed(Color color, int idx){
|
| 24 | 28 |
global.robot.setLightColor(idx, Color(color)); |
| 25 | 29 |
} |
| ... | ... | |
| 99 | 103 |
return success; |
| 100 | 104 |
} |
| 101 | 105 |
|
| 102 |
uint16_t UserThread::getProxyRingSum(){
|
|
| 103 |
uint16_t prox_sum = 0;
|
|
| 106 |
int UserThread::getProxyRingSum(){
|
|
| 107 |
int prox_sum = 0;
|
|
| 104 | 108 |
for(int i=0; i<8;i++){
|
| 105 | 109 |
prox_sum += global.robot.getProximityRingValue(i);; |
| 106 | 110 |
} |
| ... | ... | |
| 138 | 142 |
int rpmSpeed[2] = {0};
|
| 139 | 143 |
int stop[2] = {0};
|
| 140 | 144 |
int turn[2] = {5,-5};
|
| 141 |
LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY;
|
|
| 145 |
LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;
|
|
| 142 | 146 |
for (uint8_t led = 0; led < 8; ++led) {
|
| 143 | 147 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
| 144 | 148 |
} |
| ... | ... | |
| 276 | 280 |
// whiteBuf = 0; |
| 277 | 281 |
// proxyBuf = 0; |
| 278 | 282 |
// lf.followLine(rpmSpeed); |
| 279 |
setRpmSpeed(rpmSpeed); |
|
| 283 |
if (lf.getStrategy() == LineFollowStrategy::FUZZY){
|
|
| 284 |
setRpmSpeedFuzzy(rpmSpeed); |
|
| 285 |
}else{
|
|
| 286 |
|
|
| 287 |
setRpmSpeed(rpmSpeed); |
|
| 288 |
} |
|
| 280 | 289 |
|
| 281 | 290 |
break; |
| 282 | 291 |
// --------------------------------------- |
| ... | ... | |
| 373 | 382 |
}else{
|
| 374 | 383 |
// No voltage on pins -> falsely docked |
| 375 | 384 |
// deactivate pins |
| 385 |
if (!global.motorcontrol.getMotorEnable()){
|
|
| 386 |
global.motorcontrol.toggleMotorEnable(); |
|
| 387 |
} |
|
| 376 | 388 |
global.robot.requestCharging(0); |
| 377 | 389 |
if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
|
| 378 | 390 |
newState = states::RELEASE_TO_CORRECT; |
| devices/DiWheelDrive/userthread.hpp | ||
|---|---|---|
| 71 | 71 |
|
| 72 | 72 |
private: |
| 73 | 73 |
void setRpmSpeed(const int (&rpmSpeed)[2]); |
| 74 |
void setRpmSpeedFuzzy(const int (&rpmSpeed)[2]); |
|
| 74 | 75 |
void lightOneLed(Color color, int idx); |
| 75 | 76 |
void lightAllLeds(Color color); |
| 76 | 77 |
/** |
| ... | ... | |
| 80 | 81 |
void checkForMotion(); |
| 81 | 82 |
bool checkPinVoltage(); |
| 82 | 83 |
bool checkPinEnabled(); |
| 83 |
uint16_t getProxyRingSum(); |
|
| 84 |
int getProxyRingSum(); |
|
| 85 |
|
|
| 84 | 86 |
|
| 85 | 87 |
/** |
| 86 | 88 |
* Check if current position changes when the wheel are deactivated. |
Also available in: Unified diff