Revision e2002d0e devices/DiWheelDrive/userthread.cpp
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 99 | 99 |
return success; |
| 100 | 100 |
} |
| 101 | 101 |
|
| 102 |
uint16_t UserThread::getProxyRingSum(){
|
|
| 103 |
uint16_t prox_sum = 0; |
|
| 104 |
for(int i=0; i<8;i++){
|
|
| 105 |
prox_sum += global.robot.getProximityRingValue(i);; |
|
| 106 |
} |
|
| 107 |
return prox_sum; |
|
| 108 |
} |
|
| 109 |
|
|
| 110 |
|
|
| 102 | 111 |
UserThread::UserThread() : |
| 103 | 112 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
| 104 | 113 |
{
|
| ... | ... | |
| 119 | 128 |
states newState; |
| 120 | 129 |
|
| 121 | 130 |
int whiteBuf = 0; |
| 131 |
int proxyBuf = 0; |
|
| 122 | 132 |
int correctionStep = 0; |
| 123 | 133 |
int rpmSpeed[2] = {0};
|
| 124 | 134 |
int stop[2] = {0};
|
| ... | ... | |
| 228 | 238 |
whiteBuf = 0; |
| 229 | 239 |
setRpmSpeed(rpmSpeed); |
| 230 | 240 |
} |
| 241 |
|
|
| 242 |
if(getProxyRingSum() > PROXY_RING_THRESH){
|
|
| 243 |
setRpmSpeed(stop); |
|
| 244 |
proxyBuf++; |
|
| 245 |
if(proxyBuf > WHITE_COUNT_THRESH){
|
|
| 246 |
newState = states::IDLE; |
|
| 247 |
} |
|
| 248 |
}else{
|
|
| 249 |
proxyBuf = 0; |
|
| 250 |
} |
|
| 231 | 251 |
// lf.followLine(rpmSpeed); |
| 232 | 252 |
// setRpmSpeed(rpmSpeed); |
| 233 | 253 |
|
Also available in: Unified diff