Revision c9fa414d devices/DiWheelDrive/userthread.cpp
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; |
Also available in: Unified diff