David Grayson / Mbed 2 deprecated DeadReckoning

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

test.cpp@42:96671b71aac5, 2019-07-27 (annotated)

Committer:
DavidEGrayson
Date:
Sat Jul 27 20:58:46 2019 +0000
Revision:
42:96671b71aac5
Parent:
41:3ead1dd2cc3a
Child:
43:0e985a58f174
Calibrate L3G using a buffer of 1000 zero-rate readings.  Measured a drift of -1.850 degrees over 3 minutes (-0.01 degree per second).

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DavidEGrayson 8:78b1ff957cba 1 // A file for testing routines that will not be used in the final firmware.
DavidEGrayson 8:78b1ff957cba 2
DavidEGrayson 8:78b1ff957cba 3 #include <mbed.h>
DavidEGrayson 8:78b1ff957cba 4 #include "motors.h"
DavidEGrayson 9:9734347b5756 5 #include <Pacer.h>
DavidEGrayson 9:9734347b5756 6
DavidEGrayson 21:c279c6a83671 7 #include "main.h"
DavidEGrayson 9:9734347b5756 8 #include "test.h"
DavidEGrayson 8:78b1ff957cba 9 #include "leds.h"
DavidEGrayson 9:9734347b5756 10 #include "encoders.h"
DavidEGrayson 8:78b1ff957cba 11 #include "pc_serial.h"
DavidEGrayson 10:e4dd36148539 12 #include "line_sensors.h"
DavidEGrayson 40:6fa672be85ec 13 #include "l3g.h"
DavidEGrayson 40:6fa672be85ec 14 #include "turn_sensor.h"
DavidEGrayson 12:835a4d24ae3b 15 #include "reckoner.h"
DavidEGrayson 16:8eaa5bc2bdb1 16 #include "buttons.h"
DavidEGrayson 16:8eaa5bc2bdb1 17
DavidEGrayson 20:dbec34f0e76b 18 void __attribute__((noreturn)) infiniteReckonerReportLoop();
DavidEGrayson 10:e4dd36148539 19 void printBar(const char * name, uint16_t adcResult);
DavidEGrayson 10:e4dd36148539 20
DavidEGrayson 37:23000a47ed2b 21 void testLogger()
DavidEGrayson 37:23000a47ed2b 22 {
DavidEGrayson 37:23000a47ed2b 23 led1 = 1;
DavidEGrayson 37:23000a47ed2b 24 while(!button1DefinitelyPressed())
DavidEGrayson 37:23000a47ed2b 25 {
DavidEGrayson 37:23000a47ed2b 26 led3 = logger.isFull();
DavidEGrayson 37:23000a47ed2b 27
DavidEGrayson 37:23000a47ed2b 28 updateReckonerFromEncoders();
DavidEGrayson 37:23000a47ed2b 29 loggerService();
DavidEGrayson 37:23000a47ed2b 30 }
DavidEGrayson 37:23000a47ed2b 31 led2 = 1;
DavidEGrayson 37:23000a47ed2b 32 loggerReportLoop();
DavidEGrayson 37:23000a47ed2b 33 }
DavidEGrayson 37:23000a47ed2b 34
DavidEGrayson 33:58a0ab6e9ad2 35 void testCloseness()
DavidEGrayson 30:84be2d602dc0 36 {
DavidEGrayson 33:58a0ab6e9ad2 37 led1 = 1;
DavidEGrayson 33:58a0ab6e9ad2 38 while(1)
DavidEGrayson 30:84be2d602dc0 39 {
DavidEGrayson 33:58a0ab6e9ad2 40 updateReckonerFromEncoders();
DavidEGrayson 33:58a0ab6e9ad2 41 float magn = magnitude();
DavidEGrayson 33:58a0ab6e9ad2 42
DavidEGrayson 33:58a0ab6e9ad2 43 led3 = (magn < (1<<(14+7)));
DavidEGrayson 33:58a0ab6e9ad2 44 led4 = (magn < (1<<(14+9)));
DavidEGrayson 30:84be2d602dc0 45 }
DavidEGrayson 33:58a0ab6e9ad2 46 }
DavidEGrayson 33:58a0ab6e9ad2 47
DavidEGrayson 33:58a0ab6e9ad2 48 void showOrientationWithLeds34()
DavidEGrayson 33:58a0ab6e9ad2 49 {
DavidEGrayson 33:58a0ab6e9ad2 50 led3 = reckoner.cos > 0;
DavidEGrayson 33:58a0ab6e9ad2 51 led4 = reckoner.sin > 0;
DavidEGrayson 30:84be2d602dc0 52 }
DavidEGrayson 30:84be2d602dc0 53
DavidEGrayson 33:58a0ab6e9ad2 54 void testTurnInPlace()
DavidEGrayson 30:84be2d602dc0 55 {
DavidEGrayson 33:58a0ab6e9ad2 56 led1 = 1;
DavidEGrayson 33:58a0ab6e9ad2 57 while(!button1DefinitelyPressed())
DavidEGrayson 33:58a0ab6e9ad2 58 {
DavidEGrayson 33:58a0ab6e9ad2 59 updateReckonerFromEncoders();
DavidEGrayson 33:58a0ab6e9ad2 60 showOrientationWithLeds34();
DavidEGrayson 33:58a0ab6e9ad2 61 }
DavidEGrayson 33:58a0ab6e9ad2 62 led2 = 1;
DavidEGrayson 33:58a0ab6e9ad2 63
DavidEGrayson 33:58a0ab6e9ad2 64 Pacer motorUpdatePacer(10000);
DavidEGrayson 30:84be2d602dc0 65 Timer timer;
DavidEGrayson 30:84be2d602dc0 66 timer.start();
DavidEGrayson 33:58a0ab6e9ad2 67 motorsSpeedSet(-300, 300);
DavidEGrayson 33:58a0ab6e9ad2 68 while(timer.read_ms() < 4000)
DavidEGrayson 33:58a0ab6e9ad2 69 {
DavidEGrayson 33:58a0ab6e9ad2 70 updateReckonerFromEncoders();
DavidEGrayson 33:58a0ab6e9ad2 71 showOrientationWithLeds34();
DavidEGrayson 33:58a0ab6e9ad2 72 }
DavidEGrayson 33:58a0ab6e9ad2 73 timer.reset();
DavidEGrayson 33:58a0ab6e9ad2 74
DavidEGrayson 33:58a0ab6e9ad2 75 float integral = 0;
DavidEGrayson 33:58a0ab6e9ad2 76 while (timer.read_ms() < 4000)
DavidEGrayson 30:84be2d602dc0 77 {
DavidEGrayson 33:58a0ab6e9ad2 78 if (motorUpdatePacer.pace())
DavidEGrayson 30:84be2d602dc0 79 {
DavidEGrayson 33:58a0ab6e9ad2 80 int16_t rotationSpeed;
DavidEGrayson 33:58a0ab6e9ad2 81 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 33:58a0ab6e9ad2 82 integral += s;
DavidEGrayson 33:58a0ab6e9ad2 83 rotationSpeed = -(s * 2400 + integral * 20);
DavidEGrayson 33:58a0ab6e9ad2 84
DavidEGrayson 33:58a0ab6e9ad2 85 if (rotationSpeed > 450)
DavidEGrayson 33:58a0ab6e9ad2 86 {
DavidEGrayson 33:58a0ab6e9ad2 87 rotationSpeed = 450;
DavidEGrayson 33:58a0ab6e9ad2 88 }
DavidEGrayson 33:58a0ab6e9ad2 89 if (rotationSpeed < -450)
DavidEGrayson 33:58a0ab6e9ad2 90 {
DavidEGrayson 33:58a0ab6e9ad2 91 rotationSpeed = -450;
DavidEGrayson 33:58a0ab6e9ad2 92 }
DavidEGrayson 33:58a0ab6e9ad2 93
DavidEGrayson 33:58a0ab6e9ad2 94 int16_t speedLeft = -rotationSpeed;
DavidEGrayson 33:58a0ab6e9ad2 95 int16_t speedRight = rotationSpeed;
DavidEGrayson 33:58a0ab6e9ad2 96 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 30:84be2d602dc0 97 }
DavidEGrayson 30:84be2d602dc0 98 }
DavidEGrayson 33:58a0ab6e9ad2 99
DavidEGrayson 33:58a0ab6e9ad2 100 infiniteReckonerReportLoop();
DavidEGrayson 30:84be2d602dc0 101 }
DavidEGrayson 30:84be2d602dc0 102
DavidEGrayson 33:58a0ab6e9ad2 103
DavidEGrayson 30:84be2d602dc0 104 void testSensorGlitches()
DavidEGrayson 29:cfcf08d8ac79 105 {
DavidEGrayson 29:cfcf08d8ac79 106 AnalogIn testInput(p18);
DavidEGrayson 29:cfcf08d8ac79 107 Pacer reportPacer(1000000);
DavidEGrayson 29:cfcf08d8ac79 108 uint32_t badCount = 0, goodCount = 0;
DavidEGrayson 30:84be2d602dc0 109 pc.printf("hi\r\n");
DavidEGrayson 30:84be2d602dc0 110
DavidEGrayson 30:84be2d602dc0 111 //uint16_t riseCount = 0;
DavidEGrayson 30:84be2d602dc0 112 uint16_t reading = 0xFF;
DavidEGrayson 30:84be2d602dc0 113
DavidEGrayson 29:cfcf08d8ac79 114 while(1)
DavidEGrayson 29:cfcf08d8ac79 115 {
DavidEGrayson 30:84be2d602dc0 116 /** This digital filtering did not work
DavidEGrayson 30:84be2d602dc0 117 {
DavidEGrayson 30:84be2d602dc0 118 wait(0.01);
DavidEGrayson 30:84be2d602dc0 119 uint16_t raw = testInput.read_u16();
DavidEGrayson 30:84be2d602dc0 120 if (raw < reading)
DavidEGrayson 30:84be2d602dc0 121 {
DavidEGrayson 30:84be2d602dc0 122 riseCount = 0;
DavidEGrayson 30:84be2d602dc0 123 reading = raw;
DavidEGrayson 30:84be2d602dc0 124 }
DavidEGrayson 30:84be2d602dc0 125 else
DavidEGrayson 30:84be2d602dc0 126 {
DavidEGrayson 30:84be2d602dc0 127 riseCount++;
DavidEGrayson 30:84be2d602dc0 128 if (riseCount == 10)
DavidEGrayson 30:84be2d602dc0 129 {
DavidEGrayson 30:84be2d602dc0 130 riseCount = 0;
DavidEGrayson 30:84be2d602dc0 131 reading = raw;
DavidEGrayson 30:84be2d602dc0 132 }
DavidEGrayson 30:84be2d602dc0 133 }
DavidEGrayson 30:84be2d602dc0 134 }
DavidEGrayson 30:84be2d602dc0 135 **/
DavidEGrayson 33:58a0ab6e9ad2 136
DavidEGrayson 33:58a0ab6e9ad2 137 uint16_t values[LINE_SENSOR_COUNT];
DavidEGrayson 33:58a0ab6e9ad2 138 readSensors(values);
DavidEGrayson 33:58a0ab6e9ad2 139 reading = values[0];
DavidEGrayson 30:84be2d602dc0 140
DavidEGrayson 29:cfcf08d8ac79 141 if(reading > 100)
DavidEGrayson 29:cfcf08d8ac79 142 {
DavidEGrayson 29:cfcf08d8ac79 143 badCount += 1;
DavidEGrayson 30:84be2d602dc0 144 //pc.printf("f %5d %11d %11d\r\n", reading, badCount, goodCount);
DavidEGrayson 29:cfcf08d8ac79 145 }
DavidEGrayson 29:cfcf08d8ac79 146 else
DavidEGrayson 29:cfcf08d8ac79 147 {
DavidEGrayson 30:84be2d602dc0 148 goodCount += 1;
DavidEGrayson 29:cfcf08d8ac79 149 }
DavidEGrayson 29:cfcf08d8ac79 150
DavidEGrayson 29:cfcf08d8ac79 151 if (reportPacer.pace())
DavidEGrayson 29:cfcf08d8ac79 152 {
DavidEGrayson 30:84be2d602dc0 153 pc.printf("h %5d %11d %11d\r\n", reading, badCount, goodCount);
DavidEGrayson 29:cfcf08d8ac79 154 }
DavidEGrayson 29:cfcf08d8ac79 155 }
DavidEGrayson 29:cfcf08d8ac79 156 }
DavidEGrayson 29:cfcf08d8ac79 157
DavidEGrayson 28:4374035df5e0 158 void testAnalog()
DavidEGrayson 28:4374035df5e0 159 {
DavidEGrayson 28:4374035df5e0 160 AnalogIn testInput(p18);
DavidEGrayson 28:4374035df5e0 161
DavidEGrayson 29:cfcf08d8ac79 162 DigitalOut pin20(p20);
DavidEGrayson 29:cfcf08d8ac79 163 DigitalOut pin19(p19);
DavidEGrayson 29:cfcf08d8ac79 164 //DigitalOut pin18(p18);
DavidEGrayson 29:cfcf08d8ac79 165 DigitalOut pin17(p17);
DavidEGrayson 29:cfcf08d8ac79 166 DigitalOut pin16(p16);
DavidEGrayson 29:cfcf08d8ac79 167 DigitalOut pin15(p15);
DavidEGrayson 29:cfcf08d8ac79 168
DavidEGrayson 29:cfcf08d8ac79 169 pin20 = 0;
DavidEGrayson 29:cfcf08d8ac79 170 pin19 = 0;
DavidEGrayson 29:cfcf08d8ac79 171 //pin18 = 0;
DavidEGrayson 29:cfcf08d8ac79 172 pin17 = 0;
DavidEGrayson 29:cfcf08d8ac79 173 pin16 = 0;
DavidEGrayson 29:cfcf08d8ac79 174 pin15 = 0;
DavidEGrayson 29:cfcf08d8ac79 175
DavidEGrayson 28:4374035df5e0 176 uint32_t badCount = 0, goodCount = 0;
DavidEGrayson 28:4374035df5e0 177
DavidEGrayson 28:4374035df5e0 178 Pacer reportPacer(1000000);
DavidEGrayson 28:4374035df5e0 179 while(1)
DavidEGrayson 28:4374035df5e0 180 {
DavidEGrayson 28:4374035df5e0 181 uint16_t reading = testInput.read_u16();
DavidEGrayson 28:4374035df5e0 182 if(reading > 100)
DavidEGrayson 28:4374035df5e0 183 {
DavidEGrayson 28:4374035df5e0 184 badCount += 1;
DavidEGrayson 28:4374035df5e0 185 pc.printf("%5d %11d %11d\r\n", reading, badCount, goodCount);
DavidEGrayson 28:4374035df5e0 186 }
DavidEGrayson 28:4374035df5e0 187 else
DavidEGrayson 28:4374035df5e0 188 {
DavidEGrayson 28:4374035df5e0 189 goodCount += 1;
DavidEGrayson 28:4374035df5e0 190 }
DavidEGrayson 28:4374035df5e0 191
DavidEGrayson 28:4374035df5e0 192 if (reportPacer.pace())
DavidEGrayson 28:4374035df5e0 193 {
DavidEGrayson 28:4374035df5e0 194 pc.printf("Hello\r\n");
DavidEGrayson 28:4374035df5e0 195 }
DavidEGrayson 28:4374035df5e0 196 }
DavidEGrayson 28:4374035df5e0 197 }
DavidEGrayson 28:4374035df5e0 198
DavidEGrayson 28:4374035df5e0 199 // This also tests the LineTracker by printing out a lot of data from it.
DavidEGrayson 28:4374035df5e0 200 void testLineFollowing()
DavidEGrayson 28:4374035df5e0 201 {
DavidEGrayson 28:4374035df5e0 202 led1 = 1;
DavidEGrayson 28:4374035df5e0 203 while(!button1DefinitelyPressed())
DavidEGrayson 28:4374035df5e0 204 {
DavidEGrayson 28:4374035df5e0 205 updateReckonerFromEncoders();
DavidEGrayson 28:4374035df5e0 206 }
DavidEGrayson 28:4374035df5e0 207 led2 = 1;
DavidEGrayson 28:4374035df5e0 208
DavidEGrayson 28:4374035df5e0 209 Pacer reportPacer(200000);
DavidEGrayson 28:4374035df5e0 210
DavidEGrayson 42:96671b71aac5 211 loadLineCalibration();
DavidEGrayson 28:4374035df5e0 212 uint16_t loopCount = 0;
DavidEGrayson 28:4374035df5e0 213 while(1)
DavidEGrayson 28:4374035df5e0 214 {
DavidEGrayson 28:4374035df5e0 215 updateReckonerFromEncoders();
DavidEGrayson 28:4374035df5e0 216 bool lineVisiblePrevious = lineTracker.getLineVisible();
DavidEGrayson 28:4374035df5e0 217 lineTracker.read();
DavidEGrayson 28:4374035df5e0 218 updateMotorsToFollowLine();
DavidEGrayson 28:4374035df5e0 219
DavidEGrayson 28:4374035df5e0 220 loopCount += 1;
DavidEGrayson 28:4374035df5e0 221
DavidEGrayson 28:4374035df5e0 222 if (lineVisiblePrevious != lineTracker.getLineVisible())
DavidEGrayson 28:4374035df5e0 223 {
DavidEGrayson 32:83a13b06093c 224 pc.printf("%5d ! %1d %4d | %5d %5d | %4d %4d %4d\r\n",
DavidEGrayson 28:4374035df5e0 225 loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
DavidEGrayson 32:83a13b06093c 226 motorLeftSpeed, motorRightSpeed,
DavidEGrayson 32:83a13b06093c 227 lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
DavidEGrayson 28:4374035df5e0 228 );
DavidEGrayson 28:4374035df5e0 229 }
DavidEGrayson 28:4374035df5e0 230
DavidEGrayson 28:4374035df5e0 231 if (reportPacer.pace())
DavidEGrayson 28:4374035df5e0 232 {
DavidEGrayson 32:83a13b06093c 233 pc.printf("%5d %1d %4d | %5d %5d | %4d %4d %4d\r\n",
DavidEGrayson 32:83a13b06093c 234 loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
DavidEGrayson 32:83a13b06093c 235 motorLeftSpeed, motorRightSpeed,
DavidEGrayson 28:4374035df5e0 236 lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
DavidEGrayson 28:4374035df5e0 237 );
DavidEGrayson 28:4374035df5e0 238 }
DavidEGrayson 28:4374035df5e0 239 }
DavidEGrayson 28:4374035df5e0 240 }
DavidEGrayson 28:4374035df5e0 241
DavidEGrayson 17:2df9861f53ee 242 void testDriveHome()
DavidEGrayson 16:8eaa5bc2bdb1 243 {
DavidEGrayson 17:2df9861f53ee 244 led1 = 1;
DavidEGrayson 18:b65fbb795396 245 while(!button1DefinitelyPressed())
DavidEGrayson 17:2df9861f53ee 246 {
DavidEGrayson 17:2df9861f53ee 247 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 248 }
DavidEGrayson 33:58a0ab6e9ad2 249
DavidEGrayson 33:58a0ab6e9ad2 250 setLeds(1, 0, 1, 0);
DavidEGrayson 21:c279c6a83671 251 driveHomeAlmost();
DavidEGrayson 33:58a0ab6e9ad2 252
DavidEGrayson 33:58a0ab6e9ad2 253 //setLeds(0, 1, 1, 0);
DavidEGrayson 33:58a0ab6e9ad2 254 //finalSettleIn();
DavidEGrayson 33:58a0ab6e9ad2 255
DavidEGrayson 33:58a0ab6e9ad2 256 setLeds(1, 1, 1, 1);
DavidEGrayson 20:dbec34f0e76b 257 infiniteReckonerReportLoop();
DavidEGrayson 20:dbec34f0e76b 258 }
DavidEGrayson 20:dbec34f0e76b 259
DavidEGrayson 20:dbec34f0e76b 260 void testFinalSettleIn()
DavidEGrayson 20:dbec34f0e76b 261 {
DavidEGrayson 20:dbec34f0e76b 262 led1 = 1;
DavidEGrayson 20:dbec34f0e76b 263 while(!button1DefinitelyPressed())
DavidEGrayson 20:dbec34f0e76b 264 {
DavidEGrayson 20:dbec34f0e76b 265 updateReckonerFromEncoders();
DavidEGrayson 20:dbec34f0e76b 266 }
DavidEGrayson 20:dbec34f0e76b 267 finalSettleIn();
DavidEGrayson 20:dbec34f0e76b 268 infiniteReckonerReportLoop();
DavidEGrayson 16:8eaa5bc2bdb1 269 }
DavidEGrayson 16:8eaa5bc2bdb1 270
DavidEGrayson 17:2df9861f53ee 271
DavidEGrayson 16:8eaa5bc2bdb1 272 void testButtons()
DavidEGrayson 16:8eaa5bc2bdb1 273 {
DavidEGrayson 16:8eaa5bc2bdb1 274 led1 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 275
DavidEGrayson 17:2df9861f53ee 276 while(!button1DefinitelyReleased());
DavidEGrayson 17:2df9861f53ee 277 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 278 led2 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 279
DavidEGrayson 16:8eaa5bc2bdb1 280 while(!button1DefinitelyReleased());
DavidEGrayson 16:8eaa5bc2bdb1 281 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 282 led3 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 283
DavidEGrayson 16:8eaa5bc2bdb1 284 while(!button1DefinitelyReleased());
DavidEGrayson 16:8eaa5bc2bdb1 285 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 286 led4 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 287
DavidEGrayson 16:8eaa5bc2bdb1 288 while(1){};
DavidEGrayson 16:8eaa5bc2bdb1 289 }
DavidEGrayson 16:8eaa5bc2bdb1 290
DavidEGrayson 12:835a4d24ae3b 291 void testReckoner()
DavidEGrayson 12:835a4d24ae3b 292 {
DavidEGrayson 42:96671b71aac5 293 doGyroCalibration();
DavidEGrayson 42:96671b71aac5 294 led1 = 1;
DavidEGrayson 13:bba5b3abd13f 295 Pacer reportPacer(100000);
DavidEGrayson 12:835a4d24ae3b 296 while(1)
DavidEGrayson 12:835a4d24ae3b 297 {
DavidEGrayson 12:835a4d24ae3b 298 updateReckonerFromEncoders();
DavidEGrayson 33:58a0ab6e9ad2 299
DavidEGrayson 33:58a0ab6e9ad2 300 led1 = (reckoner.x > 0);
DavidEGrayson 33:58a0ab6e9ad2 301 led2 = (reckoner.y > 0);
DavidEGrayson 33:58a0ab6e9ad2 302 showOrientationWithLeds34();
DavidEGrayson 12:835a4d24ae3b 303
DavidEGrayson 13:bba5b3abd13f 304 if (reportPacer.pace())
DavidEGrayson 13:bba5b3abd13f 305 {
DavidEGrayson 19:a11ffc903774 306 pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n",
DavidEGrayson 13:bba5b3abd13f 307 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 19:a11ffc903774 308 encoderLeft.getCount(), encoderRight.getCount(), determinant());
DavidEGrayson 13:bba5b3abd13f 309 }
DavidEGrayson 12:835a4d24ae3b 310 }
DavidEGrayson 12:835a4d24ae3b 311 }
DavidEGrayson 12:835a4d24ae3b 312
DavidEGrayson 40:6fa672be85ec 313 void testTurnSensor()
DavidEGrayson 40:6fa672be85ec 314 {
DavidEGrayson 40:6fa672be85ec 315 pc.printf("Test turn sensor\r\n");
DavidEGrayson 42:96671b71aac5 316 led1 = 1;
DavidEGrayson 42:96671b71aac5 317 doGyroCalibration();
DavidEGrayson 42:96671b71aac5 318 //Pacer reportPacer(200000); // 0.2 s
DavidEGrayson 42:96671b71aac5 319 Pacer reportPacer(10000000); // 10 s
DavidEGrayson 42:96671b71aac5 320 Timer timer;
DavidEGrayson 42:96671b71aac5 321 led2 = 1;
DavidEGrayson 42:96671b71aac5 322 timer.start();
DavidEGrayson 40:6fa672be85ec 323 turnSensor.start();
DavidEGrayson 40:6fa672be85ec 324 while(1)
DavidEGrayson 40:6fa672be85ec 325 {
DavidEGrayson 40:6fa672be85ec 326 turnSensor.update();
DavidEGrayson 40:6fa672be85ec 327 if (reportPacer.pace())
DavidEGrayson 40:6fa672be85ec 328 {
DavidEGrayson 42:96671b71aac5 329 pc.printf("%u, %d, %d\r\n",
DavidEGrayson 42:96671b71aac5 330 timer.read_ms(),
DavidEGrayson 42:96671b71aac5 331 turnSensor.getAngleDegrees(),
DavidEGrayson 42:96671b71aac5 332 turnSensor.getAngleMillidegrees());
DavidEGrayson 40:6fa672be85ec 333 }
DavidEGrayson 40:6fa672be85ec 334 }
DavidEGrayson 40:6fa672be85ec 335 }
DavidEGrayson 40:6fa672be85ec 336
DavidEGrayson 42:96671b71aac5 337 void testL3gAndShowAverage()
DavidEGrayson 41:3ead1dd2cc3a 338 {
DavidEGrayson 41:3ead1dd2cc3a 339 wait_ms(2000);
DavidEGrayson 41:3ead1dd2cc3a 340 Pacer reportPacer(750000);
DavidEGrayson 41:3ead1dd2cc3a 341 Pacer readingPacer(2000);
DavidEGrayson 41:3ead1dd2cc3a 342 int32_t total = 0;
DavidEGrayson 41:3ead1dd2cc3a 343 int32_t readingCount = 0;
DavidEGrayson 41:3ead1dd2cc3a 344 while(1)
DavidEGrayson 41:3ead1dd2cc3a 345 {
DavidEGrayson 41:3ead1dd2cc3a 346 if (readingPacer.pace())
DavidEGrayson 41:3ead1dd2cc3a 347 {
DavidEGrayson 41:3ead1dd2cc3a 348 int32_t result = l3gZAvailable();
DavidEGrayson 41:3ead1dd2cc3a 349 if (result == 1)
DavidEGrayson 41:3ead1dd2cc3a 350 {
DavidEGrayson 41:3ead1dd2cc3a 351 int32_t gz = l3gZRead();
DavidEGrayson 41:3ead1dd2cc3a 352 if (gz < -500000)
DavidEGrayson 41:3ead1dd2cc3a 353 {
DavidEGrayson 41:3ead1dd2cc3a 354 pc.printf("l3gZRead error: %d\n", gz);
DavidEGrayson 41:3ead1dd2cc3a 355 }
DavidEGrayson 41:3ead1dd2cc3a 356 else
DavidEGrayson 41:3ead1dd2cc3a 357 {
DavidEGrayson 41:3ead1dd2cc3a 358 total += gz;
DavidEGrayson 41:3ead1dd2cc3a 359 readingCount += 1;
DavidEGrayson 41:3ead1dd2cc3a 360 }
DavidEGrayson 41:3ead1dd2cc3a 361 }
DavidEGrayson 41:3ead1dd2cc3a 362 else if (result != 0)
DavidEGrayson 41:3ead1dd2cc3a 363 {
DavidEGrayson 41:3ead1dd2cc3a 364 pc.printf("l3gZAvailable error: %d\n", result);
DavidEGrayson 41:3ead1dd2cc3a 365 }
DavidEGrayson 41:3ead1dd2cc3a 366 }
DavidEGrayson 41:3ead1dd2cc3a 367
DavidEGrayson 41:3ead1dd2cc3a 368 if (reportPacer.pace())
DavidEGrayson 41:3ead1dd2cc3a 369 {
DavidEGrayson 41:3ead1dd2cc3a 370 float average = (float)total / readingCount;
DavidEGrayson 41:3ead1dd2cc3a 371 pc.printf("%d, %d, %f\r\n", total, readingCount, average);
DavidEGrayson 41:3ead1dd2cc3a 372 }
DavidEGrayson 41:3ead1dd2cc3a 373 }
DavidEGrayson 41:3ead1dd2cc3a 374
DavidEGrayson 41:3ead1dd2cc3a 375 // Gyro calibration results get hardcoded into TurnSensor::update()
DavidEGrayson 41:3ead1dd2cc3a 376 // for now until we figure out something better.
DavidEGrayson 41:3ead1dd2cc3a 377 }
DavidEGrayson 41:3ead1dd2cc3a 378
DavidEGrayson 40:6fa672be85ec 379 void testL3g()
DavidEGrayson 40:6fa672be85ec 380 {
DavidEGrayson 40:6fa672be85ec 381 Pacer reportPacer(750000);
DavidEGrayson 40:6fa672be85ec 382 Timer timer;
DavidEGrayson 40:6fa672be85ec 383 timer.start();
DavidEGrayson 40:6fa672be85ec 384 int32_t gz = 0;
DavidEGrayson 40:6fa672be85ec 385 bool reportedReading = false;
DavidEGrayson 40:6fa672be85ec 386 while(1)
DavidEGrayson 40:6fa672be85ec 387 {
DavidEGrayson 40:6fa672be85ec 388 int32_t result = l3gZAvailable();
DavidEGrayson 40:6fa672be85ec 389 if (result == 1)
DavidEGrayson 40:6fa672be85ec 390 {
DavidEGrayson 40:6fa672be85ec 391 gz = l3gZRead();
DavidEGrayson 40:6fa672be85ec 392 reportedReading = false;
DavidEGrayson 40:6fa672be85ec 393 if (gz > 100 || gz < -100)
DavidEGrayson 40:6fa672be85ec 394 {
DavidEGrayson 42:96671b71aac5 395 pc.printf("%u, %d\r\n", timer.read_us(), gz);
DavidEGrayson 40:6fa672be85ec 396 reportedReading = true;
DavidEGrayson 40:6fa672be85ec 397 }
DavidEGrayson 40:6fa672be85ec 398 }
DavidEGrayson 40:6fa672be85ec 399 else if (result != 0)
DavidEGrayson 40:6fa672be85ec 400 {
DavidEGrayson 40:6fa672be85ec 401 pc.printf("l3gZAvailable error: %d\n", result);
DavidEGrayson 40:6fa672be85ec 402 }
DavidEGrayson 40:6fa672be85ec 403
DavidEGrayson 40:6fa672be85ec 404 if (reportPacer.pace() && !reportedReading)
DavidEGrayson 40:6fa672be85ec 405 {
DavidEGrayson 42:96671b71aac5 406 pc.printf("%u, %d\r\n", timer.read_us(), gz);
DavidEGrayson 40:6fa672be85ec 407 reportedReading = true;
DavidEGrayson 40:6fa672be85ec 408 }
DavidEGrayson 40:6fa672be85ec 409 }
DavidEGrayson 40:6fa672be85ec 410 }
DavidEGrayson 40:6fa672be85ec 411
DavidEGrayson 10:e4dd36148539 412 void testLineSensors()
DavidEGrayson 10:e4dd36148539 413 {
DavidEGrayson 10:e4dd36148539 414 led1 = 1;
DavidEGrayson 10:e4dd36148539 415 Pacer reportPacer(100000);
DavidEGrayson 31:739b91331f31 416 Pacer clearStatsPacer(2000000);
DavidEGrayson 31:739b91331f31 417
DavidEGrayson 31:739b91331f31 418 uint16_t min[LINE_SENSOR_COUNT];
DavidEGrayson 31:739b91331f31 419 uint16_t max[LINE_SENSOR_COUNT];
DavidEGrayson 31:739b91331f31 420
DavidEGrayson 10:e4dd36148539 421 bool const printBarGraph = true;
DavidEGrayson 10:e4dd36148539 422 while (1)
DavidEGrayson 10:e4dd36148539 423 {
DavidEGrayson 31:739b91331f31 424 if (clearStatsPacer.pace())
DavidEGrayson 31:739b91331f31 425 {
DavidEGrayson 31:739b91331f31 426 for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
DavidEGrayson 31:739b91331f31 427 {
DavidEGrayson 31:739b91331f31 428 min[i] = 0xFFFF;
DavidEGrayson 31:739b91331f31 429 max[i] = 0;
DavidEGrayson 31:739b91331f31 430 }
DavidEGrayson 31:739b91331f31 431 }
DavidEGrayson 31:739b91331f31 432
DavidEGrayson 31:739b91331f31 433 //values[0] = lineSensorsAnalog[0].read_u16();
DavidEGrayson 31:739b91331f31 434 //values[1] = lineSensorsAnalog[1].read_u16();
DavidEGrayson 31:739b91331f31 435 //values[2] = lineSensorsAnalog[2].read_u16();
DavidEGrayson 31:739b91331f31 436
DavidEGrayson 31:739b91331f31 437 uint16_t values[3];
DavidEGrayson 31:739b91331f31 438 readSensors(values);
DavidEGrayson 31:739b91331f31 439
DavidEGrayson 31:739b91331f31 440 for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
DavidEGrayson 31:739b91331f31 441 {
DavidEGrayson 31:739b91331f31 442 if (values[i] > max[i]){ max[i] = values[i]; }
DavidEGrayson 31:739b91331f31 443 if (values[i] < min[i]){ min[i] = values[i]; }
DavidEGrayson 31:739b91331f31 444 }
DavidEGrayson 31:739b91331f31 445
DavidEGrayson 10:e4dd36148539 446 if (reportPacer.pace())
DavidEGrayson 10:e4dd36148539 447 {
DavidEGrayson 10:e4dd36148539 448 if (printBarGraph)
DavidEGrayson 10:e4dd36148539 449 {
DavidEGrayson 10:e4dd36148539 450 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 31:739b91331f31 451 printBar("L", values[0]);
DavidEGrayson 31:739b91331f31 452 printBar("M", values[1]);
DavidEGrayson 31:739b91331f31 453 printBar("R", values[2]);
DavidEGrayson 31:739b91331f31 454 pc.printf("%4d %4d \r\n", min[0], max[0]);
DavidEGrayson 31:739b91331f31 455 pc.printf("%4d %4d \r\n", min[1], max[1]);
DavidEGrayson 31:739b91331f31 456 pc.printf("%4d %4d \r\n", min[2], max[2]);
DavidEGrayson 10:e4dd36148539 457 }
DavidEGrayson 10:e4dd36148539 458 else
DavidEGrayson 10:e4dd36148539 459 {
DavidEGrayson 31:739b91331f31 460 pc.printf("%8d %8d %8d\r\n", values[0], values[1], values[2]);
DavidEGrayson 10:e4dd36148539 461 }
DavidEGrayson 10:e4dd36148539 462 }
DavidEGrayson 10:e4dd36148539 463 }
DavidEGrayson 10:e4dd36148539 464 }
DavidEGrayson 8:78b1ff957cba 465
DavidEGrayson 24:fc01d9125d3b 466 // Values from David's office Values from dev lab,
DavidEGrayson 24:fc01d9125d3b 467 // in the day time, 2014-02-27: 2014-02-27:
DavidEGrayson 22:44c032e59ff5 468 // # calmin calmax
DavidEGrayson 24:fc01d9125d3b 469 // 0 34872 59726 0 40617 60222
DavidEGrayson 24:fc01d9125d3b 470 // 1 29335 60110 1 36937 61198
DavidEGrayson 24:fc01d9125d3b 471 // 2 23845 58446 2 33848 58862
DavidEGrayson 22:44c032e59ff5 472 void testCalibrate()
DavidEGrayson 22:44c032e59ff5 473 {
DavidEGrayson 22:44c032e59ff5 474 Timer timer;
DavidEGrayson 22:44c032e59ff5 475 timer.start();
DavidEGrayson 22:44c032e59ff5 476
DavidEGrayson 22:44c032e59ff5 477 Pacer reportPacer(200000);
DavidEGrayson 22:44c032e59ff5 478
DavidEGrayson 24:fc01d9125d3b 479 bool doneCalibrating = false;
DavidEGrayson 24:fc01d9125d3b 480
DavidEGrayson 24:fc01d9125d3b 481 led1 = 1;
DavidEGrayson 24:fc01d9125d3b 482
DavidEGrayson 22:44c032e59ff5 483 while(1)
DavidEGrayson 22:44c032e59ff5 484 {
DavidEGrayson 22:44c032e59ff5 485 lineTracker.read();
DavidEGrayson 24:fc01d9125d3b 486 if(!doneCalibrating)
DavidEGrayson 24:fc01d9125d3b 487 {
DavidEGrayson 24:fc01d9125d3b 488 lineTracker.updateCalibration();
DavidEGrayson 24:fc01d9125d3b 489 }
DavidEGrayson 24:fc01d9125d3b 490
DavidEGrayson 24:fc01d9125d3b 491 led3 = doneCalibrating;
DavidEGrayson 24:fc01d9125d3b 492 led4 = lineTracker.getLineVisible();
DavidEGrayson 24:fc01d9125d3b 493
DavidEGrayson 24:fc01d9125d3b 494 if (button1DefinitelyPressed())
DavidEGrayson 24:fc01d9125d3b 495 {
DavidEGrayson 24:fc01d9125d3b 496 doneCalibrating = true;
DavidEGrayson 24:fc01d9125d3b 497 }
DavidEGrayson 22:44c032e59ff5 498
DavidEGrayson 22:44c032e59ff5 499 if (reportPacer.pace())
DavidEGrayson 22:44c032e59ff5 500 {
DavidEGrayson 22:44c032e59ff5 501 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 22:44c032e59ff5 502 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 22:44c032e59ff5 503 {
DavidEGrayson 22:44c032e59ff5 504 pc.printf("%-2d %5d %5d %5d\r\n", s, lineTracker.calibratedMinimum[s], lineTracker.rawValues[s], lineTracker.calibratedMaximum[s]);
DavidEGrayson 22:44c032e59ff5 505 }
DavidEGrayson 22:44c032e59ff5 506 }
DavidEGrayson 22:44c032e59ff5 507 }
DavidEGrayson 22:44c032e59ff5 508 }
DavidEGrayson 22:44c032e59ff5 509
DavidEGrayson 39:b19dfc5d4d4b 510 void testMotorSpeed()
DavidEGrayson 39:b19dfc5d4d4b 511 {
DavidEGrayson 39:b19dfc5d4d4b 512 led1 = 1;
DavidEGrayson 39:b19dfc5d4d4b 513 motorsSpeedSet(400, 400);
DavidEGrayson 39:b19dfc5d4d4b 514 wait_ms(4000);
DavidEGrayson 39:b19dfc5d4d4b 515 uint32_t left = encoderLeft.getCount();
DavidEGrayson 39:b19dfc5d4d4b 516 uint32_t right = encoderRight.getCount();
DavidEGrayson 39:b19dfc5d4d4b 517 motorsSpeedSet(0, 0);
DavidEGrayson 39:b19dfc5d4d4b 518 Pacer reportPacer(500000);
DavidEGrayson 39:b19dfc5d4d4b 519 while (1)
DavidEGrayson 39:b19dfc5d4d4b 520 {
DavidEGrayson 39:b19dfc5d4d4b 521 if (reportPacer.pace())
DavidEGrayson 39:b19dfc5d4d4b 522 {
DavidEGrayson 39:b19dfc5d4d4b 523 led2 = 1;
DavidEGrayson 39:b19dfc5d4d4b 524 pc.printf("%8d %8d\r\n", left, right);
DavidEGrayson 39:b19dfc5d4d4b 525 led2 = 0;
DavidEGrayson 39:b19dfc5d4d4b 526 }
DavidEGrayson 39:b19dfc5d4d4b 527 }
DavidEGrayson 39:b19dfc5d4d4b 528 }
DavidEGrayson 39:b19dfc5d4d4b 529
DavidEGrayson 9:9734347b5756 530 void testEncoders()
DavidEGrayson 9:9734347b5756 531 {
DavidEGrayson 9:9734347b5756 532 Pacer reportPacer(500000);
DavidEGrayson 10:e4dd36148539 533 led1 = 1;
DavidEGrayson 9:9734347b5756 534 while(1)
DavidEGrayson 9:9734347b5756 535 {
DavidEGrayson 9:9734347b5756 536 while(encoderBuffer.hasEvents())
DavidEGrayson 9:9734347b5756 537 {
DavidEGrayson 9:9734347b5756 538 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 9:9734347b5756 539 }
DavidEGrayson 9:9734347b5756 540
DavidEGrayson 9:9734347b5756 541 if(reportPacer.pace())
DavidEGrayson 9:9734347b5756 542 {
DavidEGrayson 9:9734347b5756 543 led2 = 1;
DavidEGrayson 31:739b91331f31 544 pc.printf("%8d %8d\r\n", encoderLeft.getCount(), encoderRight.getCount());
DavidEGrayson 9:9734347b5756 545 led2 = 0;
DavidEGrayson 10:e4dd36148539 546 }
DavidEGrayson 9:9734347b5756 547 }
DavidEGrayson 9:9734347b5756 548 }
DavidEGrayson 9:9734347b5756 549
DavidEGrayson 8:78b1ff957cba 550 void testMotors()
DavidEGrayson 8:78b1ff957cba 551 {
DavidEGrayson 8:78b1ff957cba 552 led1 = 1;
DavidEGrayson 8:78b1ff957cba 553 led2 = 0;
DavidEGrayson 8:78b1ff957cba 554 led3 = 0;
DavidEGrayson 8:78b1ff957cba 555 while(1)
DavidEGrayson 8:78b1ff957cba 556 {
DavidEGrayson 9:9734347b5756 557 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 558 led2 = 0;
DavidEGrayson 8:78b1ff957cba 559 led3 = 0;
DavidEGrayson 8:78b1ff957cba 560 wait(2);
DavidEGrayson 8:78b1ff957cba 561
DavidEGrayson 9:9734347b5756 562 motorsSpeedSet(300, 300);
DavidEGrayson 8:78b1ff957cba 563 wait(2);
DavidEGrayson 8:78b1ff957cba 564
DavidEGrayson 9:9734347b5756 565 motorsSpeedSet(-300, 300);
DavidEGrayson 8:78b1ff957cba 566 wait(2);
DavidEGrayson 8:78b1ff957cba 567
DavidEGrayson 9:9734347b5756 568 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 569 led2 = 1;
DavidEGrayson 8:78b1ff957cba 570 wait(2);
DavidEGrayson 8:78b1ff957cba 571
DavidEGrayson 9:9734347b5756 572 motorsSpeedSet(600, 600);
DavidEGrayson 8:78b1ff957cba 573 wait(2);
DavidEGrayson 8:78b1ff957cba 574
DavidEGrayson 9:9734347b5756 575 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 576 led3 = 1;
DavidEGrayson 8:78b1ff957cba 577 wait(2);
DavidEGrayson 8:78b1ff957cba 578
DavidEGrayson 9:9734347b5756 579 motorsSpeedSet(1200, 1200);
DavidEGrayson 8:78b1ff957cba 580 wait(2);
DavidEGrayson 8:78b1ff957cba 581 }
DavidEGrayson 10:e4dd36148539 582 }
DavidEGrayson 10:e4dd36148539 583
DavidEGrayson 20:dbec34f0e76b 584 void infiniteReckonerReportLoop()
DavidEGrayson 20:dbec34f0e76b 585 {
DavidEGrayson 20:dbec34f0e76b 586 Pacer reportPacer(200000);
DavidEGrayson 20:dbec34f0e76b 587 while(1)
DavidEGrayson 20:dbec34f0e76b 588 {
DavidEGrayson 33:58a0ab6e9ad2 589 showOrientationWithLeds34();
DavidEGrayson 20:dbec34f0e76b 590 if(reportPacer.pace())
DavidEGrayson 20:dbec34f0e76b 591 {
DavidEGrayson 20:dbec34f0e76b 592 pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
DavidEGrayson 20:dbec34f0e76b 593 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 20:dbec34f0e76b 594 determinant(), dotProduct());
DavidEGrayson 20:dbec34f0e76b 595 }
DavidEGrayson 20:dbec34f0e76b 596 }
DavidEGrayson 20:dbec34f0e76b 597 }
DavidEGrayson 20:dbec34f0e76b 598
DavidEGrayson 31:739b91331f31 599 // with should be between 0 and 63
DavidEGrayson 31:739b91331f31 600 void printBar(const char * name, uint16_t result)
DavidEGrayson 10:e4dd36148539 601 {
DavidEGrayson 31:739b91331f31 602 pc.printf("%-2s %5d |", name, result);
DavidEGrayson 31:739b91331f31 603 uint16_t width = result >> 4;
DavidEGrayson 31:739b91331f31 604 if (width > 63) { width = 63; }
DavidEGrayson 10:e4dd36148539 605 uint8_t i;
DavidEGrayson 10:e4dd36148539 606 for(i = 0; i < width; i++){ pc.putc('#'); }
DavidEGrayson 10:e4dd36148539 607 for(; i < 63; i++){ pc.putc(' '); }
DavidEGrayson 10:e4dd36148539 608 pc.putc('|');
DavidEGrayson 31:739b91331f31 609 pc.putc('\r');
DavidEGrayson 10:e4dd36148539 610 pc.putc('\n');
DavidEGrayson 8:78b1ff957cba 611 }