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@43:0e985a58f174, 2019-07-27 (annotated)

Committer:
DavidEGrayson
Date:
Sat Jul 27 22:52:19 2019 +0000
Revision:
43:0e985a58f174
Parent:
42:96671b71aac5
Child:
44:b4a00fbab06b
Changed reckoner to use readings from turnSensor (Gyro) to get its direction vector instead of encoder ticks.

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