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@39:b19dfc5d4d4b, 2019-07-25 (annotated)

Committer:
DavidEGrayson
Date:
Thu Jul 25 02:11:25 2019 +0000
Revision:
39:b19dfc5d4d4b
Parent:
37:23000a47ed2b
Child:
40:6fa672be85ec
New motors!  100:1 HP.

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