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
Revision 43:0e985a58f174, committed 2019-07-27
- Comitter:
- DavidEGrayson
- Date:
- Sat Jul 27 22:52:19 2019 +0000
- Parent:
- 42:96671b71aac5
- Child:
- 44:b4a00fbab06b
- Commit message:
- Changed reckoner to use readings from turnSensor (Gyro) to get its direction vector instead of encoder ticks.
Changed in this revision
--- a/main.cpp Sat Jul 27 20:58:46 2019 +0000 +++ b/main.cpp Sat Jul 27 22:52:19 2019 +0000 @@ -59,15 +59,13 @@ //testLineSensors(); //testL3g(); //testL3gAndShowAverage(); - testTurnSensor(); - //testReckoner(); + //testTurnSensor(); + testReckoner(); //testButtons(); //testDriveHome(); //testFinalSettleIn(); //testCalibrate(); //testLineFollowing(); - //testAnalog(); - //testSensorGlitches(); //testTurnInPlace(); //testCloseness(); //testLogger(); @@ -148,27 +146,28 @@ lineTracker.calibratedMaximum[2] = 1000; } -void updateReckonerFromEncoders() +void updateReckoner() { + if (!encoderBuffer.hasEvents()) + { + return; + } + + reckoner.setTurnAngle(turnSensor.getAngle()); + while(encoderBuffer.hasEvents()) { PololuEncoderEvent event = encoderBuffer.readEvent(); switch(event) { case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC: - reckoner.handleTickLeftForward(); + case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC: totalEncoderCounts++; + reckoner.handleForward(); 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(); + reckoner.handleBackward(); totalEncoderCounts--; break; } @@ -182,8 +181,8 @@ float dotProduct() { - float s = (float)reckoner.sin / (1 << 30); - float c = (float)reckoner.cos / (1 << 30); + 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; @@ -193,9 +192,8 @@ // 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); + 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(); } @@ -215,12 +213,12 @@ { while(!button1DefinitelyPressed()) { - updateReckonerFromEncoders(); + updateReckoner(); } reckoner.reset(); while(button1DefinitelyPressed()) { - updateReckonerFromEncoders(); + updateReckoner(); } wait(0.2); } @@ -232,7 +230,7 @@ const int32_t straightDriveStrength = 1000; int16_t speedLeft = drivingSpeed; int16_t speedRight = drivingSpeed; - int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15); + int32_t reduction = reckoner.sinv * straightDriveStrength >> RECKONER_LOG_UNIT; if (reduction > 0) { speedRight = reduceSpeed(speedRight, reduction); @@ -270,7 +268,7 @@ { lineTracker.read(); lineTracker.updateCalibration(); - updateReckonerFromEncoders(); + updateReckoner(); loggerService(); updateMotorsToDriveStraight(); lineStatus.update(lineTracker.getLineVisible()); @@ -311,7 +309,7 @@ while(1) { lineTracker.read(); - updateReckonerFromEncoders(); + updateReckoner(); loggerService(); lineStatus.update(lineTracker.getLineVisible()); @@ -334,7 +332,7 @@ while(1) { - updateReckonerFromEncoders(); + updateReckoner(); loggerService(); float magn = magnitude(); @@ -393,7 +391,7 @@ { led1 = (state == 1); - updateReckonerFromEncoders(); + updateReckoner(); loggerService(); float dot = dotProduct(); @@ -407,7 +405,8 @@ speedModification = -settleModificationStrength; } - if (state == 0 && timer.read_ms() >= 2000 && reckoner.cos > (1 << 29)) + 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; @@ -424,7 +423,7 @@ int16_t rotationSpeed; if (state == 1) { - float s = (float)reckoner.sin / (1 << 30); + float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT); integral += s; rotationSpeed = -(s * 2400 + integral * 20); @@ -449,8 +448,8 @@ if (state == 1 && reportPacer.pace()) { - pc.printf("%11d %11d %11d %11d | %11f %11f\r\n", - reckoner.cos, reckoner.sin, reckoner.x, reckoner.y, + pc.printf("%5d %5d %11d %11d | %11f %11f\r\n", + reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y, determinant(), dotProduct()); } }
--- a/main.h Sat Jul 27 20:58:46 2019 +0000 +++ b/main.h Sat Jul 27 22:52:19 2019 +0000 @@ -17,7 +17,7 @@ void __attribute__((noreturn)) loggerReportLoop(); void updateMotorsToFollowLine(); -void updateReckonerFromEncoders(); +void updateReckoner(); void setLeds(bool v1, bool v2, bool v3, bool v4); float determinant(); float dotProduct();
--- a/reckoner.cpp Sat Jul 27 20:58:46 2019 +0000 +++ b/reckoner.cpp Sat Jul 27 22:52:19 2019 +0000 @@ -1,106 +1,34 @@ #include <mbed.h> #include "reckoner.h" -/** -First, we define two int32_t variables cos and sin that make a unit vector. -These variables hold our current estimation of the direction the robot is pointed. -We need to choose units for them that allow for a lot of accuracy without overflowing. -We choose the units to be 1/(1 << 30) so that they will typically be about half of the -way between 0 and overflow. -The starting value ofvalue of the [cos, sin] will be [1 << 30, 0]. -To record this choice, we define LOG_UNIT_MAGNITUDE: -**/ -#define LOG_UNIT_MAGNITUDE 30 - -/** -We define dA to be the effect of a single encoder tick on the angle the robot is -facing, in radians. This can be calculated from physical parameters of the robot. -The value of dA will usually be less than 0.01. -What we want to do to update cos and sin for a left-turning encoder tick is: - - cos -= sin * dA (floating point arithmetic) - sin += cos * dA (floating point arithmetic) - -Instead of doing that we can do the equivalent using integer arithmetic. -We define DA to be dA times (1 << LOG_UNIT_MAGNITUDE). -Then we do: - - cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; - sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; -**/ - -/** -We define Df to be the distance the robot moves forward per encoder tick. -This is half of the amount a single wheel turns per encoder tick. -Df is NOT an integer so we cannot store it in our program; it is a distance. -We do not need to even know Df, because all of our distances can be -measured and recorded in terms of Df. -**/ - -/** -We define two int32_t variables x and y to hold the current position of the robot. -Together, x and y are a vector that points from the starting point to the robot's -current position. - -When choosing the units of x and y, we have several considerations: - * If the units are too big, precision is lost. - * If the units are too small, the integers will overflow. - * For convenience, the units should be a power of 2 off from Df (Distance robot moves - forward per encoder tick), so we can just write lines like: - x += cos >> some_constant - -Taking this into account, we choose the units of x and y to be Df / (1 << 14). - -Let's make sure this won't result in overflow: - * To ensure no overflow happens, we need: - (Maximum distance representable by x or y) > (Maximum roam of robot) - (1 << 31) * Df / (1 << 14) > (Maximum roam of robot) - (Maximum roam of robot) / Df < (1 << 17) = 131072 - * Assume Df is 0.1 mm (this is smaller than it will usually be for the robots we work with). - * That means the robot can roam at most 13.1 m (0.0001 m * 131072) from its starting point, - which should be plenty. - -So how do we update x and y? - * When we update x and y, the position they represent changes by Df. - * x and y represent a unit vector, but their units are 1 / (1 << LOG_UNIT_MAGNITUDE). - * If we wrote x += cos it would mean the units of x are Df / (1 << LOG_UNIT_MAGNITUDE). - (and x would overflow after 2 or 3 ticks). - * Instead, we write x += cos >> 16 so the units are correct. -**/ - - - -/** -W: Wheel-to-wheel distance: 139 mm // Measured with calipers. -G: Gear ratio factor: 30 -T: Encoder ticks per backshaft rotation: 12 (three-toothed encoder wheel) -R: Wheel radius: 35 mm (Pololu 70mm diameter wheel) - -Dw: Distance wheel turns per encoder tick = 2*pi*R/(G*T) -Df: Distance robot moves forward per encoder tick = Dw/2 -dA: Change in angle per encoder tick = Dw/W = 2*pi*35/(30*12) / 150 = 0.00439471394 -**/ - -/** The theoretical value for dA above turned out to not be so accurate. -After playing with the robot for a few minutes, the robot was confused about which direction -it was facing by about 30 degrees. -So I did an experiment to find out what dA really is. I turned the robot around many times and -then took the difference in the encoder ticks from the left and right wheels. -dA should be equal to 2*pi*(turn count) / (left_ticks - right_ticks). -The experiment was performed several times to see how accurate the number is. - -(Theoretical dA = 0.00439471394 ) -Run 1: dA = 2*pi*15 / (11691 + 9414) = 0.00446566119 -Run 2: dA = 2*pi*15 / (10232 + 10961) = 0.00444711836 -Run 3: dA = 2*pi*30 / (19823 + 22435) = 0.00446058874 -Run 4: dA = 2*pi*30 / (19794 + 22447) = 0.00446238392 - -The dA I decided to use is the average from runs 3 and 4: dA = 0.00446148633 -**/ - -#define DA 4790484 // 0.00446148633 * 0x40000000 - -#define LOG_COS_TO_X_CONVERSION 16 // 30 - 14 +// We define Df to be the distance the robot moves forward per encoder tick. +// This is half of the amount a single wheel turns per encoder tick. +// Df is NOT an integer so we cannot store it in our program; it is a distance. +// We do not need to even know Df, because all of our distances can be +// measured and recorded in terms of Df. +// +// We define two int32_t variables x and y to hold the current position of the +// robot. Together, x and y are a vector that points from the starting point to +// the robot's current position. +// +// We choose the units of x and y to be Df / (1 << 14). +// +// Let's make sure this won't result in overflow: +// To ensure no overflow happens, we need: +// (Maximum distance representable by x or y) > (Maximum roam of robot) +// (1 << 31) * Df / (1 << 14) > (Maximum roam of robot) +// (Maximum roam of robot) / Df < (1 << 17) = 131072 +// +// If we assume Df is 0.1 mm (which is pretty small), then this means the robot +// can roam at most 13.1 m (0.0001 m * 131072) from its starting point, +// which should be plenty. +// +// So how do we update x and y? +// We define two int32_t variables named cosv and sinv that are in the same +// units as x and y and represent a vector that points in the direction that +// the robot is pointing and has a magnitude of Df (i.e. 1 << 14). +// So we just do x += cosv and y += sinv to update x and y when the robot +// moves one encoder tick forward. Reckoner::Reckoner() { @@ -109,66 +37,28 @@ void Reckoner::reset() { - cos = 1 << LOG_UNIT_MAGNITUDE; - sin = 0; - x = 0; - y = 0; -} - -void Reckoner::handleTickLeftForward() -{ - handleForward(); - handleRight(); -} - -void Reckoner::handleTickLeftBackward() -{ - handleBackward(); - handleLeft(); -} - -void Reckoner::handleTickRightForward() -{ - handleForward(); - handleLeft(); -} - -void Reckoner::handleTickRightBackward() -{ - handleBackward(); - handleRight(); + cosv = 1 << RECKONER_LOG_UNIT; + sinv = 0; + x = 0; + y = 0; } void Reckoner::handleForward() { - x += cos >> LOG_COS_TO_X_CONVERSION; - y += sin >> LOG_COS_TO_X_CONVERSION; + x += cosv; + y += sinv; } void Reckoner::handleBackward() { - x -= cos >> LOG_COS_TO_X_CONVERSION; - y -= sin >> LOG_COS_TO_X_CONVERSION; -} - -void Reckoner::handleRight() -{ - // DA = 4790484 - // 0.2% boost - handleTurnRadians(-4800065); + x -= cosv; + y -= sinv; } -void Reckoner::handleLeft() -{ - handleTurnRadians(DA); -} - -void Reckoner::handleTurnRadians(int32_t radians) +void Reckoner::setTurnAngle(int32_t angle) { - int32_t dc = -((int64_t)sin * radians) >> LOG_UNIT_MAGNITUDE; - int32_t ds = ((int64_t)cos * radians) >> LOG_UNIT_MAGNITUDE; - dc = -((int64_t)(sin+ds/2) * radians) >> LOG_UNIT_MAGNITUDE; - ds = ((int64_t)(cos+dc/2) * radians) >> LOG_UNIT_MAGNITUDE; - cos += dc; - sin += ds; -} \ No newline at end of file + // 0x20000000 represents 45 degrees. + const double angleToRadiansFactor = 1.46291808e-9; // pi/4 / 0x20000000 + cosv = (int32_t)(cos(angle * angleToRadiansFactor) * (1 << RECKONER_LOG_UNIT)); + sinv = (int32_t)(sin(angle * angleToRadiansFactor) * (1 << RECKONER_LOG_UNIT)); +}
--- a/reckoner.h Sat Jul 27 20:58:46 2019 +0000 +++ b/reckoner.h Sat Jul 27 22:52:19 2019 +0000 @@ -1,32 +1,24 @@ #pragma once +#define RECKONER_LOG_UNIT 14 + class Reckoner { - public: +public: Reckoner(); - // Together, cos and sin form a vector with a magnitude close to 2^30 that indicates - // the current position of the robot. - // cos corresponds to the x component of the orientation vector. - // sin corresponds to the y component of the orientation vector. - int32_t cos, sin; + // Together, cos and sin form a vector with a magnitude close to 2^14 that + // indicate the current direction the robot is facint. + int32_t cosv, sinv; // Together, x and y are a vector that points from the starting point to the - // robot's current position. - // The untis are Df / (1<<14), where Df is the distance the robot moves forward - // or backwards due to a single encoder tick. + // robot's current position, with units of Df / (1<<14), where Df is the + // distance the robot moves due to a single encoder tick. int32_t x, y; void reset(); - - void handleTickLeftForward(); - void handleTickLeftBackward(); - void handleTickRightForward(); - void handleTickRightBackward(); void handleForward(); void handleBackward(); - void handleRight(); - void handleLeft(); - void handleTurnRadians(int32_t radians); + void setTurnAngle(int32_t angle); };
--- a/test.cpp Sat Jul 27 20:58:46 2019 +0000 +++ b/test.cpp Sat Jul 27 22:52:19 2019 +0000 @@ -25,7 +25,7 @@ { led3 = logger.isFull(); - updateReckonerFromEncoders(); + updateReckoner(); loggerService(); } led2 = 1; @@ -34,29 +34,31 @@ void testCloseness() { + doGyroCalibration(); led1 = 1; while(1) { - updateReckonerFromEncoders(); + updateReckoner(); float magn = magnitude(); - led3 = (magn < (1<<(14+7))); - led4 = (magn < (1<<(14+9))); + led3 = magn < (1 << 5); + led4 = magn < (1 << 7); } } void showOrientationWithLeds34() { - led3 = reckoner.cos > 0; - led4 = reckoner.sin > 0; + led3 = reckoner.cosv > 0; + led4 = reckoner.sinv > 0; } void testTurnInPlace() { + doGyroCalibration(); led1 = 1; while(!button1DefinitelyPressed()) { - updateReckonerFromEncoders(); + updateReckoner(); showOrientationWithLeds34(); } led2 = 1; @@ -67,7 +69,7 @@ motorsSpeedSet(-300, 300); while(timer.read_ms() < 4000) { - updateReckonerFromEncoders(); + updateReckoner(); showOrientationWithLeds34(); } timer.reset(); @@ -78,7 +80,7 @@ if (motorUpdatePacer.pace()) { int16_t rotationSpeed; - float s = (float)reckoner.sin / (1 << 30); + float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT); integral += s; rotationSpeed = -(s * 2400 + integral * 20); @@ -100,109 +102,13 @@ infiniteReckonerReportLoop(); } - -void testSensorGlitches() -{ - AnalogIn testInput(p18); - Pacer reportPacer(1000000); - uint32_t badCount = 0, goodCount = 0; - pc.printf("hi\r\n"); - - //uint16_t riseCount = 0; - uint16_t reading = 0xFF; - - while(1) - { - /** This digital filtering did not work - { - wait(0.01); - uint16_t raw = testInput.read_u16(); - if (raw < reading) - { - riseCount = 0; - reading = raw; - } - else - { - riseCount++; - if (riseCount == 10) - { - riseCount = 0; - reading = raw; - } - } - } - **/ - - uint16_t values[LINE_SENSOR_COUNT]; - readSensors(values); - reading = values[0]; - - if(reading > 100) - { - badCount += 1; - //pc.printf("f %5d %11d %11d\r\n", reading, badCount, goodCount); - } - else - { - goodCount += 1; - } - - if (reportPacer.pace()) - { - pc.printf("h %5d %11d %11d\r\n", reading, badCount, goodCount); - } - } -} - -void testAnalog() -{ - AnalogIn testInput(p18); - - DigitalOut pin20(p20); - DigitalOut pin19(p19); - //DigitalOut pin18(p18); - DigitalOut pin17(p17); - DigitalOut pin16(p16); - DigitalOut pin15(p15); - - pin20 = 0; - pin19 = 0; - //pin18 = 0; - pin17 = 0; - pin16 = 0; - pin15 = 0; - - uint32_t badCount = 0, goodCount = 0; - - Pacer reportPacer(1000000); - while(1) - { - uint16_t reading = testInput.read_u16(); - if(reading > 100) - { - badCount += 1; - pc.printf("%5d %11d %11d\r\n", reading, badCount, goodCount); - } - else - { - goodCount += 1; - } - - if (reportPacer.pace()) - { - pc.printf("Hello\r\n"); - } - } -} - // This also tests the LineTracker by printing out a lot of data from it. void testLineFollowing() { led1 = 1; while(!button1DefinitelyPressed()) { - updateReckonerFromEncoders(); + updateReckoner(); } led2 = 1; @@ -212,7 +118,7 @@ uint16_t loopCount = 0; while(1) { - updateReckonerFromEncoders(); + updateReckoner(); bool lineVisiblePrevious = lineTracker.getLineVisible(); lineTracker.read(); updateMotorsToFollowLine(); @@ -241,10 +147,11 @@ void testDriveHome() { + doGyroCalibration(); led1 = 1; while(!button1DefinitelyPressed()) { - updateReckonerFromEncoders(); + updateReckoner(); } setLeds(1, 0, 1, 0); @@ -259,10 +166,11 @@ void testFinalSettleIn() { + doGyroCalibration(); led1 = 1; while(!button1DefinitelyPressed()) { - updateReckonerFromEncoders(); + updateReckoner(); } finalSettleIn(); infiniteReckonerReportLoop(); @@ -292,19 +200,21 @@ { doGyroCalibration(); led1 = 1; + turnSensor.start(); Pacer reportPacer(100000); while(1) { - updateReckonerFromEncoders(); + turnSensor.update(); + updateReckoner(); - led1 = (reckoner.x > 0); - led2 = (reckoner.y > 0); + led1 = reckoner.x > 0; + led2 = reckoner.y > 0; showOrientationWithLeds34(); if (reportPacer.pace()) { - pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n", - reckoner.cos, reckoner.sin, reckoner.x, reckoner.y, + pc.printf("%6d %6d %11d %11d | %8d %8d %10f\r\n", + reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y, encoderLeft.getCount(), encoderRight.getCount(), determinant()); } } @@ -313,12 +223,11 @@ void testTurnSensor() { pc.printf("Test turn sensor\r\n"); + doGyroCalibration(); led1 = 1; - doGyroCalibration(); //Pacer reportPacer(200000); // 0.2 s Pacer reportPacer(10000000); // 10 s Timer timer; - led2 = 1; timer.start(); turnSensor.start(); while(1) @@ -430,10 +339,6 @@ } } - //values[0] = lineSensorsAnalog[0].read_u16(); - //values[1] = lineSensorsAnalog[1].read_u16(); - //values[2] = lineSensorsAnalog[2].read_u16(); - uint16_t values[3]; readSensors(values); @@ -589,8 +494,8 @@ showOrientationWithLeds34(); if(reportPacer.pace()) { - pc.printf("%11d %11d %11d %11d | %11f %11f\r\n", - reckoner.cos, reckoner.sin, reckoner.x, reckoner.y, + pc.printf("%6d %6d %11d %11d | %11f %11f\r\n", + reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y, determinant(), dotProduct()); } }
--- a/test.h Sat Jul 27 20:58:46 2019 +0000 +++ b/test.h Sat Jul 27 22:52:19 2019 +0000 @@ -13,8 +13,6 @@ void __attribute__((noreturn)) testFinalSettleIn(); void __attribute__((noreturn)) testCalibrate(); void __attribute__((noreturn)) testLineFollowing(); -void __attribute__((noreturn)) testAnalog(); -void __attribute__((noreturn)) testSensorGlitches(); void __attribute__((noreturn)) testTurnInPlace(); void __attribute__((noreturn)) testCloseness(); void __attribute__((noreturn)) testLogger(); \ No newline at end of file