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@30:84be2d602dc0, 2014-03-04 (annotated)

Committer:
DavidEGrayson
Date:
Tue Mar 04 02:09:54 2014 +0000
Revision:
30:84be2d602dc0
Parent:
29:cfcf08d8ac79
Child:
31:739b91331f31
Seems like the QTR-3RC could work, based on my tests on P10.

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 30:84be2d602dc0 19 uint16_t readPin18()
DavidEGrayson 30:84be2d602dc0 20 {
DavidEGrayson 30:84be2d602dc0 21 // Set PCADC bit in PCONP register. (Table 46).
DavidEGrayson 30:84be2d602dc0 22 LPC_SC->PCONP |= (1 << 12);
DavidEGrayson 30:84be2d602dc0 23
DavidEGrayson 30:84be2d602dc0 24 // Select PCLK_ADC in PCLKSEL0 register. (Table 40, Table 531).
DavidEGrayson 30:84be2d602dc0 25 LPC_SC->PCLKSEL0 |= (3 << 24); // PCLK for ADC = CCLK/8
DavidEGrayson 30:84be2d602dc0 26
DavidEGrayson 30:84be2d602dc0 27 // Enable ADC0 through PINSEL registers. (Section 8.5).
DavidEGrayson 30:84be2d602dc0 28 LPC_PINCON->PINSEL1 = (LPC_PINCON->PINSEL1 & ~(3 << 20)) | (1 << 20);
DavidEGrayson 30:84be2d602dc0 29
DavidEGrayson 30:84be2d602dc0 30 // Pin 18: P0.26/AD0.3/AOUT/RXD3
DavidEGrayson 30:84be2d602dc0 31
DavidEGrayson 30:84be2d602dc0 32 // 7:0 Bitmap to select what channel to use
DavidEGrayson 30:84be2d602dc0 33 // 15:8 Select clock.
DavidEGrayson 30:84be2d602dc0 34 // 16 BURST = 0
DavidEGrayson 30:84be2d602dc0 35 // 20:17 Reserved
DavidEGrayson 30:84be2d602dc0 36 // 21 PDN = 1, A/D converter is operational
DavidEGrayson 30:84be2d602dc0 37 // 26:24 START = 001 to start conversion now
DavidEGrayson 30:84be2d602dc0 38 LPC_ADC->ADCR = (1 << 3) | (0xFF << 8) | (1 << 21);
DavidEGrayson 30:84be2d602dc0 39 LPC_ADC->ADCR |= (1 << 24);
DavidEGrayson 30:84be2d602dc0 40
DavidEGrayson 30:84be2d602dc0 41 while(!(LPC_ADC->ADGDR >> 31 & 1)) // while not done
DavidEGrayson 30:84be2d602dc0 42 {
DavidEGrayson 30:84be2d602dc0 43 }
DavidEGrayson 30:84be2d602dc0 44 //return 2;
DavidEGrayson 30:84be2d602dc0 45 return LPC_ADC->ADGDR & 0xFFFF;
DavidEGrayson 30:84be2d602dc0 46 }
DavidEGrayson 30:84be2d602dc0 47
DavidEGrayson 30:84be2d602dc0 48 uint16_t readP10()
DavidEGrayson 30:84be2d602dc0 49 {
DavidEGrayson 30:84be2d602dc0 50 DigitalInOut pin(p10);
DavidEGrayson 30:84be2d602dc0 51 pin.output();
DavidEGrayson 30:84be2d602dc0 52 pin = 1;
DavidEGrayson 30:84be2d602dc0 53 wait_us(20);
DavidEGrayson 30:84be2d602dc0 54 uint16_t value = 0x8000;
DavidEGrayson 30:84be2d602dc0 55 Timer timer;
DavidEGrayson 30:84be2d602dc0 56 timer.start();
DavidEGrayson 30:84be2d602dc0 57 pin.input();
DavidEGrayson 30:84be2d602dc0 58 while(timer.read_us() < 0x8000)
DavidEGrayson 30:84be2d602dc0 59 {
DavidEGrayson 30:84be2d602dc0 60 if(pin.read() == 0)
DavidEGrayson 30:84be2d602dc0 61 {
DavidEGrayson 30:84be2d602dc0 62 return timer.read_us();
DavidEGrayson 30:84be2d602dc0 63 }
DavidEGrayson 30:84be2d602dc0 64 }
DavidEGrayson 30:84be2d602dc0 65 return value;
DavidEGrayson 30:84be2d602dc0 66 }
DavidEGrayson 30:84be2d602dc0 67
DavidEGrayson 30:84be2d602dc0 68 void testSensorGlitches()
DavidEGrayson 29:cfcf08d8ac79 69 {
DavidEGrayson 29:cfcf08d8ac79 70 AnalogIn testInput(p18);
DavidEGrayson 29:cfcf08d8ac79 71 Pacer reportPacer(1000000);
DavidEGrayson 29:cfcf08d8ac79 72 uint32_t badCount = 0, goodCount = 0;
DavidEGrayson 30:84be2d602dc0 73 pc.printf("hi\r\n");
DavidEGrayson 30:84be2d602dc0 74
DavidEGrayson 30:84be2d602dc0 75 //uint16_t riseCount = 0;
DavidEGrayson 30:84be2d602dc0 76 uint16_t reading = 0xFF;
DavidEGrayson 30:84be2d602dc0 77
DavidEGrayson 29:cfcf08d8ac79 78 while(1)
DavidEGrayson 29:cfcf08d8ac79 79 {
DavidEGrayson 30:84be2d602dc0 80 /** This digital filtering did not work
DavidEGrayson 30:84be2d602dc0 81 {
DavidEGrayson 30:84be2d602dc0 82 wait(0.01);
DavidEGrayson 30:84be2d602dc0 83 uint16_t raw = testInput.read_u16();
DavidEGrayson 30:84be2d602dc0 84 if (raw < reading)
DavidEGrayson 30:84be2d602dc0 85 {
DavidEGrayson 30:84be2d602dc0 86 riseCount = 0;
DavidEGrayson 30:84be2d602dc0 87 reading = raw;
DavidEGrayson 30:84be2d602dc0 88 }
DavidEGrayson 30:84be2d602dc0 89 else
DavidEGrayson 30:84be2d602dc0 90 {
DavidEGrayson 30:84be2d602dc0 91 riseCount++;
DavidEGrayson 30:84be2d602dc0 92 if (riseCount == 10)
DavidEGrayson 30:84be2d602dc0 93 {
DavidEGrayson 30:84be2d602dc0 94 riseCount = 0;
DavidEGrayson 30:84be2d602dc0 95 reading = raw;
DavidEGrayson 30:84be2d602dc0 96 }
DavidEGrayson 30:84be2d602dc0 97 }
DavidEGrayson 30:84be2d602dc0 98 }
DavidEGrayson 30:84be2d602dc0 99 **/
DavidEGrayson 30:84be2d602dc0 100 reading = readP10();
DavidEGrayson 30:84be2d602dc0 101
DavidEGrayson 29:cfcf08d8ac79 102 if(reading > 100)
DavidEGrayson 29:cfcf08d8ac79 103 {
DavidEGrayson 29:cfcf08d8ac79 104 badCount += 1;
DavidEGrayson 30:84be2d602dc0 105 //pc.printf("f %5d %11d %11d\r\n", reading, badCount, goodCount);
DavidEGrayson 29:cfcf08d8ac79 106 }
DavidEGrayson 29:cfcf08d8ac79 107 else
DavidEGrayson 29:cfcf08d8ac79 108 {
DavidEGrayson 30:84be2d602dc0 109 goodCount += 1;
DavidEGrayson 29:cfcf08d8ac79 110 }
DavidEGrayson 29:cfcf08d8ac79 111
DavidEGrayson 29:cfcf08d8ac79 112 if (reportPacer.pace())
DavidEGrayson 29:cfcf08d8ac79 113 {
DavidEGrayson 30:84be2d602dc0 114 pc.printf("h %5d %11d %11d\r\n", reading, badCount, goodCount);
DavidEGrayson 29:cfcf08d8ac79 115 }
DavidEGrayson 29:cfcf08d8ac79 116 }
DavidEGrayson 29:cfcf08d8ac79 117 }
DavidEGrayson 29:cfcf08d8ac79 118
DavidEGrayson 28:4374035df5e0 119 void testAnalog()
DavidEGrayson 28:4374035df5e0 120 {
DavidEGrayson 28:4374035df5e0 121 AnalogIn testInput(p18);
DavidEGrayson 28:4374035df5e0 122
DavidEGrayson 29:cfcf08d8ac79 123 DigitalOut pin20(p20);
DavidEGrayson 29:cfcf08d8ac79 124 DigitalOut pin19(p19);
DavidEGrayson 29:cfcf08d8ac79 125 //DigitalOut pin18(p18);
DavidEGrayson 29:cfcf08d8ac79 126 DigitalOut pin17(p17);
DavidEGrayson 29:cfcf08d8ac79 127 DigitalOut pin16(p16);
DavidEGrayson 29:cfcf08d8ac79 128 DigitalOut pin15(p15);
DavidEGrayson 29:cfcf08d8ac79 129
DavidEGrayson 29:cfcf08d8ac79 130 pin20 = 0;
DavidEGrayson 29:cfcf08d8ac79 131 pin19 = 0;
DavidEGrayson 29:cfcf08d8ac79 132 //pin18 = 0;
DavidEGrayson 29:cfcf08d8ac79 133 pin17 = 0;
DavidEGrayson 29:cfcf08d8ac79 134 pin16 = 0;
DavidEGrayson 29:cfcf08d8ac79 135 pin15 = 0;
DavidEGrayson 29:cfcf08d8ac79 136
DavidEGrayson 28:4374035df5e0 137 uint32_t badCount = 0, goodCount = 0;
DavidEGrayson 28:4374035df5e0 138
DavidEGrayson 28:4374035df5e0 139 Pacer reportPacer(1000000);
DavidEGrayson 28:4374035df5e0 140 while(1)
DavidEGrayson 28:4374035df5e0 141 {
DavidEGrayson 28:4374035df5e0 142 uint16_t reading = testInput.read_u16();
DavidEGrayson 28:4374035df5e0 143 if(reading > 100)
DavidEGrayson 28:4374035df5e0 144 {
DavidEGrayson 28:4374035df5e0 145 badCount += 1;
DavidEGrayson 28:4374035df5e0 146 pc.printf("%5d %11d %11d\r\n", reading, badCount, goodCount);
DavidEGrayson 28:4374035df5e0 147 }
DavidEGrayson 28:4374035df5e0 148 else
DavidEGrayson 28:4374035df5e0 149 {
DavidEGrayson 28:4374035df5e0 150 goodCount += 1;
DavidEGrayson 28:4374035df5e0 151 }
DavidEGrayson 28:4374035df5e0 152
DavidEGrayson 28:4374035df5e0 153 if (reportPacer.pace())
DavidEGrayson 28:4374035df5e0 154 {
DavidEGrayson 28:4374035df5e0 155 pc.printf("Hello\r\n");
DavidEGrayson 28:4374035df5e0 156 }
DavidEGrayson 28:4374035df5e0 157 }
DavidEGrayson 28:4374035df5e0 158 }
DavidEGrayson 28:4374035df5e0 159
DavidEGrayson 28:4374035df5e0 160 // This also tests the LineTracker by printing out a lot of data from it.
DavidEGrayson 28:4374035df5e0 161 void testLineFollowing()
DavidEGrayson 28:4374035df5e0 162 {
DavidEGrayson 28:4374035df5e0 163 led1 = 1;
DavidEGrayson 28:4374035df5e0 164 while(!button1DefinitelyPressed())
DavidEGrayson 28:4374035df5e0 165 {
DavidEGrayson 28:4374035df5e0 166 updateReckonerFromEncoders();
DavidEGrayson 28:4374035df5e0 167 }
DavidEGrayson 28:4374035df5e0 168 led2 = 1;
DavidEGrayson 28:4374035df5e0 169
DavidEGrayson 28:4374035df5e0 170 Pacer reportPacer(200000);
DavidEGrayson 28:4374035df5e0 171
DavidEGrayson 28:4374035df5e0 172 loadCalibration();
DavidEGrayson 28:4374035df5e0 173 uint16_t loopCount = 0;
DavidEGrayson 28:4374035df5e0 174 while(1)
DavidEGrayson 28:4374035df5e0 175 {
DavidEGrayson 28:4374035df5e0 176 updateReckonerFromEncoders();
DavidEGrayson 28:4374035df5e0 177 bool lineVisiblePrevious = lineTracker.getLineVisible();
DavidEGrayson 28:4374035df5e0 178 lineTracker.read();
DavidEGrayson 28:4374035df5e0 179 updateMotorsToFollowLine();
DavidEGrayson 28:4374035df5e0 180
DavidEGrayson 28:4374035df5e0 181 loopCount += 1;
DavidEGrayson 28:4374035df5e0 182
DavidEGrayson 28:4374035df5e0 183 if (lineVisiblePrevious != lineTracker.getLineVisible())
DavidEGrayson 28:4374035df5e0 184 {
DavidEGrayson 28:4374035df5e0 185 pc.printf("%5d ! %1d %4d | %4d %4d %4d | %5d %5d %5d\r\n",
DavidEGrayson 28:4374035df5e0 186 loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
DavidEGrayson 28:4374035df5e0 187 lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2],
DavidEGrayson 28:4374035df5e0 188 lineTracker.rawValues[0], lineTracker.rawValues[1], lineTracker.rawValues[2]
DavidEGrayson 28:4374035df5e0 189 );
DavidEGrayson 28:4374035df5e0 190 }
DavidEGrayson 28:4374035df5e0 191
DavidEGrayson 28:4374035df5e0 192 if (reportPacer.pace())
DavidEGrayson 28:4374035df5e0 193 {
DavidEGrayson 28:4374035df5e0 194 pc.printf("%5d %1d %4d | %4d %4d %4d\r\n", loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
DavidEGrayson 28:4374035df5e0 195 lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
DavidEGrayson 28:4374035df5e0 196 );
DavidEGrayson 28:4374035df5e0 197 }
DavidEGrayson 28:4374035df5e0 198 }
DavidEGrayson 28:4374035df5e0 199 }
DavidEGrayson 28:4374035df5e0 200
DavidEGrayson 17:2df9861f53ee 201 void testDriveHome()
DavidEGrayson 16:8eaa5bc2bdb1 202 {
DavidEGrayson 17:2df9861f53ee 203 led1 = 1;
DavidEGrayson 18:b65fbb795396 204 while(!button1DefinitelyPressed())
DavidEGrayson 17:2df9861f53ee 205 {
DavidEGrayson 17:2df9861f53ee 206 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 207 }
DavidEGrayson 21:c279c6a83671 208 driveHomeAlmost();
DavidEGrayson 21:c279c6a83671 209 finalSettleIn();
DavidEGrayson 20:dbec34f0e76b 210 infiniteReckonerReportLoop();
DavidEGrayson 20:dbec34f0e76b 211 }
DavidEGrayson 20:dbec34f0e76b 212
DavidEGrayson 20:dbec34f0e76b 213 void testFinalSettleIn()
DavidEGrayson 20:dbec34f0e76b 214 {
DavidEGrayson 20:dbec34f0e76b 215 led1 = 1;
DavidEGrayson 20:dbec34f0e76b 216 while(!button1DefinitelyPressed())
DavidEGrayson 20:dbec34f0e76b 217 {
DavidEGrayson 20:dbec34f0e76b 218 updateReckonerFromEncoders();
DavidEGrayson 20:dbec34f0e76b 219 }
DavidEGrayson 20:dbec34f0e76b 220 finalSettleIn();
DavidEGrayson 20:dbec34f0e76b 221 infiniteReckonerReportLoop();
DavidEGrayson 16:8eaa5bc2bdb1 222 }
DavidEGrayson 16:8eaa5bc2bdb1 223
DavidEGrayson 17:2df9861f53ee 224
DavidEGrayson 16:8eaa5bc2bdb1 225 void testButtons()
DavidEGrayson 16:8eaa5bc2bdb1 226 {
DavidEGrayson 16:8eaa5bc2bdb1 227 led1 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 228
DavidEGrayson 17:2df9861f53ee 229 while(!button1DefinitelyReleased());
DavidEGrayson 17:2df9861f53ee 230 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 231 led2 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 232
DavidEGrayson 16:8eaa5bc2bdb1 233 while(!button1DefinitelyReleased());
DavidEGrayson 16:8eaa5bc2bdb1 234 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 235 led3 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 236
DavidEGrayson 16:8eaa5bc2bdb1 237 while(!button1DefinitelyReleased());
DavidEGrayson 16:8eaa5bc2bdb1 238 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 239 led4 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 240
DavidEGrayson 16:8eaa5bc2bdb1 241 while(1){};
DavidEGrayson 16:8eaa5bc2bdb1 242 }
DavidEGrayson 16:8eaa5bc2bdb1 243
DavidEGrayson 12:835a4d24ae3b 244 void testReckoner()
DavidEGrayson 12:835a4d24ae3b 245 {
DavidEGrayson 13:bba5b3abd13f 246 Pacer reportPacer(100000);
DavidEGrayson 12:835a4d24ae3b 247 while(1)
DavidEGrayson 12:835a4d24ae3b 248 {
DavidEGrayson 12:835a4d24ae3b 249 updateReckonerFromEncoders();
DavidEGrayson 13:bba5b3abd13f 250 led1 = (reckoner.cos > 0);
DavidEGrayson 13:bba5b3abd13f 251 led2 = (reckoner.sin > 0);
DavidEGrayson 13:bba5b3abd13f 252 led3 = (reckoner.x > 0);
DavidEGrayson 13:bba5b3abd13f 253 led4 = (reckoner.y > 0);
DavidEGrayson 12:835a4d24ae3b 254
DavidEGrayson 13:bba5b3abd13f 255 if (reportPacer.pace())
DavidEGrayson 13:bba5b3abd13f 256 {
DavidEGrayson 19:a11ffc903774 257 pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n",
DavidEGrayson 13:bba5b3abd13f 258 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 19:a11ffc903774 259 encoderLeft.getCount(), encoderRight.getCount(), determinant());
DavidEGrayson 13:bba5b3abd13f 260 }
DavidEGrayson 12:835a4d24ae3b 261 }
DavidEGrayson 12:835a4d24ae3b 262 }
DavidEGrayson 12:835a4d24ae3b 263
DavidEGrayson 10:e4dd36148539 264 void testLineSensors()
DavidEGrayson 10:e4dd36148539 265 {
DavidEGrayson 10:e4dd36148539 266 led1 = 1;
DavidEGrayson 10:e4dd36148539 267 Pacer reportPacer(100000);
DavidEGrayson 10:e4dd36148539 268 bool const printBarGraph = true;
DavidEGrayson 10:e4dd36148539 269 while (1)
DavidEGrayson 10:e4dd36148539 270 {
DavidEGrayson 10:e4dd36148539 271 if (reportPacer.pace())
DavidEGrayson 10:e4dd36148539 272 {
DavidEGrayson 10:e4dd36148539 273 uint16_t left = lineSensorsAnalog[0].read_u16();
DavidEGrayson 10:e4dd36148539 274 uint16_t middle = lineSensorsAnalog[1].read_u16();
DavidEGrayson 10:e4dd36148539 275 uint16_t right = lineSensorsAnalog[2].read_u16();
DavidEGrayson 10:e4dd36148539 276
DavidEGrayson 10:e4dd36148539 277 if (printBarGraph)
DavidEGrayson 10:e4dd36148539 278 {
DavidEGrayson 10:e4dd36148539 279 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 10:e4dd36148539 280 printBar("L", left);
DavidEGrayson 10:e4dd36148539 281 printBar("M", middle);
DavidEGrayson 10:e4dd36148539 282 printBar("R", right);
DavidEGrayson 10:e4dd36148539 283 }
DavidEGrayson 10:e4dd36148539 284 else
DavidEGrayson 10:e4dd36148539 285 {
DavidEGrayson 10:e4dd36148539 286 pc.printf("%8d %8d %8d\n", left, middle, right);
DavidEGrayson 10:e4dd36148539 287 }
DavidEGrayson 10:e4dd36148539 288 }
DavidEGrayson 10:e4dd36148539 289 }
DavidEGrayson 10:e4dd36148539 290 }
DavidEGrayson 8:78b1ff957cba 291
DavidEGrayson 24:fc01d9125d3b 292 // Values from David's office Values from dev lab,
DavidEGrayson 24:fc01d9125d3b 293 // in the day time, 2014-02-27: 2014-02-27:
DavidEGrayson 22:44c032e59ff5 294 // # calmin calmax
DavidEGrayson 24:fc01d9125d3b 295 // 0 34872 59726 0 40617 60222
DavidEGrayson 24:fc01d9125d3b 296 // 1 29335 60110 1 36937 61198
DavidEGrayson 24:fc01d9125d3b 297 // 2 23845 58446 2 33848 58862
DavidEGrayson 22:44c032e59ff5 298 void testCalibrate()
DavidEGrayson 22:44c032e59ff5 299 {
DavidEGrayson 22:44c032e59ff5 300 Timer timer;
DavidEGrayson 22:44c032e59ff5 301 timer.start();
DavidEGrayson 22:44c032e59ff5 302
DavidEGrayson 22:44c032e59ff5 303 Pacer reportPacer(200000);
DavidEGrayson 22:44c032e59ff5 304
DavidEGrayson 24:fc01d9125d3b 305 bool doneCalibrating = false;
DavidEGrayson 24:fc01d9125d3b 306
DavidEGrayson 24:fc01d9125d3b 307 led1 = 1;
DavidEGrayson 24:fc01d9125d3b 308
DavidEGrayson 22:44c032e59ff5 309 while(1)
DavidEGrayson 22:44c032e59ff5 310 {
DavidEGrayson 22:44c032e59ff5 311 lineTracker.read();
DavidEGrayson 24:fc01d9125d3b 312 if(!doneCalibrating)
DavidEGrayson 24:fc01d9125d3b 313 {
DavidEGrayson 24:fc01d9125d3b 314 lineTracker.updateCalibration();
DavidEGrayson 24:fc01d9125d3b 315 }
DavidEGrayson 24:fc01d9125d3b 316
DavidEGrayson 24:fc01d9125d3b 317 led2 = calibrationLooksGood();
DavidEGrayson 24:fc01d9125d3b 318 led3 = doneCalibrating;
DavidEGrayson 24:fc01d9125d3b 319 led4 = lineTracker.getLineVisible();
DavidEGrayson 24:fc01d9125d3b 320
DavidEGrayson 24:fc01d9125d3b 321 if (button1DefinitelyPressed())
DavidEGrayson 24:fc01d9125d3b 322 {
DavidEGrayson 24:fc01d9125d3b 323 doneCalibrating = true;
DavidEGrayson 24:fc01d9125d3b 324 }
DavidEGrayson 22:44c032e59ff5 325
DavidEGrayson 22:44c032e59ff5 326 if (reportPacer.pace())
DavidEGrayson 22:44c032e59ff5 327 {
DavidEGrayson 22:44c032e59ff5 328 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 22:44c032e59ff5 329 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 22:44c032e59ff5 330 {
DavidEGrayson 22:44c032e59ff5 331 pc.printf("%-2d %5d %5d %5d\r\n", s, lineTracker.calibratedMinimum[s], lineTracker.rawValues[s], lineTracker.calibratedMaximum[s]);
DavidEGrayson 22:44c032e59ff5 332 }
DavidEGrayson 22:44c032e59ff5 333 if (calibrationLooksGood())
DavidEGrayson 22:44c032e59ff5 334 {
DavidEGrayson 22:44c032e59ff5 335 pc.puts("Good. \r\n");
DavidEGrayson 22:44c032e59ff5 336 }
DavidEGrayson 22:44c032e59ff5 337 else
DavidEGrayson 22:44c032e59ff5 338 {
DavidEGrayson 22:44c032e59ff5 339 pc.puts("Not good yet.\r\n");
DavidEGrayson 22:44c032e59ff5 340 }
DavidEGrayson 22:44c032e59ff5 341 }
DavidEGrayson 22:44c032e59ff5 342 }
DavidEGrayson 22:44c032e59ff5 343 }
DavidEGrayson 22:44c032e59ff5 344
DavidEGrayson 9:9734347b5756 345 void testEncoders()
DavidEGrayson 9:9734347b5756 346 {
DavidEGrayson 9:9734347b5756 347 Pacer reportPacer(500000);
DavidEGrayson 10:e4dd36148539 348 led1 = 1;
DavidEGrayson 9:9734347b5756 349 while(1)
DavidEGrayson 9:9734347b5756 350 {
DavidEGrayson 9:9734347b5756 351 while(encoderBuffer.hasEvents())
DavidEGrayson 9:9734347b5756 352 {
DavidEGrayson 9:9734347b5756 353 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 9:9734347b5756 354 }
DavidEGrayson 9:9734347b5756 355
DavidEGrayson 9:9734347b5756 356 if(reportPacer.pace())
DavidEGrayson 9:9734347b5756 357 {
DavidEGrayson 9:9734347b5756 358 led2 = 1;
DavidEGrayson 9:9734347b5756 359 pc.printf("%8d %8d\n", encoderLeft.getCount(), encoderRight.getCount());
DavidEGrayson 9:9734347b5756 360 led2 = 0;
DavidEGrayson 10:e4dd36148539 361 }
DavidEGrayson 9:9734347b5756 362 }
DavidEGrayson 9:9734347b5756 363 }
DavidEGrayson 9:9734347b5756 364
DavidEGrayson 8:78b1ff957cba 365 void testMotors()
DavidEGrayson 8:78b1ff957cba 366 {
DavidEGrayson 8:78b1ff957cba 367 led1 = 1;
DavidEGrayson 8:78b1ff957cba 368 led2 = 0;
DavidEGrayson 8:78b1ff957cba 369 led3 = 0;
DavidEGrayson 8:78b1ff957cba 370 while(1)
DavidEGrayson 8:78b1ff957cba 371 {
DavidEGrayson 9:9734347b5756 372 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 373 led2 = 0;
DavidEGrayson 8:78b1ff957cba 374 led3 = 0;
DavidEGrayson 8:78b1ff957cba 375 wait(2);
DavidEGrayson 8:78b1ff957cba 376
DavidEGrayson 9:9734347b5756 377 motorsSpeedSet(300, 300);
DavidEGrayson 8:78b1ff957cba 378 wait(2);
DavidEGrayson 8:78b1ff957cba 379
DavidEGrayson 9:9734347b5756 380 motorsSpeedSet(-300, 300);
DavidEGrayson 8:78b1ff957cba 381 wait(2);
DavidEGrayson 8:78b1ff957cba 382
DavidEGrayson 9:9734347b5756 383 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 384 led2 = 1;
DavidEGrayson 8:78b1ff957cba 385 wait(2);
DavidEGrayson 8:78b1ff957cba 386
DavidEGrayson 9:9734347b5756 387 motorsSpeedSet(600, 600);
DavidEGrayson 8:78b1ff957cba 388 wait(2);
DavidEGrayson 8:78b1ff957cba 389
DavidEGrayson 9:9734347b5756 390 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 391 led3 = 1;
DavidEGrayson 8:78b1ff957cba 392 wait(2);
DavidEGrayson 8:78b1ff957cba 393
DavidEGrayson 9:9734347b5756 394 motorsSpeedSet(1200, 1200);
DavidEGrayson 8:78b1ff957cba 395 wait(2);
DavidEGrayson 8:78b1ff957cba 396 }
DavidEGrayson 10:e4dd36148539 397 }
DavidEGrayson 10:e4dd36148539 398
DavidEGrayson 20:dbec34f0e76b 399 void infiniteReckonerReportLoop()
DavidEGrayson 20:dbec34f0e76b 400 {
DavidEGrayson 20:dbec34f0e76b 401 Pacer reportPacer(200000);
DavidEGrayson 20:dbec34f0e76b 402 while(1)
DavidEGrayson 20:dbec34f0e76b 403 {
DavidEGrayson 20:dbec34f0e76b 404 if(reportPacer.pace())
DavidEGrayson 20:dbec34f0e76b 405 {
DavidEGrayson 20:dbec34f0e76b 406 led4 = 1;
DavidEGrayson 20:dbec34f0e76b 407 pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
DavidEGrayson 20:dbec34f0e76b 408 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 20:dbec34f0e76b 409 determinant(), dotProduct());
DavidEGrayson 20:dbec34f0e76b 410 led4 = 0;
DavidEGrayson 20:dbec34f0e76b 411 }
DavidEGrayson 20:dbec34f0e76b 412 }
DavidEGrayson 20:dbec34f0e76b 413
DavidEGrayson 20:dbec34f0e76b 414 }
DavidEGrayson 20:dbec34f0e76b 415
DavidEGrayson 10:e4dd36148539 416 void printBar(const char * name, uint16_t adcResult)
DavidEGrayson 10:e4dd36148539 417 {
DavidEGrayson 10:e4dd36148539 418 pc.printf("%-2s %5d |", name, adcResult);
DavidEGrayson 10:e4dd36148539 419 uint8_t width = adcResult >> 10;
DavidEGrayson 10:e4dd36148539 420 uint8_t i;
DavidEGrayson 10:e4dd36148539 421 for(i = 0; i < width; i++){ pc.putc('#'); }
DavidEGrayson 10:e4dd36148539 422 for(; i < 63; i++){ pc.putc(' '); }
DavidEGrayson 10:e4dd36148539 423 pc.putc('|');
DavidEGrayson 10:e4dd36148539 424 pc.putc('\n');
DavidEGrayson 8:78b1ff957cba 425 }