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@32:83a13b06093c, 2014-03-04 (annotated)

Committer:
DavidEGrayson
Date:
Tue Mar 04 04:32:51 2014 +0000
Revision:
32:83a13b06093c
Parent:
31:739b91331f31
Child:
33:58a0ab6e9ad2
getting line sensors to work again;

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