David Grayson / Mbed 2 deprecated LineFollowing

David's line following code from the LVBots competition, 2015.

Dependencies:   GeneralDebouncer Pacer PololuEncoder mbed

Fork of DeadReckoning by David Grayson

test.cpp@39:a5e25fd52ff8, 2015-04-14 (annotated)

Committer:
DavidEGrayson
Date:
Tue Apr 14 01:06:41 2015 +0000
Revision:
39:a5e25fd52ff8
Parent:
37:23000a47ed2b
Child:
44:edcacba44760
Made it follow the line a little faster (600 out of 1200).  Need a derivative term though, because it oscillates back and forth on the straight part.

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 16:8eaa5bc2bdb1 240 void testButtons()
DavidEGrayson 16:8eaa5bc2bdb1 241 {
DavidEGrayson 16:8eaa5bc2bdb1 242 led1 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 243
DavidEGrayson 17:2df9861f53ee 244 while(!button1DefinitelyReleased());
DavidEGrayson 17:2df9861f53ee 245 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 246 led2 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 247
DavidEGrayson 16:8eaa5bc2bdb1 248 while(!button1DefinitelyReleased());
DavidEGrayson 16:8eaa5bc2bdb1 249 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 250 led3 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 251
DavidEGrayson 16:8eaa5bc2bdb1 252 while(!button1DefinitelyReleased());
DavidEGrayson 16:8eaa5bc2bdb1 253 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 254 led4 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 255
DavidEGrayson 16:8eaa5bc2bdb1 256 while(1){};
DavidEGrayson 16:8eaa5bc2bdb1 257 }
DavidEGrayson 16:8eaa5bc2bdb1 258
DavidEGrayson 12:835a4d24ae3b 259 void testReckoner()
DavidEGrayson 12:835a4d24ae3b 260 {
DavidEGrayson 13:bba5b3abd13f 261 Pacer reportPacer(100000);
DavidEGrayson 12:835a4d24ae3b 262 while(1)
DavidEGrayson 12:835a4d24ae3b 263 {
DavidEGrayson 12:835a4d24ae3b 264 updateReckonerFromEncoders();
DavidEGrayson 33:58a0ab6e9ad2 265
DavidEGrayson 33:58a0ab6e9ad2 266 led1 = (reckoner.x > 0);
DavidEGrayson 33:58a0ab6e9ad2 267 led2 = (reckoner.y > 0);
DavidEGrayson 33:58a0ab6e9ad2 268 showOrientationWithLeds34();
DavidEGrayson 12:835a4d24ae3b 269
DavidEGrayson 13:bba5b3abd13f 270 if (reportPacer.pace())
DavidEGrayson 13:bba5b3abd13f 271 {
DavidEGrayson 19:a11ffc903774 272 pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n",
DavidEGrayson 13:bba5b3abd13f 273 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 19:a11ffc903774 274 encoderLeft.getCount(), encoderRight.getCount(), determinant());
DavidEGrayson 13:bba5b3abd13f 275 }
DavidEGrayson 12:835a4d24ae3b 276 }
DavidEGrayson 12:835a4d24ae3b 277 }
DavidEGrayson 12:835a4d24ae3b 278
DavidEGrayson 10:e4dd36148539 279 void testLineSensors()
DavidEGrayson 10:e4dd36148539 280 {
DavidEGrayson 10:e4dd36148539 281 led1 = 1;
DavidEGrayson 10:e4dd36148539 282 Pacer reportPacer(100000);
DavidEGrayson 31:739b91331f31 283 Pacer clearStatsPacer(2000000);
DavidEGrayson 31:739b91331f31 284
DavidEGrayson 31:739b91331f31 285 uint16_t min[LINE_SENSOR_COUNT];
DavidEGrayson 31:739b91331f31 286 uint16_t max[LINE_SENSOR_COUNT];
DavidEGrayson 31:739b91331f31 287
DavidEGrayson 10:e4dd36148539 288 bool const printBarGraph = true;
DavidEGrayson 10:e4dd36148539 289 while (1)
DavidEGrayson 10:e4dd36148539 290 {
DavidEGrayson 31:739b91331f31 291 if (clearStatsPacer.pace())
DavidEGrayson 31:739b91331f31 292 {
DavidEGrayson 31:739b91331f31 293 for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
DavidEGrayson 31:739b91331f31 294 {
DavidEGrayson 31:739b91331f31 295 min[i] = 0xFFFF;
DavidEGrayson 31:739b91331f31 296 max[i] = 0;
DavidEGrayson 31:739b91331f31 297 }
DavidEGrayson 31:739b91331f31 298 }
DavidEGrayson 31:739b91331f31 299
DavidEGrayson 31:739b91331f31 300 //values[0] = lineSensorsAnalog[0].read_u16();
DavidEGrayson 31:739b91331f31 301 //values[1] = lineSensorsAnalog[1].read_u16();
DavidEGrayson 31:739b91331f31 302 //values[2] = lineSensorsAnalog[2].read_u16();
DavidEGrayson 31:739b91331f31 303
DavidEGrayson 31:739b91331f31 304 uint16_t values[3];
DavidEGrayson 31:739b91331f31 305 readSensors(values);
DavidEGrayson 31:739b91331f31 306
DavidEGrayson 31:739b91331f31 307 for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
DavidEGrayson 31:739b91331f31 308 {
DavidEGrayson 31:739b91331f31 309 if (values[i] > max[i]){ max[i] = values[i]; }
DavidEGrayson 31:739b91331f31 310 if (values[i] < min[i]){ min[i] = values[i]; }
DavidEGrayson 31:739b91331f31 311 }
DavidEGrayson 31:739b91331f31 312
DavidEGrayson 10:e4dd36148539 313 if (reportPacer.pace())
DavidEGrayson 10:e4dd36148539 314 {
DavidEGrayson 10:e4dd36148539 315 if (printBarGraph)
DavidEGrayson 10:e4dd36148539 316 {
DavidEGrayson 10:e4dd36148539 317 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 31:739b91331f31 318 printBar("L", values[0]);
DavidEGrayson 31:739b91331f31 319 printBar("M", values[1]);
DavidEGrayson 31:739b91331f31 320 printBar("R", values[2]);
DavidEGrayson 31:739b91331f31 321 pc.printf("%4d %4d \r\n", min[0], max[0]);
DavidEGrayson 31:739b91331f31 322 pc.printf("%4d %4d \r\n", min[1], max[1]);
DavidEGrayson 31:739b91331f31 323 pc.printf("%4d %4d \r\n", min[2], max[2]);
DavidEGrayson 10:e4dd36148539 324 }
DavidEGrayson 10:e4dd36148539 325 else
DavidEGrayson 10:e4dd36148539 326 {
DavidEGrayson 31:739b91331f31 327 pc.printf("%8d %8d %8d\r\n", values[0], values[1], values[2]);
DavidEGrayson 10:e4dd36148539 328 }
DavidEGrayson 10:e4dd36148539 329 }
DavidEGrayson 10:e4dd36148539 330 }
DavidEGrayson 10:e4dd36148539 331 }
DavidEGrayson 8:78b1ff957cba 332
DavidEGrayson 24:fc01d9125d3b 333 // Values from David's office Values from dev lab,
DavidEGrayson 24:fc01d9125d3b 334 // in the day time, 2014-02-27: 2014-02-27:
DavidEGrayson 22:44c032e59ff5 335 // # calmin calmax
DavidEGrayson 24:fc01d9125d3b 336 // 0 34872 59726 0 40617 60222
DavidEGrayson 24:fc01d9125d3b 337 // 1 29335 60110 1 36937 61198
DavidEGrayson 24:fc01d9125d3b 338 // 2 23845 58446 2 33848 58862
DavidEGrayson 22:44c032e59ff5 339 void testCalibrate()
DavidEGrayson 22:44c032e59ff5 340 {
DavidEGrayson 22:44c032e59ff5 341 Timer timer;
DavidEGrayson 22:44c032e59ff5 342 timer.start();
DavidEGrayson 22:44c032e59ff5 343
DavidEGrayson 22:44c032e59ff5 344 Pacer reportPacer(200000);
DavidEGrayson 22:44c032e59ff5 345
DavidEGrayson 24:fc01d9125d3b 346 bool doneCalibrating = false;
DavidEGrayson 24:fc01d9125d3b 347
DavidEGrayson 24:fc01d9125d3b 348 led1 = 1;
DavidEGrayson 24:fc01d9125d3b 349
DavidEGrayson 22:44c032e59ff5 350 while(1)
DavidEGrayson 22:44c032e59ff5 351 {
DavidEGrayson 22:44c032e59ff5 352 lineTracker.read();
DavidEGrayson 24:fc01d9125d3b 353 if(!doneCalibrating)
DavidEGrayson 24:fc01d9125d3b 354 {
DavidEGrayson 24:fc01d9125d3b 355 lineTracker.updateCalibration();
DavidEGrayson 24:fc01d9125d3b 356 }
DavidEGrayson 24:fc01d9125d3b 357
DavidEGrayson 24:fc01d9125d3b 358 led3 = doneCalibrating;
DavidEGrayson 24:fc01d9125d3b 359 led4 = lineTracker.getLineVisible();
DavidEGrayson 24:fc01d9125d3b 360
DavidEGrayson 24:fc01d9125d3b 361 if (button1DefinitelyPressed())
DavidEGrayson 24:fc01d9125d3b 362 {
DavidEGrayson 24:fc01d9125d3b 363 doneCalibrating = true;
DavidEGrayson 24:fc01d9125d3b 364 }
DavidEGrayson 22:44c032e59ff5 365
DavidEGrayson 22:44c032e59ff5 366 if (reportPacer.pace())
DavidEGrayson 22:44c032e59ff5 367 {
DavidEGrayson 22:44c032e59ff5 368 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 22:44c032e59ff5 369 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 22:44c032e59ff5 370 {
DavidEGrayson 22:44c032e59ff5 371 pc.printf("%-2d %5d %5d %5d\r\n", s, lineTracker.calibratedMinimum[s], lineTracker.rawValues[s], lineTracker.calibratedMaximum[s]);
DavidEGrayson 22:44c032e59ff5 372 }
DavidEGrayson 22:44c032e59ff5 373 }
DavidEGrayson 22:44c032e59ff5 374 }
DavidEGrayson 22:44c032e59ff5 375 }
DavidEGrayson 22:44c032e59ff5 376
DavidEGrayson 9:9734347b5756 377 void testEncoders()
DavidEGrayson 9:9734347b5756 378 {
DavidEGrayson 9:9734347b5756 379 Pacer reportPacer(500000);
DavidEGrayson 10:e4dd36148539 380 led1 = 1;
DavidEGrayson 9:9734347b5756 381 while(1)
DavidEGrayson 9:9734347b5756 382 {
DavidEGrayson 9:9734347b5756 383 while(encoderBuffer.hasEvents())
DavidEGrayson 9:9734347b5756 384 {
DavidEGrayson 9:9734347b5756 385 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 9:9734347b5756 386 }
DavidEGrayson 9:9734347b5756 387
DavidEGrayson 9:9734347b5756 388 if(reportPacer.pace())
DavidEGrayson 9:9734347b5756 389 {
DavidEGrayson 9:9734347b5756 390 led2 = 1;
DavidEGrayson 31:739b91331f31 391 pc.printf("%8d %8d\r\n", encoderLeft.getCount(), encoderRight.getCount());
DavidEGrayson 9:9734347b5756 392 led2 = 0;
DavidEGrayson 10:e4dd36148539 393 }
DavidEGrayson 9:9734347b5756 394 }
DavidEGrayson 9:9734347b5756 395 }
DavidEGrayson 9:9734347b5756 396
DavidEGrayson 8:78b1ff957cba 397 void testMotors()
DavidEGrayson 8:78b1ff957cba 398 {
DavidEGrayson 8:78b1ff957cba 399 led1 = 1;
DavidEGrayson 8:78b1ff957cba 400 led2 = 0;
DavidEGrayson 8:78b1ff957cba 401 led3 = 0;
DavidEGrayson 8:78b1ff957cba 402 while(1)
DavidEGrayson 8:78b1ff957cba 403 {
DavidEGrayson 9:9734347b5756 404 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 405 led2 = 0;
DavidEGrayson 8:78b1ff957cba 406 led3 = 0;
DavidEGrayson 8:78b1ff957cba 407 wait(2);
DavidEGrayson 8:78b1ff957cba 408
DavidEGrayson 9:9734347b5756 409 motorsSpeedSet(300, 300);
DavidEGrayson 8:78b1ff957cba 410 wait(2);
DavidEGrayson 8:78b1ff957cba 411
DavidEGrayson 9:9734347b5756 412 motorsSpeedSet(-300, 300);
DavidEGrayson 8:78b1ff957cba 413 wait(2);
DavidEGrayson 8:78b1ff957cba 414
DavidEGrayson 9:9734347b5756 415 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 416 led2 = 1;
DavidEGrayson 8:78b1ff957cba 417 wait(2);
DavidEGrayson 8:78b1ff957cba 418
DavidEGrayson 9:9734347b5756 419 motorsSpeedSet(600, 600);
DavidEGrayson 39:a5e25fd52ff8 420 wait(1);
DavidEGrayson 8:78b1ff957cba 421
DavidEGrayson 9:9734347b5756 422 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 423 led3 = 1;
DavidEGrayson 8:78b1ff957cba 424 wait(2);
DavidEGrayson 8:78b1ff957cba 425
DavidEGrayson 9:9734347b5756 426 motorsSpeedSet(1200, 1200);
DavidEGrayson 39:a5e25fd52ff8 427 wait(1);
DavidEGrayson 8:78b1ff957cba 428 }
DavidEGrayson 10:e4dd36148539 429 }
DavidEGrayson 10:e4dd36148539 430
DavidEGrayson 20:dbec34f0e76b 431 void infiniteReckonerReportLoop()
DavidEGrayson 20:dbec34f0e76b 432 {
DavidEGrayson 20:dbec34f0e76b 433 Pacer reportPacer(200000);
DavidEGrayson 20:dbec34f0e76b 434 while(1)
DavidEGrayson 20:dbec34f0e76b 435 {
DavidEGrayson 33:58a0ab6e9ad2 436 showOrientationWithLeds34();
DavidEGrayson 20:dbec34f0e76b 437 if(reportPacer.pace())
DavidEGrayson 20:dbec34f0e76b 438 {
DavidEGrayson 20:dbec34f0e76b 439 pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
DavidEGrayson 20:dbec34f0e76b 440 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 20:dbec34f0e76b 441 determinant(), dotProduct());
DavidEGrayson 20:dbec34f0e76b 442 }
DavidEGrayson 20:dbec34f0e76b 443 }
DavidEGrayson 20:dbec34f0e76b 444
DavidEGrayson 20:dbec34f0e76b 445 }
DavidEGrayson 20:dbec34f0e76b 446
DavidEGrayson 31:739b91331f31 447 // with should be between 0 and 63
DavidEGrayson 31:739b91331f31 448 void printBar(const char * name, uint16_t result)
DavidEGrayson 10:e4dd36148539 449 {
DavidEGrayson 31:739b91331f31 450 pc.printf("%-2s %5d |", name, result);
DavidEGrayson 31:739b91331f31 451 uint16_t width = result >> 4;
DavidEGrayson 31:739b91331f31 452 if (width > 63) { width = 63; }
DavidEGrayson 10:e4dd36148539 453 uint8_t i;
DavidEGrayson 10:e4dd36148539 454 for(i = 0; i < width; i++){ pc.putc('#'); }
DavidEGrayson 10:e4dd36148539 455 for(; i < 63; i++){ pc.putc(' '); }
DavidEGrayson 10:e4dd36148539 456 pc.putc('|');
DavidEGrayson 31:739b91331f31 457 pc.putc('\r');
DavidEGrayson 10:e4dd36148539 458 pc.putc('\n');
DavidEGrayson 8:78b1ff957cba 459 }