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@33:58a0ab6e9ad2, 2014-03-05 (annotated)

Committer:
DavidEGrayson
Date:
Wed Mar 05 02:50:09 2014 +0000
Revision:
33:58a0ab6e9ad2
Parent:
32:83a13b06093c
Child:
37:23000a47ed2b
Bunch of stuff.  Then reduced drivingSpeed to 400.

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