David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
main.cpp
- Committer:
- DavidEGrayson
- Date:
- 2014-02-24
- Revision:
- 18:b65fbb795396
- Parent:
- 17:2df9861f53ee
- Child:
- 19:a11ffc903774
File content as of revision 18:b65fbb795396:
#include <mbed.h> #include <Pacer.h> #include "motors.h" #include "encoders.h" #include "leds.h" #include "pc_serial.h" #include "test.h" #include "reckoner.h" #include "buttons.h" int __attribute__((noreturn)) main() { pc.baud(115200); // Enable pull-ups on encoder pins and give them a chance to settle. encodersInit(); motorsInit(); buttonsInit(); // Test routines //testMotors(); //testEncoders(); //testLineSensors(); //testReckoner(); //testButtons(); testDriveHome(); while(1) { } } void updateReckonerFromEncoders() { while(encoderBuffer.hasEvents()) { PololuEncoderEvent event = encoderBuffer.readEvent(); switch(event) { case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC: reckoner.handleTickLeftForward(); break; case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC: reckoner.handleTickLeftBackward(); break; case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC: reckoner.handleTickRightForward(); break; case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC: reckoner.handleTickRightBackward(); break; } } } // The closer this is to zero, the closer we are to pointing towards the home position. // It is basically a cross product of the two vectors (x, y) and (cos, sin). float det() { // 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); return reckoner.x * s - reckoner.y * c; } void __attribute__((noreturn)) driveHome() { led1 = 1; led2 = 1; led3 = 0; led4 = 0; // First, point the robot at the goal. bool dir = false; uint16_t transitions = 0; Timer timer; timer.start(); while(transitions < 100 || timer.read_ms() < 500) { updateReckonerFromEncoders(); { bool nextDir = det() > 0; if (nextDir != dir) { transitions++; } dir = nextDir; } if(dir) { led3 = 1; motorsSpeedSet(-300, 300); } else { led3 = 0; motorsSpeedSet(300, -300); } } motorsSpeedSet(0, 0); while(1) { } }