Revision e2002d0e
| 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 |
|
| devices/DiWheelDrive/userthread.hpp | ||
|---|---|---|
| 12 | 12 |
#define MAX_CORRECTION_STEPS 250 |
| 13 | 13 |
// Thresh for wheel proxy sensors, when summed values fall below the state changes |
| 14 | 14 |
#define PROXY_WHEEL_THRESH 18000 |
| 15 |
// Thresh for detecting obsticles |
|
| 16 |
#define PROXY_RING_THRESH 20000 |
|
| 15 | 17 |
// Thresh for how long (update steps) the front sensors are allowed to detect white |
| 16 | 18 |
#define WHITE_COUNT_THRESH 120 |
| 17 | 19 |
// Rotation around 180 degrees in microradian |
| ... | ... | |
| 71 | 73 |
void checkForMotion(); |
| 72 | 74 |
bool checkPinVoltage(); |
| 73 | 75 |
bool checkPinEnabled(); |
| 76 |
uint16_t getProxyRingSum(); |
|
| 74 | 77 |
|
| 75 | 78 |
/** |
| 76 | 79 |
* Check if current position changes when the wheel are deactivated. |
Also available in: Unified diff