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@31:739b91331f31, 2014-03-04 (annotated)

Committer:
DavidEGrayson
Date:
Tue Mar 04 03:04:00 2014 +0000
Revision:
31:739b91331f31
Parent:
30:84be2d602dc0
Child:
32:83a13b06093c
testLineSensors seems to work fine with QTR-3RC.

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