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-27
- Revision:
- 20:dbec34f0e76b
- Parent:
- 19:a11ffc903774
- Child:
- 21:c279c6a83671
File content as of revision 20:dbec34f0e76b:
#include <mbed.h> #include <Pacer.h> #include <math.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(); testFinalSettleIn(); 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; } } } float magnitude() { return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y); } float dotProduct() { float s = (float)reckoner.sin / (1 << 30); float c = (float)reckoner.cos / (1 << 30); float magn = magnitude(); if (magn == 0){ return 0; } return ((float)reckoner.x * c + (float)reckoner.y * s) / magn; } // 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 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); return (reckoner.x * s - reckoner.y * c) / magnitude(); } int16_t reduceSpeed(int16_t speed, int16_t reduction) { if (reduction > speed) { return 0; } else { return speed - reduction; } } void driveHomeAlmost(); void finalSettleIn(); void driveHome() { driveHomeAlmost(); finalSettleIn(); } void driveHomeAlmost() { led1 = 1; led2 = 1; led3 = 0; led4 = 0; bool dir = false; uint16_t transitions = 0; Timer timer; timer.start(); const int16_t maxSpeed = 300; while(1) { updateReckonerFromEncoders(); float magn = magnitude(); if (magn < (1<<17)) { // We are within 8 encoder ticks, so go to the next step. break; } float det = determinant(); { bool nextDir = det > 0; if (nextDir != dir) { transitions++; } dir = nextDir; } int16_t speedLeft = maxSpeed; int16_t speedRight = maxSpeed; if (magn < (1<<20)) // Within 64 encoder ticks of the origin, so slow down. { int16_t reduction = (1 - magn/(1<<20)) * maxSpeed; speedLeft = reduceSpeed(speedLeft, reduction); speedRight = reduceSpeed(speedRight, reduction); } if (det > 0) { speedLeft = reduceSpeed(speedLeft, det * 1000); } else { speedRight = reduceSpeed(speedRight, -det * 1000); } motorsSpeedSet(speedLeft, speedRight); } motorsSpeedSet(0, 0); } void finalSettleIn() { led1 = 1; led2 = 1; led3 = 1; led4 = 0; const int16_t settleSpeed = 200; const int16_t settleModificationStrength = 100; Timer timer; timer.start(); // State 0: rotating // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0]. uint8_t state = 0; while(1) { updateReckonerFromEncoders(); float dot = dotProduct(); int16_t speedModification = -dot * settleModificationStrength; if (speedModification > settleModificationStrength) { speedModification = settleModificationStrength; } else if (speedModification < -settleModificationStrength) { speedModification = -settleModificationStrength; } if (state == 0 && timer.read_ms() >= 6000 && reckoner.cos > (1 << 29)) { led1 = 1; led2 = 0; led3 = 1; led4 = 0; state = 1; } int16_t rotationSpeed; if (state == 1) { float s = (float)reckoner.sin / (1 << 30); rotationSpeed = -s * 300; } else { rotationSpeed = settleSpeed; } int16_t speedLeft = -rotationSpeed + speedModification; int16_t speedRight = rotationSpeed + speedModification; motorsSpeedSet(speedLeft, speedRight); } led1 = 1; led2 = 1; led3 = 1; led4 = 1; motorsSpeedSet(0, 0); }