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