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@45:81dd782bc0b4, 2019-07-28 (annotated)

Committer:
DavidEGrayson
Date:
Sun Jul 28 01:52:34 2019 +0000
Revision:
45:81dd782bc0b4
Parent:
44:b4a00fbab06b
Child:
46:df2c2d25c070
Calibrate the line sensors.  Add testLineSensorsAndCalibrate.  Unfortunately, testLineFollowing is not working though.

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 43:0e985a58f174 28 updateReckoner();
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 43:0e985a58f174 37 doGyroCalibration();
DavidEGrayson 44:b4a00fbab06b 38 turnSensor.start();
DavidEGrayson 33:58a0ab6e9ad2 39 led1 = 1;
DavidEGrayson 33:58a0ab6e9ad2 40 while(1)
DavidEGrayson 30:84be2d602dc0 41 {
DavidEGrayson 43:0e985a58f174 42 updateReckoner();
DavidEGrayson 33:58a0ab6e9ad2 43 float magn = magnitude();
DavidEGrayson 33:58a0ab6e9ad2 44
DavidEGrayson 43:0e985a58f174 45 led3 = magn < (1 << 5);
DavidEGrayson 43:0e985a58f174 46 led4 = magn < (1 << 7);
DavidEGrayson 30:84be2d602dc0 47 }
DavidEGrayson 33:58a0ab6e9ad2 48 }
DavidEGrayson 33:58a0ab6e9ad2 49
DavidEGrayson 33:58a0ab6e9ad2 50 void showOrientationWithLeds34()
DavidEGrayson 33:58a0ab6e9ad2 51 {
DavidEGrayson 43:0e985a58f174 52 led3 = reckoner.cosv > 0;
DavidEGrayson 43:0e985a58f174 53 led4 = reckoner.sinv > 0;
DavidEGrayson 30:84be2d602dc0 54 }
DavidEGrayson 30:84be2d602dc0 55
DavidEGrayson 33:58a0ab6e9ad2 56 void testTurnInPlace()
DavidEGrayson 30:84be2d602dc0 57 {
DavidEGrayson 43:0e985a58f174 58 doGyroCalibration();
DavidEGrayson 44:b4a00fbab06b 59 turnSensor.start();
DavidEGrayson 44:b4a00fbab06b 60
DavidEGrayson 33:58a0ab6e9ad2 61 led1 = 1;
DavidEGrayson 33:58a0ab6e9ad2 62 while(!button1DefinitelyPressed())
DavidEGrayson 33:58a0ab6e9ad2 63 {
DavidEGrayson 43:0e985a58f174 64 updateReckoner();
DavidEGrayson 33:58a0ab6e9ad2 65 showOrientationWithLeds34();
DavidEGrayson 33:58a0ab6e9ad2 66 }
DavidEGrayson 33:58a0ab6e9ad2 67 led2 = 1;
DavidEGrayson 33:58a0ab6e9ad2 68
DavidEGrayson 33:58a0ab6e9ad2 69 Pacer motorUpdatePacer(10000);
DavidEGrayson 30:84be2d602dc0 70 Timer timer;
DavidEGrayson 30:84be2d602dc0 71 timer.start();
DavidEGrayson 33:58a0ab6e9ad2 72 motorsSpeedSet(-300, 300);
DavidEGrayson 33:58a0ab6e9ad2 73 while(timer.read_ms() < 4000)
DavidEGrayson 33:58a0ab6e9ad2 74 {
DavidEGrayson 43:0e985a58f174 75 updateReckoner();
DavidEGrayson 33:58a0ab6e9ad2 76 showOrientationWithLeds34();
DavidEGrayson 33:58a0ab6e9ad2 77 }
DavidEGrayson 33:58a0ab6e9ad2 78 timer.reset();
DavidEGrayson 33:58a0ab6e9ad2 79
DavidEGrayson 33:58a0ab6e9ad2 80 float integral = 0;
DavidEGrayson 33:58a0ab6e9ad2 81 while (timer.read_ms() < 4000)
DavidEGrayson 30:84be2d602dc0 82 {
DavidEGrayson 33:58a0ab6e9ad2 83 if (motorUpdatePacer.pace())
DavidEGrayson 30:84be2d602dc0 84 {
DavidEGrayson 33:58a0ab6e9ad2 85 int16_t rotationSpeed;
DavidEGrayson 43:0e985a58f174 86 float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT);
DavidEGrayson 33:58a0ab6e9ad2 87 integral += s;
DavidEGrayson 33:58a0ab6e9ad2 88 rotationSpeed = -(s * 2400 + integral * 20);
DavidEGrayson 33:58a0ab6e9ad2 89
DavidEGrayson 33:58a0ab6e9ad2 90 if (rotationSpeed > 450)
DavidEGrayson 33:58a0ab6e9ad2 91 {
DavidEGrayson 33:58a0ab6e9ad2 92 rotationSpeed = 450;
DavidEGrayson 33:58a0ab6e9ad2 93 }
DavidEGrayson 33:58a0ab6e9ad2 94 if (rotationSpeed < -450)
DavidEGrayson 33:58a0ab6e9ad2 95 {
DavidEGrayson 33:58a0ab6e9ad2 96 rotationSpeed = -450;
DavidEGrayson 33:58a0ab6e9ad2 97 }
DavidEGrayson 33:58a0ab6e9ad2 98
DavidEGrayson 33:58a0ab6e9ad2 99 int16_t speedLeft = -rotationSpeed;
DavidEGrayson 33:58a0ab6e9ad2 100 int16_t speedRight = rotationSpeed;
DavidEGrayson 33:58a0ab6e9ad2 101 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 30:84be2d602dc0 102 }
DavidEGrayson 30:84be2d602dc0 103 }
DavidEGrayson 33:58a0ab6e9ad2 104
DavidEGrayson 33:58a0ab6e9ad2 105 infiniteReckonerReportLoop();
DavidEGrayson 30:84be2d602dc0 106 }
DavidEGrayson 30:84be2d602dc0 107
DavidEGrayson 28:4374035df5e0 108 // This also tests the LineTracker by printing out a lot of data from it.
DavidEGrayson 28:4374035df5e0 109 void testLineFollowing()
DavidEGrayson 28:4374035df5e0 110 {
DavidEGrayson 44:b4a00fbab06b 111 doGyroCalibration();
DavidEGrayson 44:b4a00fbab06b 112 turnSensor.start();
DavidEGrayson 44:b4a00fbab06b 113
DavidEGrayson 28:4374035df5e0 114 led1 = 1;
DavidEGrayson 28:4374035df5e0 115 while(!button1DefinitelyPressed())
DavidEGrayson 28:4374035df5e0 116 {
DavidEGrayson 43:0e985a58f174 117 updateReckoner();
DavidEGrayson 28:4374035df5e0 118 }
DavidEGrayson 28:4374035df5e0 119 led2 = 1;
DavidEGrayson 28:4374035df5e0 120
DavidEGrayson 28:4374035df5e0 121 Pacer reportPacer(200000);
DavidEGrayson 28:4374035df5e0 122
DavidEGrayson 42:96671b71aac5 123 loadLineCalibration();
DavidEGrayson 28:4374035df5e0 124 uint16_t loopCount = 0;
DavidEGrayson 28:4374035df5e0 125 while(1)
DavidEGrayson 28:4374035df5e0 126 {
DavidEGrayson 43:0e985a58f174 127 updateReckoner();
DavidEGrayson 28:4374035df5e0 128 bool lineVisiblePrevious = lineTracker.getLineVisible();
DavidEGrayson 28:4374035df5e0 129 lineTracker.read();
DavidEGrayson 28:4374035df5e0 130 updateMotorsToFollowLine();
DavidEGrayson 28:4374035df5e0 131
DavidEGrayson 28:4374035df5e0 132 loopCount += 1;
DavidEGrayson 28:4374035df5e0 133
DavidEGrayson 28:4374035df5e0 134 if (lineVisiblePrevious != lineTracker.getLineVisible())
DavidEGrayson 28:4374035df5e0 135 {
DavidEGrayson 32:83a13b06093c 136 pc.printf("%5d ! %1d %4d | %5d %5d | %4d %4d %4d\r\n",
DavidEGrayson 28:4374035df5e0 137 loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
DavidEGrayson 32:83a13b06093c 138 motorLeftSpeed, motorRightSpeed,
DavidEGrayson 32:83a13b06093c 139 lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
DavidEGrayson 28:4374035df5e0 140 );
DavidEGrayson 28:4374035df5e0 141 }
DavidEGrayson 28:4374035df5e0 142
DavidEGrayson 28:4374035df5e0 143 if (reportPacer.pace())
DavidEGrayson 28:4374035df5e0 144 {
DavidEGrayson 32:83a13b06093c 145 pc.printf("%5d %1d %4d | %5d %5d | %4d %4d %4d\r\n",
DavidEGrayson 32:83a13b06093c 146 loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
DavidEGrayson 32:83a13b06093c 147 motorLeftSpeed, motorRightSpeed,
DavidEGrayson 28:4374035df5e0 148 lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
DavidEGrayson 28:4374035df5e0 149 );
DavidEGrayson 28:4374035df5e0 150 }
DavidEGrayson 28:4374035df5e0 151 }
DavidEGrayson 28:4374035df5e0 152 }
DavidEGrayson 28:4374035df5e0 153
DavidEGrayson 17:2df9861f53ee 154 void testDriveHome()
DavidEGrayson 16:8eaa5bc2bdb1 155 {
DavidEGrayson 43:0e985a58f174 156 doGyroCalibration();
DavidEGrayson 44:b4a00fbab06b 157 turnSensor.start();
DavidEGrayson 17:2df9861f53ee 158 led1 = 1;
DavidEGrayson 18:b65fbb795396 159 while(!button1DefinitelyPressed())
DavidEGrayson 17:2df9861f53ee 160 {
DavidEGrayson 43:0e985a58f174 161 updateReckoner();
DavidEGrayson 21:c279c6a83671 162 }
DavidEGrayson 33:58a0ab6e9ad2 163
DavidEGrayson 33:58a0ab6e9ad2 164 setLeds(1, 0, 1, 0);
DavidEGrayson 21:c279c6a83671 165 driveHomeAlmost();
DavidEGrayson 33:58a0ab6e9ad2 166
DavidEGrayson 33:58a0ab6e9ad2 167 //setLeds(0, 1, 1, 0);
DavidEGrayson 33:58a0ab6e9ad2 168 //finalSettleIn();
DavidEGrayson 33:58a0ab6e9ad2 169
DavidEGrayson 33:58a0ab6e9ad2 170 setLeds(1, 1, 1, 1);
DavidEGrayson 20:dbec34f0e76b 171 infiniteReckonerReportLoop();
DavidEGrayson 20:dbec34f0e76b 172 }
DavidEGrayson 20:dbec34f0e76b 173
DavidEGrayson 20:dbec34f0e76b 174 void testFinalSettleIn()
DavidEGrayson 20:dbec34f0e76b 175 {
DavidEGrayson 43:0e985a58f174 176 doGyroCalibration();
DavidEGrayson 44:b4a00fbab06b 177 turnSensor.start();
DavidEGrayson 20:dbec34f0e76b 178 led1 = 1;
DavidEGrayson 20:dbec34f0e76b 179 while(!button1DefinitelyPressed())
DavidEGrayson 20:dbec34f0e76b 180 {
DavidEGrayson 43:0e985a58f174 181 updateReckoner();
DavidEGrayson 20:dbec34f0e76b 182 }
DavidEGrayson 20:dbec34f0e76b 183 finalSettleIn();
DavidEGrayson 20:dbec34f0e76b 184 infiniteReckonerReportLoop();
DavidEGrayson 16:8eaa5bc2bdb1 185 }
DavidEGrayson 16:8eaa5bc2bdb1 186
DavidEGrayson 17:2df9861f53ee 187
DavidEGrayson 16:8eaa5bc2bdb1 188 void testButtons()
DavidEGrayson 16:8eaa5bc2bdb1 189 {
DavidEGrayson 16:8eaa5bc2bdb1 190 led1 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 191
DavidEGrayson 17:2df9861f53ee 192 while(!button1DefinitelyReleased());
DavidEGrayson 17:2df9861f53ee 193 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 194 led2 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 195
DavidEGrayson 16:8eaa5bc2bdb1 196 while(!button1DefinitelyReleased());
DavidEGrayson 16:8eaa5bc2bdb1 197 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 198 led3 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 199
DavidEGrayson 16:8eaa5bc2bdb1 200 while(!button1DefinitelyReleased());
DavidEGrayson 16:8eaa5bc2bdb1 201 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 202 led4 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 203
DavidEGrayson 16:8eaa5bc2bdb1 204 while(1){};
DavidEGrayson 16:8eaa5bc2bdb1 205 }
DavidEGrayson 16:8eaa5bc2bdb1 206
DavidEGrayson 12:835a4d24ae3b 207 void testReckoner()
DavidEGrayson 12:835a4d24ae3b 208 {
DavidEGrayson 42:96671b71aac5 209 doGyroCalibration();
DavidEGrayson 44:b4a00fbab06b 210 turnSensor.start();
DavidEGrayson 42:96671b71aac5 211 led1 = 1;
DavidEGrayson 13:bba5b3abd13f 212 Pacer reportPacer(100000);
DavidEGrayson 12:835a4d24ae3b 213 while(1)
DavidEGrayson 12:835a4d24ae3b 214 {
DavidEGrayson 43:0e985a58f174 215 updateReckoner();
DavidEGrayson 33:58a0ab6e9ad2 216
DavidEGrayson 43:0e985a58f174 217 led1 = reckoner.x > 0;
DavidEGrayson 43:0e985a58f174 218 led2 = reckoner.y > 0;
DavidEGrayson 33:58a0ab6e9ad2 219 showOrientationWithLeds34();
DavidEGrayson 12:835a4d24ae3b 220
DavidEGrayson 13:bba5b3abd13f 221 if (reportPacer.pace())
DavidEGrayson 13:bba5b3abd13f 222 {
DavidEGrayson 43:0e985a58f174 223 pc.printf("%6d %6d %11d %11d | %8d %8d %10f\r\n",
DavidEGrayson 43:0e985a58f174 224 reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
DavidEGrayson 19:a11ffc903774 225 encoderLeft.getCount(), encoderRight.getCount(), determinant());
DavidEGrayson 13:bba5b3abd13f 226 }
DavidEGrayson 12:835a4d24ae3b 227 }
DavidEGrayson 12:835a4d24ae3b 228 }
DavidEGrayson 12:835a4d24ae3b 229
DavidEGrayson 40:6fa672be85ec 230 void testTurnSensor()
DavidEGrayson 40:6fa672be85ec 231 {
DavidEGrayson 40:6fa672be85ec 232 pc.printf("Test turn sensor\r\n");
DavidEGrayson 43:0e985a58f174 233 doGyroCalibration();
DavidEGrayson 42:96671b71aac5 234 led1 = 1;
DavidEGrayson 42:96671b71aac5 235 //Pacer reportPacer(200000); // 0.2 s
DavidEGrayson 42:96671b71aac5 236 Pacer reportPacer(10000000); // 10 s
DavidEGrayson 42:96671b71aac5 237 Timer timer;
DavidEGrayson 42:96671b71aac5 238 timer.start();
DavidEGrayson 40:6fa672be85ec 239 turnSensor.start();
DavidEGrayson 40:6fa672be85ec 240 while(1)
DavidEGrayson 40:6fa672be85ec 241 {
DavidEGrayson 40:6fa672be85ec 242 turnSensor.update();
DavidEGrayson 40:6fa672be85ec 243 if (reportPacer.pace())
DavidEGrayson 40:6fa672be85ec 244 {
DavidEGrayson 42:96671b71aac5 245 pc.printf("%u, %d, %d\r\n",
DavidEGrayson 42:96671b71aac5 246 timer.read_ms(),
DavidEGrayson 42:96671b71aac5 247 turnSensor.getAngleDegrees(),
DavidEGrayson 42:96671b71aac5 248 turnSensor.getAngleMillidegrees());
DavidEGrayson 40:6fa672be85ec 249 }
DavidEGrayson 40:6fa672be85ec 250 }
DavidEGrayson 40:6fa672be85ec 251 }
DavidEGrayson 40:6fa672be85ec 252
DavidEGrayson 42:96671b71aac5 253 void testL3gAndShowAverage()
DavidEGrayson 41:3ead1dd2cc3a 254 {
DavidEGrayson 41:3ead1dd2cc3a 255 wait_ms(2000);
DavidEGrayson 41:3ead1dd2cc3a 256 Pacer reportPacer(750000);
DavidEGrayson 41:3ead1dd2cc3a 257 Pacer readingPacer(2000);
DavidEGrayson 41:3ead1dd2cc3a 258 int32_t total = 0;
DavidEGrayson 41:3ead1dd2cc3a 259 int32_t readingCount = 0;
DavidEGrayson 41:3ead1dd2cc3a 260 while(1)
DavidEGrayson 41:3ead1dd2cc3a 261 {
DavidEGrayson 41:3ead1dd2cc3a 262 if (readingPacer.pace())
DavidEGrayson 41:3ead1dd2cc3a 263 {
DavidEGrayson 41:3ead1dd2cc3a 264 int32_t result = l3gZAvailable();
DavidEGrayson 41:3ead1dd2cc3a 265 if (result == 1)
DavidEGrayson 41:3ead1dd2cc3a 266 {
DavidEGrayson 41:3ead1dd2cc3a 267 int32_t gz = l3gZRead();
DavidEGrayson 41:3ead1dd2cc3a 268 if (gz < -500000)
DavidEGrayson 41:3ead1dd2cc3a 269 {
DavidEGrayson 41:3ead1dd2cc3a 270 pc.printf("l3gZRead error: %d\n", gz);
DavidEGrayson 41:3ead1dd2cc3a 271 }
DavidEGrayson 41:3ead1dd2cc3a 272 else
DavidEGrayson 41:3ead1dd2cc3a 273 {
DavidEGrayson 41:3ead1dd2cc3a 274 total += gz;
DavidEGrayson 41:3ead1dd2cc3a 275 readingCount += 1;
DavidEGrayson 41:3ead1dd2cc3a 276 }
DavidEGrayson 41:3ead1dd2cc3a 277 }
DavidEGrayson 41:3ead1dd2cc3a 278 else if (result != 0)
DavidEGrayson 41:3ead1dd2cc3a 279 {
DavidEGrayson 41:3ead1dd2cc3a 280 pc.printf("l3gZAvailable error: %d\n", result);
DavidEGrayson 41:3ead1dd2cc3a 281 }
DavidEGrayson 41:3ead1dd2cc3a 282 }
DavidEGrayson 41:3ead1dd2cc3a 283
DavidEGrayson 41:3ead1dd2cc3a 284 if (reportPacer.pace())
DavidEGrayson 41:3ead1dd2cc3a 285 {
DavidEGrayson 41:3ead1dd2cc3a 286 float average = (float)total / readingCount;
DavidEGrayson 41:3ead1dd2cc3a 287 pc.printf("%d, %d, %f\r\n", total, readingCount, average);
DavidEGrayson 41:3ead1dd2cc3a 288 }
DavidEGrayson 41:3ead1dd2cc3a 289 }
DavidEGrayson 41:3ead1dd2cc3a 290
DavidEGrayson 41:3ead1dd2cc3a 291 // Gyro calibration results get hardcoded into TurnSensor::update()
DavidEGrayson 41:3ead1dd2cc3a 292 // for now until we figure out something better.
DavidEGrayson 41:3ead1dd2cc3a 293 }
DavidEGrayson 41:3ead1dd2cc3a 294
DavidEGrayson 40:6fa672be85ec 295 void testL3g()
DavidEGrayson 40:6fa672be85ec 296 {
DavidEGrayson 40:6fa672be85ec 297 Pacer reportPacer(750000);
DavidEGrayson 40:6fa672be85ec 298 Timer timer;
DavidEGrayson 40:6fa672be85ec 299 timer.start();
DavidEGrayson 40:6fa672be85ec 300 int32_t gz = 0;
DavidEGrayson 40:6fa672be85ec 301 bool reportedReading = false;
DavidEGrayson 40:6fa672be85ec 302 while(1)
DavidEGrayson 40:6fa672be85ec 303 {
DavidEGrayson 40:6fa672be85ec 304 int32_t result = l3gZAvailable();
DavidEGrayson 40:6fa672be85ec 305 if (result == 1)
DavidEGrayson 40:6fa672be85ec 306 {
DavidEGrayson 40:6fa672be85ec 307 gz = l3gZRead();
DavidEGrayson 40:6fa672be85ec 308 reportedReading = false;
DavidEGrayson 40:6fa672be85ec 309 if (gz > 100 || gz < -100)
DavidEGrayson 40:6fa672be85ec 310 {
DavidEGrayson 42:96671b71aac5 311 pc.printf("%u, %d\r\n", timer.read_us(), gz);
DavidEGrayson 40:6fa672be85ec 312 reportedReading = true;
DavidEGrayson 40:6fa672be85ec 313 }
DavidEGrayson 40:6fa672be85ec 314 }
DavidEGrayson 40:6fa672be85ec 315 else if (result != 0)
DavidEGrayson 40:6fa672be85ec 316 {
DavidEGrayson 40:6fa672be85ec 317 pc.printf("l3gZAvailable error: %d\n", result);
DavidEGrayson 40:6fa672be85ec 318 }
DavidEGrayson 40:6fa672be85ec 319
DavidEGrayson 40:6fa672be85ec 320 if (reportPacer.pace() && !reportedReading)
DavidEGrayson 40:6fa672be85ec 321 {
DavidEGrayson 42:96671b71aac5 322 pc.printf("%u, %d\r\n", timer.read_us(), gz);
DavidEGrayson 40:6fa672be85ec 323 reportedReading = true;
DavidEGrayson 40:6fa672be85ec 324 }
DavidEGrayson 40:6fa672be85ec 325 }
DavidEGrayson 40:6fa672be85ec 326 }
DavidEGrayson 40:6fa672be85ec 327
DavidEGrayson 45:81dd782bc0b4 328 void testLineSensorsAndCalibrate()
DavidEGrayson 10:e4dd36148539 329 {
DavidEGrayson 10:e4dd36148539 330 led1 = 1;
DavidEGrayson 10:e4dd36148539 331 Pacer reportPacer(100000);
DavidEGrayson 31:739b91331f31 332
DavidEGrayson 45:81dd782bc0b4 333 const uint16_t * values = lineTracker.rawValues;
DavidEGrayson 45:81dd782bc0b4 334 const uint16_t * min = lineTracker.calibratedMinimum;
DavidEGrayson 45:81dd782bc0b4 335 const uint16_t * max = lineTracker.calibratedMaximum;
DavidEGrayson 45:81dd782bc0b4 336
DavidEGrayson 45:81dd782bc0b4 337 const bool printBarGraph = true;
DavidEGrayson 10:e4dd36148539 338 while (1)
DavidEGrayson 45:81dd782bc0b4 339 {
DavidEGrayson 45:81dd782bc0b4 340 lineTracker.read();
DavidEGrayson 45:81dd782bc0b4 341
DavidEGrayson 45:81dd782bc0b4 342 // Hold down button 1 and expose the line sensor to its typical
DavidEGrayson 45:81dd782bc0b4 343 // values to do calibration.
DavidEGrayson 45:81dd782bc0b4 344 if (button1DefinitelyPressed())
DavidEGrayson 31:739b91331f31 345 {
DavidEGrayson 45:81dd782bc0b4 346 led2 = 1;
DavidEGrayson 45:81dd782bc0b4 347 lineTracker.updateCalibration();
DavidEGrayson 31:739b91331f31 348 }
DavidEGrayson 45:81dd782bc0b4 349 else
DavidEGrayson 31:739b91331f31 350 {
DavidEGrayson 45:81dd782bc0b4 351 led2 = 0;
DavidEGrayson 31:739b91331f31 352 }
DavidEGrayson 31:739b91331f31 353
DavidEGrayson 10:e4dd36148539 354 if (reportPacer.pace())
DavidEGrayson 10:e4dd36148539 355 {
DavidEGrayson 10:e4dd36148539 356 if (printBarGraph)
DavidEGrayson 10:e4dd36148539 357 {
DavidEGrayson 10:e4dd36148539 358 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 31:739b91331f31 359 printBar("L", values[0]);
DavidEGrayson 31:739b91331f31 360 printBar("M", values[1]);
DavidEGrayson 31:739b91331f31 361 printBar("R", values[2]);
DavidEGrayson 31:739b91331f31 362 pc.printf("%4d %4d \r\n", min[0], max[0]);
DavidEGrayson 31:739b91331f31 363 pc.printf("%4d %4d \r\n", min[1], max[1]);
DavidEGrayson 31:739b91331f31 364 pc.printf("%4d %4d \r\n", min[2], max[2]);
DavidEGrayson 10:e4dd36148539 365 }
DavidEGrayson 10:e4dd36148539 366 else
DavidEGrayson 10:e4dd36148539 367 {
DavidEGrayson 31:739b91331f31 368 pc.printf("%8d %8d %8d\r\n", values[0], values[1], values[2]);
DavidEGrayson 10:e4dd36148539 369 }
DavidEGrayson 10:e4dd36148539 370 }
DavidEGrayson 10:e4dd36148539 371 }
DavidEGrayson 10:e4dd36148539 372 }
DavidEGrayson 8:78b1ff957cba 373
DavidEGrayson 39:b19dfc5d4d4b 374 void testMotorSpeed()
DavidEGrayson 39:b19dfc5d4d4b 375 {
DavidEGrayson 39:b19dfc5d4d4b 376 led1 = 1;
DavidEGrayson 39:b19dfc5d4d4b 377 motorsSpeedSet(400, 400);
DavidEGrayson 39:b19dfc5d4d4b 378 wait_ms(4000);
DavidEGrayson 39:b19dfc5d4d4b 379 uint32_t left = encoderLeft.getCount();
DavidEGrayson 39:b19dfc5d4d4b 380 uint32_t right = encoderRight.getCount();
DavidEGrayson 39:b19dfc5d4d4b 381 motorsSpeedSet(0, 0);
DavidEGrayson 39:b19dfc5d4d4b 382 Pacer reportPacer(500000);
DavidEGrayson 39:b19dfc5d4d4b 383 while (1)
DavidEGrayson 39:b19dfc5d4d4b 384 {
DavidEGrayson 39:b19dfc5d4d4b 385 if (reportPacer.pace())
DavidEGrayson 39:b19dfc5d4d4b 386 {
DavidEGrayson 39:b19dfc5d4d4b 387 led2 = 1;
DavidEGrayson 39:b19dfc5d4d4b 388 pc.printf("%8d %8d\r\n", left, right);
DavidEGrayson 39:b19dfc5d4d4b 389 led2 = 0;
DavidEGrayson 39:b19dfc5d4d4b 390 }
DavidEGrayson 39:b19dfc5d4d4b 391 }
DavidEGrayson 39:b19dfc5d4d4b 392 }
DavidEGrayson 39:b19dfc5d4d4b 393
DavidEGrayson 9:9734347b5756 394 void testEncoders()
DavidEGrayson 9:9734347b5756 395 {
DavidEGrayson 9:9734347b5756 396 Pacer reportPacer(500000);
DavidEGrayson 10:e4dd36148539 397 led1 = 1;
DavidEGrayson 9:9734347b5756 398 while(1)
DavidEGrayson 9:9734347b5756 399 {
DavidEGrayson 9:9734347b5756 400 while(encoderBuffer.hasEvents())
DavidEGrayson 9:9734347b5756 401 {
DavidEGrayson 9:9734347b5756 402 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 9:9734347b5756 403 }
DavidEGrayson 9:9734347b5756 404
DavidEGrayson 9:9734347b5756 405 if(reportPacer.pace())
DavidEGrayson 9:9734347b5756 406 {
DavidEGrayson 9:9734347b5756 407 led2 = 1;
DavidEGrayson 31:739b91331f31 408 pc.printf("%8d %8d\r\n", encoderLeft.getCount(), encoderRight.getCount());
DavidEGrayson 9:9734347b5756 409 led2 = 0;
DavidEGrayson 10:e4dd36148539 410 }
DavidEGrayson 9:9734347b5756 411 }
DavidEGrayson 9:9734347b5756 412 }
DavidEGrayson 9:9734347b5756 413
DavidEGrayson 8:78b1ff957cba 414 void testMotors()
DavidEGrayson 8:78b1ff957cba 415 {
DavidEGrayson 8:78b1ff957cba 416 led1 = 1;
DavidEGrayson 8:78b1ff957cba 417 led2 = 0;
DavidEGrayson 8:78b1ff957cba 418 led3 = 0;
DavidEGrayson 8:78b1ff957cba 419 while(1)
DavidEGrayson 8:78b1ff957cba 420 {
DavidEGrayson 9:9734347b5756 421 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 422 led2 = 0;
DavidEGrayson 8:78b1ff957cba 423 led3 = 0;
DavidEGrayson 8:78b1ff957cba 424 wait(2);
DavidEGrayson 8:78b1ff957cba 425
DavidEGrayson 9:9734347b5756 426 motorsSpeedSet(300, 300);
DavidEGrayson 8:78b1ff957cba 427 wait(2);
DavidEGrayson 8:78b1ff957cba 428
DavidEGrayson 9:9734347b5756 429 motorsSpeedSet(-300, 300);
DavidEGrayson 8:78b1ff957cba 430 wait(2);
DavidEGrayson 8:78b1ff957cba 431
DavidEGrayson 9:9734347b5756 432 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 433 led2 = 1;
DavidEGrayson 8:78b1ff957cba 434 wait(2);
DavidEGrayson 8:78b1ff957cba 435
DavidEGrayson 9:9734347b5756 436 motorsSpeedSet(600, 600);
DavidEGrayson 8:78b1ff957cba 437 wait(2);
DavidEGrayson 8:78b1ff957cba 438
DavidEGrayson 9:9734347b5756 439 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 440 led3 = 1;
DavidEGrayson 8:78b1ff957cba 441 wait(2);
DavidEGrayson 8:78b1ff957cba 442
DavidEGrayson 9:9734347b5756 443 motorsSpeedSet(1200, 1200);
DavidEGrayson 8:78b1ff957cba 444 wait(2);
DavidEGrayson 8:78b1ff957cba 445 }
DavidEGrayson 10:e4dd36148539 446 }
DavidEGrayson 10:e4dd36148539 447
DavidEGrayson 20:dbec34f0e76b 448 void infiniteReckonerReportLoop()
DavidEGrayson 20:dbec34f0e76b 449 {
DavidEGrayson 20:dbec34f0e76b 450 Pacer reportPacer(200000);
DavidEGrayson 20:dbec34f0e76b 451 while(1)
DavidEGrayson 20:dbec34f0e76b 452 {
DavidEGrayson 33:58a0ab6e9ad2 453 showOrientationWithLeds34();
DavidEGrayson 20:dbec34f0e76b 454 if(reportPacer.pace())
DavidEGrayson 20:dbec34f0e76b 455 {
DavidEGrayson 43:0e985a58f174 456 pc.printf("%6d %6d %11d %11d | %11f %11f\r\n",
DavidEGrayson 43:0e985a58f174 457 reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
DavidEGrayson 20:dbec34f0e76b 458 determinant(), dotProduct());
DavidEGrayson 20:dbec34f0e76b 459 }
DavidEGrayson 20:dbec34f0e76b 460 }
DavidEGrayson 20:dbec34f0e76b 461 }
DavidEGrayson 20:dbec34f0e76b 462
DavidEGrayson 31:739b91331f31 463 // with should be between 0 and 63
DavidEGrayson 31:739b91331f31 464 void printBar(const char * name, uint16_t result)
DavidEGrayson 10:e4dd36148539 465 {
DavidEGrayson 31:739b91331f31 466 pc.printf("%-2s %5d |", name, result);
DavidEGrayson 31:739b91331f31 467 uint16_t width = result >> 4;
DavidEGrayson 31:739b91331f31 468 if (width > 63) { width = 63; }
DavidEGrayson 10:e4dd36148539 469 uint8_t i;
DavidEGrayson 10:e4dd36148539 470 for(i = 0; i < width; i++){ pc.putc('#'); }
DavidEGrayson 10:e4dd36148539 471 for(; i < 63; i++){ pc.putc(' '); }
DavidEGrayson 10:e4dd36148539 472 pc.putc('|');
DavidEGrayson 31:739b91331f31 473 pc.putc('\r');
DavidEGrayson 10:e4dd36148539 474 pc.putc('\n');
DavidEGrayson 8:78b1ff957cba 475 }