David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: main.cpp
- Revision:
- 46:f11cb4f93aac
- Parent:
- 45:e16e74bbbf8c
- Child:
- 48:c84b7b3ab0e8
diff -r e16e74bbbf8c -r f11cb4f93aac main.cpp --- a/main.cpp Wed Apr 15 19:19:19 2015 +0000 +++ b/main.cpp Wed Apr 15 21:19:22 2015 +0000 @@ -60,7 +60,8 @@ //testCloseness(); //testLogger(); //testL3g(); - testTurnSensor(); + //testTurnSensor(); + //testReckoningWithGyro(); // Real routines for the contest. loadCalibration(); @@ -138,6 +139,32 @@ } } +void updateReckoner(TurnSensor & turnSensor) +{ + if (!encoderBuffer.hasEvents()) + { + return; + } + + reckoner.setTurnAngle(turnSensor.getAngle()); + + while(encoderBuffer.hasEvents()) + { + PololuEncoderEvent event = encoderBuffer.readEvent(); + switch(event) + { + case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC: + case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC: + reckoner.handleForward(); + break; + case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC: + case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC: + reckoner.handleBackward(); + break; + } + } +} + float magnitude() { return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y); @@ -145,8 +172,8 @@ float dotProduct() { - float s = (float)reckoner.sin / (1 << 30); - float c = (float)reckoner.cos / (1 << 30); + float s = (float)reckoner.sinv / (1 << 30); + float c = (float)reckoner.cosv / (1 << 30); float magn = magnitude(); if (magn == 0){ return 0; } return ((float)reckoner.x * c + (float)reckoner.y * s) / magn; @@ -157,8 +184,8 @@ float determinant() { // TODO: get rid of the magic numbers here (i.e. 30) - float s = (float)reckoner.sin / (1 << 30); - float c = (float)reckoner.cos / (1 << 30); + float s = (float)reckoner.sinv / (1 << 30); + float c = (float)reckoner.cosv / (1 << 30); return (reckoner.x * s - reckoner.y * c) / magnitude(); } @@ -241,10 +268,13 @@ uint32_t loopCount = 0; Timer timer; timer.start(); + TurnSensor turnSensor; + turnSensor.start(); while(1) { loopCount += 1; - updateReckonerFromEncoders(); + turnSensor.update(); + updateReckoner(turnSensor); loggerService(); if ((loopCount % 256) == 0)