David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
main.cpp
- Committer:
- DavidEGrayson
- Date:
- 2015-04-15
- Revision:
- 50:517c0f0e621f
- Parent:
- 49:eaa6fd514f4f
- Child:
- 51:b9f7243609d4
File content as of revision 50:517c0f0e621f:
#include <mbed.h> #include <Pacer.h> #include <GeneralDebouncer.h> #include <math.h> #include "main.h" #include "motors.h" #include "encoders.h" #include "leds.h" #include "pc_serial.h" #include "test.h" #include "reckoner.h" #include "buttons.h" #include "line_tracker.h" #include "l3g.h" #include "turn_sensor.h" Reckoner reckoner; LineTracker lineTracker; TurnSensor turnSensor; Logger logger; Pacer loggerPacer(50000); uint32_t totalEncoderCounts = 0; uint32_t nextLogEncoderCount = 0; const uint32_t logSpacing = 100; void setLeds(bool v1, bool v2, bool v3, bool v4) { led1 = v1; led2 = v2; led3 = v3; led4 = v4; } int __attribute__((noreturn)) main() { pc.baud(115200); // Enable pull-ups on encoder pins and give them a chance to settle. if (l3gInit()) { // Error initializing the gyro. setLeds(0, 0, 1, 1); while(1); } encodersInit(); motorsInit(); buttonsInit(); // Test routines //testMotors(); //testEncoders(); //testLineSensors(); //testReckoner(); //testButtons(); //testDriveHome(); //testFinalSettleIn(); //testCalibrate(); //testLineFollowing(); //testAnalog(); //testSensorGlitches(); //testTurnInPlace(); //testCloseness(); //testLogger(); //testL3g(); //testTurnSensor(); //testReckoningWithGyro(); // Real routines for the contest. loadCalibration(); setLeds(1, 0, 0, 0); waitForSignalToStart(); setLeds(0, 1, 0, 0); // led4 gets set when it detects the start //followLineFast(); followLineSmart(); setLeds(1, 1, 1, 1); loggerReportLoop(); } void loggerService() { // loggerPacer.pace() if (totalEncoderCounts > nextLogEncoderCount) { nextLogEncoderCount += logSpacing; struct LogEntry entry; entry.turnAngle = turnSensor.getAngle() >> 16; entry.x = reckoner.x >> 16; entry.y = reckoner.y >> 16; logger.log(&entry); } } void loggerReportLoop() { while(1) { if(button1DefinitelyPressed()) { logger.dump(); } } } void loadCalibration() { /** QTR-3RC **/ lineTracker.calibratedMinimum[0] = 137; lineTracker.calibratedMinimum[1] = 132; lineTracker.calibratedMinimum[2] = 154; lineTracker.calibratedMaximum[0] = 644; lineTracker.calibratedMaximum[1] = 779; lineTracker.calibratedMaximum[2] = 1000; /** QTR-3A lineTracker.calibratedMinimum[0] = 34872; lineTracker.calibratedMinimum[1] = 29335; lineTracker.calibratedMinimum[2] = 23845; lineTracker.calibratedMaximum[0] = 59726; lineTracker.calibratedMaximum[1] = 60110; lineTracker.calibratedMaximum[2] = 58446; **/ } void updateReckonerFromEncoders() { while(encoderBuffer.hasEvents()) { PololuEncoderEvent event = encoderBuffer.readEvent(); switch(event) { case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC: reckoner.handleTickLeftForward(); totalEncoderCounts++; break; case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC: reckoner.handleTickLeftBackward(); totalEncoderCounts--; break; case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC: reckoner.handleTickRightForward(); totalEncoderCounts++; break; case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC: reckoner.handleTickRightBackward(); totalEncoderCounts--; break; } } } 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: totalEncoderCounts++; reckoner.handleForward(); break; case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC: case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC: reckoner.handleBackward(); totalEncoderCounts--; break; } } } float magnitude() { return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y); } float dotProduct() { 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; } // 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.sinv / (1 << 30); float c = (float)reckoner.cosv / (1 << 30); return (reckoner.x * s - reckoner.y * c) / magnitude(); } int16_t reduceSpeed(int16_t speed, int32_t reduction) { if (reduction > speed) { return 0; } else { return speed - reduction; } } void waitForSignalToStart() { while(!button1DefinitelyPressed()) { updateReckonerFromEncoders(); } reckoner.reset(); while(button1DefinitelyPressed()) { updateReckonerFromEncoders(); } wait(0.2); } void updateMotorsToFollowLine() { const int16_t drivingSpeed = 400; const int32_t followLineStrength = drivingSpeed * 5 / 4; int16_t speedLeft = drivingSpeed; int16_t speedRight = drivingSpeed; int16_t reduction = (lineTracker.getLinePosition() - 1000) * followLineStrength / 1000; if(reduction < 0) { speedLeft = reduceSpeed(speedLeft, -reduction); } else { speedRight = reduceSpeed(speedRight, reduction); } motorsSpeedSet(speedLeft, speedRight); } void updateMotorsToFollowLineFast() { const int16_t drivingSpeed = 1000; const int32_t followLineStrength = drivingSpeed * 5 / 4; static int16_t lastPosition = 1000; int16_t position = lineTracker.getLinePosition(); int16_t speedLeft = drivingSpeed; int16_t speedRight = drivingSpeed; int32_t veer = (position - 1000) * followLineStrength / 1000 + (position - lastPosition) * 200; if(veer > 0) { speedRight = reduceSpeed(speedRight, veer); } else { speedLeft = reduceSpeed(speedLeft, -veer); } motorsSpeedSet(speedLeft, speedRight); lastPosition = position; } void followLineFast() { totalEncoderCounts = 0; Pacer reportPacer(200000); loadCalibration(); Timer timer; timer.start(); turnSensor.start(); while(1) { turnSensor.update(); updateReckoner(turnSensor); loggerService(); lineTracker.read(); updateMotorsToFollowLineFast(); if (button1DefinitelyPressed()) { break; } } motorsSpeedSet(0, 0); } bool foundStart() { static int16_t lastX = 0; return lastX < 0 && reckoner.x >= 0 && abs(reckoner.y) < (85 << 16) && totalEncoderCounts > 10000 && abs(turnSensor.getAngle()) < turnAngle1 * 30; lastX = reckoner.x; } void followLineSmart() { totalEncoderCounts = 0; Pacer reportPacer(200000); loadCalibration(); turnSensor.start(); while(1) { turnSensor.update(); updateReckoner(turnSensor); loggerService(); lineTracker.read(); updateMotorsToFollowLineFast(); if (foundStart()) { reckoner.reset(); turnSensor.reset(); totalEncoderCounts = 0; nextLogEncoderCount = 0; led4 = 1; } if (button1DefinitelyPressed()) { break; } } motorsSpeedSet(0, 0); }