David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.
Dependencies: PololuEncoder Pacer mbed GeneralDebouncer
main.cpp
- Committer:
- DavidEGrayson
- Date:
- 2019-08-13
- Revision:
- 48:597738b77f77
- Parent:
- 47:9773dc14c834
File content as of revision 48:597738b77f77:
#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" void __attribute__((noreturn)) doDeadReckoning(); Reckoner reckoner; LineTracker lineTracker; TurnSensor turnSensor; Logger logger; const uint32_t timeout = 0; const uint32_t logSpacing = 200; const int16_t drivingSpeed = 500; uint32_t totalEncoderCounts = 0; uint32_t nextLogEncoderCount = 0; 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); if (l3gInit()) { // Error initializing the gyro. setLeds(0, 0, 1, 1); while(1); } // Enable pull-ups on encoder pins and give them a chance to settle. encodersInit(); motorsInit(); buttonsInit(); // Test routines //testButtons(); //testEncoders(); //testMotors(); //testMotorSpeed(); //testL3g(); //testL3gAndShowAverage(); //testTurnSensor(); //testReckoner(); //testCloseness(); // didn't do it in 2019 //testDriveHome(); //testFinalSettleIn(); // doesn't really work //testLineSensorsAndCalibrate(); //testLineFollowing(); //testTurnInPlace(); // didn't do it in 2019 //testLogger(); // didn't do it in 2019 doDeadReckoning(); } void doDeadReckoning() { loadLineCalibration(); doGyroCalibration(); turnSensor.start(); setLeds(1, 0, 0, 0); waitForSignalToStart(); setLeds(0, 1, 0, 0); findLine(); setLeds(0, 0, 1, 0); followLineToEnd(); setLeds(1, 0, 1, 0); driveHomeAlmost(); //setLeds(0, 1, 1, 0); //finalSettleIn(); setLeds(1, 1, 1, 1); loggerReportLoop(); } void loggerService() { 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 doGyroCalibration() { wait_ms(1000); // Time for the robot to stop moving. while (!l3gCalibrateDone()) { l3gCalibrate(); wait_ms(2); } } void loadLineCalibration() { // QTR-3RC calibration from contest in 2014. //lineTracker.calibratedMinimum[0] = 100; //lineTracker.calibratedMaximum[0] = 792; //lineTracker.calibratedMinimum[1] = 94; //lineTracker.calibratedMaximum[1] = 807; //lineTracker.calibratedMinimum[2] = 103; //lineTracker.calibratedMaximum[2] = 1000; // QTR-3RC calibration from David's home setup, 2019-07-27. // Generated with testLineSensorsAndCalibrate(). lineTracker.calibratedMinimum[0] = 210; lineTracker.calibratedMaximum[0] = 757; lineTracker.calibratedMinimum[1] = 197; lineTracker.calibratedMaximum[1] = 1000; lineTracker.calibratedMinimum[2] = 203; lineTracker.calibratedMaximum[2] = 746; } void updateReckoner() { turnSensor.update(); reckoner.setTurnAngle(turnSensor.getAngle()); if (!encoderBuffer.hasEvents()) { return; } 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 << RECKONER_LOG_UNIT); float c = (float)reckoner.cosv / (1 << RECKONER_LOG_UNIT); 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() { float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT); float c = (float)reckoner.cosv / (1 << RECKONER_LOG_UNIT); 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()) { updateReckoner(); } reckoner.reset(); turnSensor.reset(); while(button1DefinitelyPressed()) { updateReckoner(); } wait(0.2); } // Keep the robot pointing the in the right direction (1, 0). // This should basically keep it going straight. void updateMotorsToDriveStraight() { const int32_t straightDriveStrength = 1000; int16_t speedLeft = drivingSpeed; int16_t speedRight = drivingSpeed; int32_t reduction = reckoner.sinv * straightDriveStrength >> RECKONER_LOG_UNIT; if (reduction > 0) { speedRight = reduceSpeed(speedRight, reduction); } else { speedLeft = reduceSpeed(speedLeft, -reduction); } motorsSpeedSet(speedLeft, speedRight); } void updateMotorsToFollowLine() { const int 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 findLine() { GeneralDebouncer lineStatus(10000); while(1) { lineTracker.read(); lineTracker.updateCalibration(); updateReckoner(); loggerService(); updateMotorsToDriveStraight(); lineStatus.update(lineTracker.getLineVisible()); if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 20000) { break; } } } void followLineToEnd() { Timer timer; timer.start(); GeneralDebouncer lineStatus(10000); const uint32_t lineDebounceTime = 1000000; while(1) { lineTracker.read(); updateReckoner(); loggerService(); lineStatus.update(lineTracker.getLineVisible()); bool lostLine = lineStatus.getState() == false && lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime; if (lostLine && timer.read_ms() >= 2000) { break; } if (timeout && timer.read_ms() > timeout) { break; } updateMotorsToFollowLine(); } } void driveHomeAlmost() { Timer timer; timer.start(); while(1) { updateReckoner(); loggerService(); float magn = magnitude(); if (magn < (1<<(14+7))) { // We are within 128 encoder ticks, so go to the next step. break; } float det = determinant(); int16_t speedLeft = drivingSpeed; int16_t speedRight = drivingSpeed; if (magn < (1<<(14+9))) // Within 512 encoder ticks of the origin, so slow down. { int16_t reduction = (1 - magn/(1<<(14+8))) * (drivingSpeed/2); speedLeft = reduceSpeed(speedLeft, reduction); speedRight = reduceSpeed(speedRight, reduction); } // tmphax if (0) { if (det != det) { // NaN setLeds(1, 0, 0, 1); } else if (det > 0.5) { setLeds(0, 0, 1, 1); } else if (det > 0.1) { setLeds(0, 0, 0, 1); } else if (det < -0.5) { setLeds(1, 1, 0, 0); } else if (det < -0.1) { setLeds(1, 0, 0, 0); } else { // Heading basically the right direction. setLeds(1, 1, 1, 1); } speedLeft = speedRight = 0; } if (det > 0) { speedLeft = reduceSpeed(speedLeft, det * 1000); } else { speedRight = reduceSpeed(speedRight, -det * 1000); } motorsSpeedSet(speedLeft, speedRight); } motorsSpeedSet(0, 0); } void finalSettleIn() { const int16_t settleSpeed = 300; const int16_t settleModificationStrength = 150; 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; Pacer reportPacer(200000); Pacer motorUpdatePacer(10000); float integral = 0; motorsSpeedSet(-settleSpeed, settleSpeed); // avoid waiting 10 ms before we start settling while(1) { led1 = (state == 1); updateReckoner(); loggerService(); 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() >= 2000 && reckoner.cosv > (1 << (RECKONER_LOG_UNIT - 1))) { // Stop turning and start trying to maintain the right position. state = 1; } if (state == 1 && timer.read_ms() >= 5000) { // Stop moving. break; } if (motorUpdatePacer.pace()) { int16_t rotationSpeed; if (state == 1) { float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT); integral += s; rotationSpeed = -(s * 2400 + integral * 20); if (rotationSpeed > 300) { rotationSpeed = 300; } if (rotationSpeed < -300) { rotationSpeed = -300; } } else { rotationSpeed = settleSpeed; } int16_t speedLeft = -rotationSpeed + speedModification; int16_t speedRight = rotationSpeed + speedModification; motorsSpeedSet(speedLeft, speedRight); } if (state == 1 && reportPacer.pace()) { pc.printf("%5d %5d %11d %11d | %11f %11f\r\n", reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y, determinant(), dotProduct()); } } // Done! Stop moving. motorsSpeedSet(0, 0); }