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@48:597738b77f77, 2019-08-13 (annotated)

Committer:
DavidEGrayson
Date:
Tue Aug 13 21:21:17 2019 +0000
Revision:
48:597738b77f77
Parent:
46:df2c2d25c070 
Changes from before the contest, I think.

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