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@44:b4a00fbab06b, 2019-07-28 (annotated)

Committer:
DavidEGrayson
Date:
Sun Jul 28 01:22:01 2019 +0000
Revision:
44:b4a00fbab06b
Parent:
43:0e985a58f174
Child:
45:81dd782bc0b4
Get testDriveHome to work: the robot got to within about 6 inches of where it was in one test I did.

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 10:e4dd36148539 328 void testLineSensors()
DavidEGrayson 10:e4dd36148539 329 {
DavidEGrayson 10:e4dd36148539 330 led1 = 1;
DavidEGrayson 10:e4dd36148539 331 Pacer reportPacer(100000);
DavidEGrayson 31:739b91331f31 332 Pacer clearStatsPacer(2000000);
DavidEGrayson 31:739b91331f31 333
DavidEGrayson 31:739b91331f31 334 uint16_t min[LINE_SENSOR_COUNT];
DavidEGrayson 31:739b91331f31 335 uint16_t max[LINE_SENSOR_COUNT];
DavidEGrayson 31:739b91331f31 336
DavidEGrayson 10:e4dd36148539 337 bool const printBarGraph = true;
DavidEGrayson 10:e4dd36148539 338 while (1)
DavidEGrayson 10:e4dd36148539 339 {
DavidEGrayson 31:739b91331f31 340 if (clearStatsPacer.pace())
DavidEGrayson 31:739b91331f31 341 {
DavidEGrayson 31:739b91331f31 342 for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
DavidEGrayson 31:739b91331f31 343 {
DavidEGrayson 31:739b91331f31 344 min[i] = 0xFFFF;
DavidEGrayson 31:739b91331f31 345 max[i] = 0;
DavidEGrayson 31:739b91331f31 346 }
DavidEGrayson 31:739b91331f31 347 }
DavidEGrayson 31:739b91331f31 348
DavidEGrayson 31:739b91331f31 349 uint16_t values[3];
DavidEGrayson 31:739b91331f31 350 readSensors(values);
DavidEGrayson 31:739b91331f31 351
DavidEGrayson 31:739b91331f31 352 for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
DavidEGrayson 31:739b91331f31 353 {
DavidEGrayson 31:739b91331f31 354 if (values[i] > max[i]){ max[i] = values[i]; }
DavidEGrayson 31:739b91331f31 355 if (values[i] < min[i]){ min[i] = values[i]; }
DavidEGrayson 31:739b91331f31 356 }
DavidEGrayson 31:739b91331f31 357
DavidEGrayson 10:e4dd36148539 358 if (reportPacer.pace())
DavidEGrayson 10:e4dd36148539 359 {
DavidEGrayson 10:e4dd36148539 360 if (printBarGraph)
DavidEGrayson 10:e4dd36148539 361 {
DavidEGrayson 10:e4dd36148539 362 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 31:739b91331f31 363 printBar("L", values[0]);
DavidEGrayson 31:739b91331f31 364 printBar("M", values[1]);
DavidEGrayson 31:739b91331f31 365 printBar("R", values[2]);
DavidEGrayson 31:739b91331f31 366 pc.printf("%4d %4d \r\n", min[0], max[0]);
DavidEGrayson 31:739b91331f31 367 pc.printf("%4d %4d \r\n", min[1], max[1]);
DavidEGrayson 31:739b91331f31 368 pc.printf("%4d %4d \r\n", min[2], max[2]);
DavidEGrayson 10:e4dd36148539 369 }
DavidEGrayson 10:e4dd36148539 370 else
DavidEGrayson 10:e4dd36148539 371 {
DavidEGrayson 31:739b91331f31 372 pc.printf("%8d %8d %8d\r\n", values[0], values[1], values[2]);
DavidEGrayson 10:e4dd36148539 373 }
DavidEGrayson 10:e4dd36148539 374 }
DavidEGrayson 10:e4dd36148539 375 }
DavidEGrayson 10:e4dd36148539 376 }
DavidEGrayson 8:78b1ff957cba 377
DavidEGrayson 24:fc01d9125d3b 378 // Values from David's office Values from dev lab,
DavidEGrayson 24:fc01d9125d3b 379 // in the day time, 2014-02-27: 2014-02-27:
DavidEGrayson 22:44c032e59ff5 380 // # calmin calmax
DavidEGrayson 24:fc01d9125d3b 381 // 0 34872 59726 0 40617 60222
DavidEGrayson 24:fc01d9125d3b 382 // 1 29335 60110 1 36937 61198
DavidEGrayson 24:fc01d9125d3b 383 // 2 23845 58446 2 33848 58862
DavidEGrayson 22:44c032e59ff5 384 void testCalibrate()
DavidEGrayson 22:44c032e59ff5 385 {
DavidEGrayson 22:44c032e59ff5 386 Timer timer;
DavidEGrayson 22:44c032e59ff5 387 timer.start();
DavidEGrayson 22:44c032e59ff5 388
DavidEGrayson 22:44c032e59ff5 389 Pacer reportPacer(200000);
DavidEGrayson 22:44c032e59ff5 390
DavidEGrayson 24:fc01d9125d3b 391 bool doneCalibrating = false;
DavidEGrayson 24:fc01d9125d3b 392
DavidEGrayson 24:fc01d9125d3b 393 led1 = 1;
DavidEGrayson 24:fc01d9125d3b 394
DavidEGrayson 22:44c032e59ff5 395 while(1)
DavidEGrayson 22:44c032e59ff5 396 {
DavidEGrayson 22:44c032e59ff5 397 lineTracker.read();
DavidEGrayson 24:fc01d9125d3b 398 if(!doneCalibrating)
DavidEGrayson 24:fc01d9125d3b 399 {
DavidEGrayson 24:fc01d9125d3b 400 lineTracker.updateCalibration();
DavidEGrayson 24:fc01d9125d3b 401 }
DavidEGrayson 24:fc01d9125d3b 402
DavidEGrayson 24:fc01d9125d3b 403 led3 = doneCalibrating;
DavidEGrayson 24:fc01d9125d3b 404 led4 = lineTracker.getLineVisible();
DavidEGrayson 24:fc01d9125d3b 405
DavidEGrayson 24:fc01d9125d3b 406 if (button1DefinitelyPressed())
DavidEGrayson 24:fc01d9125d3b 407 {
DavidEGrayson 24:fc01d9125d3b 408 doneCalibrating = true;
DavidEGrayson 24:fc01d9125d3b 409 }
DavidEGrayson 22:44c032e59ff5 410
DavidEGrayson 22:44c032e59ff5 411 if (reportPacer.pace())
DavidEGrayson 22:44c032e59ff5 412 {
DavidEGrayson 22:44c032e59ff5 413 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 22:44c032e59ff5 414 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 22:44c032e59ff5 415 {
DavidEGrayson 22:44c032e59ff5 416 pc.printf("%-2d %5d %5d %5d\r\n", s, lineTracker.calibratedMinimum[s], lineTracker.rawValues[s], lineTracker.calibratedMaximum[s]);
DavidEGrayson 22:44c032e59ff5 417 }
DavidEGrayson 22:44c032e59ff5 418 }
DavidEGrayson 22:44c032e59ff5 419 }
DavidEGrayson 22:44c032e59ff5 420 }
DavidEGrayson 22:44c032e59ff5 421
DavidEGrayson 39:b19dfc5d4d4b 422 void testMotorSpeed()
DavidEGrayson 39:b19dfc5d4d4b 423 {
DavidEGrayson 39:b19dfc5d4d4b 424 led1 = 1;
DavidEGrayson 39:b19dfc5d4d4b 425 motorsSpeedSet(400, 400);
DavidEGrayson 39:b19dfc5d4d4b 426 wait_ms(4000);
DavidEGrayson 39:b19dfc5d4d4b 427 uint32_t left = encoderLeft.getCount();
DavidEGrayson 39:b19dfc5d4d4b 428 uint32_t right = encoderRight.getCount();
DavidEGrayson 39:b19dfc5d4d4b 429 motorsSpeedSet(0, 0);
DavidEGrayson 39:b19dfc5d4d4b 430 Pacer reportPacer(500000);
DavidEGrayson 39:b19dfc5d4d4b 431 while (1)
DavidEGrayson 39:b19dfc5d4d4b 432 {
DavidEGrayson 39:b19dfc5d4d4b 433 if (reportPacer.pace())
DavidEGrayson 39:b19dfc5d4d4b 434 {
DavidEGrayson 39:b19dfc5d4d4b 435 led2 = 1;
DavidEGrayson 39:b19dfc5d4d4b 436 pc.printf("%8d %8d\r\n", left, right);
DavidEGrayson 39:b19dfc5d4d4b 437 led2 = 0;
DavidEGrayson 39:b19dfc5d4d4b 438 }
DavidEGrayson 39:b19dfc5d4d4b 439 }
DavidEGrayson 39:b19dfc5d4d4b 440 }
DavidEGrayson 39:b19dfc5d4d4b 441
DavidEGrayson 9:9734347b5756 442 void testEncoders()
DavidEGrayson 9:9734347b5756 443 {
DavidEGrayson 9:9734347b5756 444 Pacer reportPacer(500000);
DavidEGrayson 10:e4dd36148539 445 led1 = 1;
DavidEGrayson 9:9734347b5756 446 while(1)
DavidEGrayson 9:9734347b5756 447 {
DavidEGrayson 9:9734347b5756 448 while(encoderBuffer.hasEvents())
DavidEGrayson 9:9734347b5756 449 {
DavidEGrayson 9:9734347b5756 450 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 9:9734347b5756 451 }
DavidEGrayson 9:9734347b5756 452
DavidEGrayson 9:9734347b5756 453 if(reportPacer.pace())
DavidEGrayson 9:9734347b5756 454 {
DavidEGrayson 9:9734347b5756 455 led2 = 1;
DavidEGrayson 31:739b91331f31 456 pc.printf("%8d %8d\r\n", encoderLeft.getCount(), encoderRight.getCount());
DavidEGrayson 9:9734347b5756 457 led2 = 0;
DavidEGrayson 10:e4dd36148539 458 }
DavidEGrayson 9:9734347b5756 459 }
DavidEGrayson 9:9734347b5756 460 }
DavidEGrayson 9:9734347b5756 461
DavidEGrayson 8:78b1ff957cba 462 void testMotors()
DavidEGrayson 8:78b1ff957cba 463 {
DavidEGrayson 8:78b1ff957cba 464 led1 = 1;
DavidEGrayson 8:78b1ff957cba 465 led2 = 0;
DavidEGrayson 8:78b1ff957cba 466 led3 = 0;
DavidEGrayson 8:78b1ff957cba 467 while(1)
DavidEGrayson 8:78b1ff957cba 468 {
DavidEGrayson 9:9734347b5756 469 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 470 led2 = 0;
DavidEGrayson 8:78b1ff957cba 471 led3 = 0;
DavidEGrayson 8:78b1ff957cba 472 wait(2);
DavidEGrayson 8:78b1ff957cba 473
DavidEGrayson 9:9734347b5756 474 motorsSpeedSet(300, 300);
DavidEGrayson 8:78b1ff957cba 475 wait(2);
DavidEGrayson 8:78b1ff957cba 476
DavidEGrayson 9:9734347b5756 477 motorsSpeedSet(-300, 300);
DavidEGrayson 8:78b1ff957cba 478 wait(2);
DavidEGrayson 8:78b1ff957cba 479
DavidEGrayson 9:9734347b5756 480 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 481 led2 = 1;
DavidEGrayson 8:78b1ff957cba 482 wait(2);
DavidEGrayson 8:78b1ff957cba 483
DavidEGrayson 9:9734347b5756 484 motorsSpeedSet(600, 600);
DavidEGrayson 8:78b1ff957cba 485 wait(2);
DavidEGrayson 8:78b1ff957cba 486
DavidEGrayson 9:9734347b5756 487 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 488 led3 = 1;
DavidEGrayson 8:78b1ff957cba 489 wait(2);
DavidEGrayson 8:78b1ff957cba 490
DavidEGrayson 9:9734347b5756 491 motorsSpeedSet(1200, 1200);
DavidEGrayson 8:78b1ff957cba 492 wait(2);
DavidEGrayson 8:78b1ff957cba 493 }
DavidEGrayson 10:e4dd36148539 494 }
DavidEGrayson 10:e4dd36148539 495
DavidEGrayson 20:dbec34f0e76b 496 void infiniteReckonerReportLoop()
DavidEGrayson 20:dbec34f0e76b 497 {
DavidEGrayson 20:dbec34f0e76b 498 Pacer reportPacer(200000);
DavidEGrayson 20:dbec34f0e76b 499 while(1)
DavidEGrayson 20:dbec34f0e76b 500 {
DavidEGrayson 33:58a0ab6e9ad2 501 showOrientationWithLeds34();
DavidEGrayson 20:dbec34f0e76b 502 if(reportPacer.pace())
DavidEGrayson 20:dbec34f0e76b 503 {
DavidEGrayson 43:0e985a58f174 504 pc.printf("%6d %6d %11d %11d | %11f %11f\r\n",
DavidEGrayson 43:0e985a58f174 505 reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
DavidEGrayson 20:dbec34f0e76b 506 determinant(), dotProduct());
DavidEGrayson 20:dbec34f0e76b 507 }
DavidEGrayson 20:dbec34f0e76b 508 }
DavidEGrayson 20:dbec34f0e76b 509 }
DavidEGrayson 20:dbec34f0e76b 510
DavidEGrayson 31:739b91331f31 511 // with should be between 0 and 63
DavidEGrayson 31:739b91331f31 512 void printBar(const char * name, uint16_t result)
DavidEGrayson 10:e4dd36148539 513 {
DavidEGrayson 31:739b91331f31 514 pc.printf("%-2s %5d |", name, result);
DavidEGrayson 31:739b91331f31 515 uint16_t width = result >> 4;
DavidEGrayson 31:739b91331f31 516 if (width > 63) { width = 63; }
DavidEGrayson 10:e4dd36148539 517 uint8_t i;
DavidEGrayson 10:e4dd36148539 518 for(i = 0; i < width; i++){ pc.putc('#'); }
DavidEGrayson 10:e4dd36148539 519 for(; i < 63; i++){ pc.putc(' '); }
DavidEGrayson 10:e4dd36148539 520 pc.putc('|');
DavidEGrayson 31:739b91331f31 521 pc.putc('\r');
DavidEGrayson 10:e4dd36148539 522 pc.putc('\n');
DavidEGrayson 8:78b1ff957cba 523 }