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@28:4374035df5e0, 2014-03-01 (annotated)

Committer:
DavidEGrayson
Date:
Sat Mar 01 03:13:57 2014 +0000
Revision:
28:4374035df5e0
Parent:
24:fc01d9125d3b
Child:
29:cfcf08d8ac79
Discovered that 4 out of the 6 analog inputs on the mbed are severely messed up.  Might need to get a new mbed or do digital filtering (a median of three readings out to work).

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 28:4374035df5e0 19 void testAnalog()
DavidEGrayson 28:4374035df5e0 20 {
DavidEGrayson 28:4374035df5e0 21 AnalogIn testInput(p18);
DavidEGrayson 28:4374035df5e0 22
DavidEGrayson 28:4374035df5e0 23 uint32_t badCount = 0, goodCount = 0;
DavidEGrayson 28:4374035df5e0 24
DavidEGrayson 28:4374035df5e0 25 Pacer reportPacer(1000000);
DavidEGrayson 28:4374035df5e0 26 while(1)
DavidEGrayson 28:4374035df5e0 27 {
DavidEGrayson 28:4374035df5e0 28 uint16_t reading = testInput.read_u16();
DavidEGrayson 28:4374035df5e0 29 if(reading > 100)
DavidEGrayson 28:4374035df5e0 30 {
DavidEGrayson 28:4374035df5e0 31 badCount += 1;
DavidEGrayson 28:4374035df5e0 32 pc.printf("%5d %11d %11d\r\n", reading, badCount, goodCount);
DavidEGrayson 28:4374035df5e0 33 }
DavidEGrayson 28:4374035df5e0 34 else
DavidEGrayson 28:4374035df5e0 35 {
DavidEGrayson 28:4374035df5e0 36 goodCount += 1;
DavidEGrayson 28:4374035df5e0 37 }
DavidEGrayson 28:4374035df5e0 38
DavidEGrayson 28:4374035df5e0 39 if (reportPacer.pace())
DavidEGrayson 28:4374035df5e0 40 {
DavidEGrayson 28:4374035df5e0 41 pc.printf("Hello\r\n");
DavidEGrayson 28:4374035df5e0 42 }
DavidEGrayson 28:4374035df5e0 43 }
DavidEGrayson 28:4374035df5e0 44 }
DavidEGrayson 28:4374035df5e0 45
DavidEGrayson 28:4374035df5e0 46 // This also tests the LineTracker by printing out a lot of data from it.
DavidEGrayson 28:4374035df5e0 47 void testLineFollowing()
DavidEGrayson 28:4374035df5e0 48 {
DavidEGrayson 28:4374035df5e0 49 led1 = 1;
DavidEGrayson 28:4374035df5e0 50 while(!button1DefinitelyPressed())
DavidEGrayson 28:4374035df5e0 51 {
DavidEGrayson 28:4374035df5e0 52 updateReckonerFromEncoders();
DavidEGrayson 28:4374035df5e0 53 }
DavidEGrayson 28:4374035df5e0 54 led2 = 1;
DavidEGrayson 28:4374035df5e0 55
DavidEGrayson 28:4374035df5e0 56 Pacer reportPacer(200000);
DavidEGrayson 28:4374035df5e0 57
DavidEGrayson 28:4374035df5e0 58 loadCalibration();
DavidEGrayson 28:4374035df5e0 59 uint16_t loopCount = 0;
DavidEGrayson 28:4374035df5e0 60 while(1)
DavidEGrayson 28:4374035df5e0 61 {
DavidEGrayson 28:4374035df5e0 62 updateReckonerFromEncoders();
DavidEGrayson 28:4374035df5e0 63 bool lineVisiblePrevious = lineTracker.getLineVisible();
DavidEGrayson 28:4374035df5e0 64 lineTracker.read();
DavidEGrayson 28:4374035df5e0 65 updateMotorsToFollowLine();
DavidEGrayson 28:4374035df5e0 66
DavidEGrayson 28:4374035df5e0 67 loopCount += 1;
DavidEGrayson 28:4374035df5e0 68
DavidEGrayson 28:4374035df5e0 69 if (lineVisiblePrevious != lineTracker.getLineVisible())
DavidEGrayson 28:4374035df5e0 70 {
DavidEGrayson 28:4374035df5e0 71 pc.printf("%5d ! %1d %4d | %4d %4d %4d | %5d %5d %5d\r\n",
DavidEGrayson 28:4374035df5e0 72 loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
DavidEGrayson 28:4374035df5e0 73 lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2],
DavidEGrayson 28:4374035df5e0 74 lineTracker.rawValues[0], lineTracker.rawValues[1], lineTracker.rawValues[2]
DavidEGrayson 28:4374035df5e0 75 );
DavidEGrayson 28:4374035df5e0 76 }
DavidEGrayson 28:4374035df5e0 77
DavidEGrayson 28:4374035df5e0 78 if (reportPacer.pace())
DavidEGrayson 28:4374035df5e0 79 {
DavidEGrayson 28:4374035df5e0 80 pc.printf("%5d %1d %4d | %4d %4d %4d\r\n", loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
DavidEGrayson 28:4374035df5e0 81 lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
DavidEGrayson 28:4374035df5e0 82 );
DavidEGrayson 28:4374035df5e0 83 }
DavidEGrayson 28:4374035df5e0 84 }
DavidEGrayson 28:4374035df5e0 85 }
DavidEGrayson 28:4374035df5e0 86
DavidEGrayson 17:2df9861f53ee 87 void testDriveHome()
DavidEGrayson 16:8eaa5bc2bdb1 88 {
DavidEGrayson 17:2df9861f53ee 89 led1 = 1;
DavidEGrayson 18:b65fbb795396 90 while(!button1DefinitelyPressed())
DavidEGrayson 17:2df9861f53ee 91 {
DavidEGrayson 17:2df9861f53ee 92 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 93 }
DavidEGrayson 21:c279c6a83671 94 driveHomeAlmost();
DavidEGrayson 21:c279c6a83671 95 finalSettleIn();
DavidEGrayson 20:dbec34f0e76b 96 infiniteReckonerReportLoop();
DavidEGrayson 20:dbec34f0e76b 97 }
DavidEGrayson 20:dbec34f0e76b 98
DavidEGrayson 20:dbec34f0e76b 99 void testFinalSettleIn()
DavidEGrayson 20:dbec34f0e76b 100 {
DavidEGrayson 20:dbec34f0e76b 101 led1 = 1;
DavidEGrayson 20:dbec34f0e76b 102 while(!button1DefinitelyPressed())
DavidEGrayson 20:dbec34f0e76b 103 {
DavidEGrayson 20:dbec34f0e76b 104 updateReckonerFromEncoders();
DavidEGrayson 20:dbec34f0e76b 105 }
DavidEGrayson 20:dbec34f0e76b 106 finalSettleIn();
DavidEGrayson 20:dbec34f0e76b 107 infiniteReckonerReportLoop();
DavidEGrayson 16:8eaa5bc2bdb1 108 }
DavidEGrayson 16:8eaa5bc2bdb1 109
DavidEGrayson 17:2df9861f53ee 110
DavidEGrayson 16:8eaa5bc2bdb1 111 void testButtons()
DavidEGrayson 16:8eaa5bc2bdb1 112 {
DavidEGrayson 16:8eaa5bc2bdb1 113 led1 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 114
DavidEGrayson 17:2df9861f53ee 115 while(!button1DefinitelyReleased());
DavidEGrayson 17:2df9861f53ee 116 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 117 led2 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 118
DavidEGrayson 16:8eaa5bc2bdb1 119 while(!button1DefinitelyReleased());
DavidEGrayson 16:8eaa5bc2bdb1 120 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 121 led3 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 122
DavidEGrayson 16:8eaa5bc2bdb1 123 while(!button1DefinitelyReleased());
DavidEGrayson 16:8eaa5bc2bdb1 124 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 125 led4 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 126
DavidEGrayson 16:8eaa5bc2bdb1 127 while(1){};
DavidEGrayson 16:8eaa5bc2bdb1 128 }
DavidEGrayson 16:8eaa5bc2bdb1 129
DavidEGrayson 12:835a4d24ae3b 130 void testReckoner()
DavidEGrayson 12:835a4d24ae3b 131 {
DavidEGrayson 13:bba5b3abd13f 132 Pacer reportPacer(100000);
DavidEGrayson 12:835a4d24ae3b 133 while(1)
DavidEGrayson 12:835a4d24ae3b 134 {
DavidEGrayson 12:835a4d24ae3b 135 updateReckonerFromEncoders();
DavidEGrayson 13:bba5b3abd13f 136 led1 = (reckoner.cos > 0);
DavidEGrayson 13:bba5b3abd13f 137 led2 = (reckoner.sin > 0);
DavidEGrayson 13:bba5b3abd13f 138 led3 = (reckoner.x > 0);
DavidEGrayson 13:bba5b3abd13f 139 led4 = (reckoner.y > 0);
DavidEGrayson 12:835a4d24ae3b 140
DavidEGrayson 13:bba5b3abd13f 141 if (reportPacer.pace())
DavidEGrayson 13:bba5b3abd13f 142 {
DavidEGrayson 19:a11ffc903774 143 pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n",
DavidEGrayson 13:bba5b3abd13f 144 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 19:a11ffc903774 145 encoderLeft.getCount(), encoderRight.getCount(), determinant());
DavidEGrayson 13:bba5b3abd13f 146 }
DavidEGrayson 12:835a4d24ae3b 147 }
DavidEGrayson 12:835a4d24ae3b 148 }
DavidEGrayson 12:835a4d24ae3b 149
DavidEGrayson 10:e4dd36148539 150 void testLineSensors()
DavidEGrayson 10:e4dd36148539 151 {
DavidEGrayson 10:e4dd36148539 152 led1 = 1;
DavidEGrayson 10:e4dd36148539 153 Pacer reportPacer(100000);
DavidEGrayson 10:e4dd36148539 154 bool const printBarGraph = true;
DavidEGrayson 10:e4dd36148539 155 while (1)
DavidEGrayson 10:e4dd36148539 156 {
DavidEGrayson 10:e4dd36148539 157 if (reportPacer.pace())
DavidEGrayson 10:e4dd36148539 158 {
DavidEGrayson 10:e4dd36148539 159 uint16_t left = lineSensorsAnalog[0].read_u16();
DavidEGrayson 10:e4dd36148539 160 uint16_t middle = lineSensorsAnalog[1].read_u16();
DavidEGrayson 10:e4dd36148539 161 uint16_t right = lineSensorsAnalog[2].read_u16();
DavidEGrayson 10:e4dd36148539 162
DavidEGrayson 10:e4dd36148539 163 if (printBarGraph)
DavidEGrayson 10:e4dd36148539 164 {
DavidEGrayson 10:e4dd36148539 165 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 10:e4dd36148539 166 printBar("L", left);
DavidEGrayson 10:e4dd36148539 167 printBar("M", middle);
DavidEGrayson 10:e4dd36148539 168 printBar("R", right);
DavidEGrayson 10:e4dd36148539 169 }
DavidEGrayson 10:e4dd36148539 170 else
DavidEGrayson 10:e4dd36148539 171 {
DavidEGrayson 10:e4dd36148539 172 pc.printf("%8d %8d %8d\n", left, middle, right);
DavidEGrayson 10:e4dd36148539 173 }
DavidEGrayson 10:e4dd36148539 174 }
DavidEGrayson 10:e4dd36148539 175 }
DavidEGrayson 10:e4dd36148539 176 }
DavidEGrayson 8:78b1ff957cba 177
DavidEGrayson 24:fc01d9125d3b 178 // Values from David's office Values from dev lab,
DavidEGrayson 24:fc01d9125d3b 179 // in the day time, 2014-02-27: 2014-02-27:
DavidEGrayson 22:44c032e59ff5 180 // # calmin calmax
DavidEGrayson 24:fc01d9125d3b 181 // 0 34872 59726 0 40617 60222
DavidEGrayson 24:fc01d9125d3b 182 // 1 29335 60110 1 36937 61198
DavidEGrayson 24:fc01d9125d3b 183 // 2 23845 58446 2 33848 58862
DavidEGrayson 22:44c032e59ff5 184 void testCalibrate()
DavidEGrayson 22:44c032e59ff5 185 {
DavidEGrayson 22:44c032e59ff5 186 Timer timer;
DavidEGrayson 22:44c032e59ff5 187 timer.start();
DavidEGrayson 22:44c032e59ff5 188
DavidEGrayson 22:44c032e59ff5 189 Pacer reportPacer(200000);
DavidEGrayson 22:44c032e59ff5 190
DavidEGrayson 24:fc01d9125d3b 191 bool doneCalibrating = false;
DavidEGrayson 24:fc01d9125d3b 192
DavidEGrayson 24:fc01d9125d3b 193 led1 = 1;
DavidEGrayson 24:fc01d9125d3b 194
DavidEGrayson 22:44c032e59ff5 195 while(1)
DavidEGrayson 22:44c032e59ff5 196 {
DavidEGrayson 22:44c032e59ff5 197 lineTracker.read();
DavidEGrayson 24:fc01d9125d3b 198 if(!doneCalibrating)
DavidEGrayson 24:fc01d9125d3b 199 {
DavidEGrayson 24:fc01d9125d3b 200 lineTracker.updateCalibration();
DavidEGrayson 24:fc01d9125d3b 201 }
DavidEGrayson 24:fc01d9125d3b 202
DavidEGrayson 24:fc01d9125d3b 203 led2 = calibrationLooksGood();
DavidEGrayson 24:fc01d9125d3b 204 led3 = doneCalibrating;
DavidEGrayson 24:fc01d9125d3b 205 led4 = lineTracker.getLineVisible();
DavidEGrayson 24:fc01d9125d3b 206
DavidEGrayson 24:fc01d9125d3b 207 if (button1DefinitelyPressed())
DavidEGrayson 24:fc01d9125d3b 208 {
DavidEGrayson 24:fc01d9125d3b 209 doneCalibrating = true;
DavidEGrayson 24:fc01d9125d3b 210 }
DavidEGrayson 22:44c032e59ff5 211
DavidEGrayson 22:44c032e59ff5 212 if (reportPacer.pace())
DavidEGrayson 22:44c032e59ff5 213 {
DavidEGrayson 22:44c032e59ff5 214 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 22:44c032e59ff5 215 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 22:44c032e59ff5 216 {
DavidEGrayson 22:44c032e59ff5 217 pc.printf("%-2d %5d %5d %5d\r\n", s, lineTracker.calibratedMinimum[s], lineTracker.rawValues[s], lineTracker.calibratedMaximum[s]);
DavidEGrayson 22:44c032e59ff5 218 }
DavidEGrayson 22:44c032e59ff5 219 if (calibrationLooksGood())
DavidEGrayson 22:44c032e59ff5 220 {
DavidEGrayson 22:44c032e59ff5 221 pc.puts("Good. \r\n");
DavidEGrayson 22:44c032e59ff5 222 }
DavidEGrayson 22:44c032e59ff5 223 else
DavidEGrayson 22:44c032e59ff5 224 {
DavidEGrayson 22:44c032e59ff5 225 pc.puts("Not good yet.\r\n");
DavidEGrayson 22:44c032e59ff5 226 }
DavidEGrayson 22:44c032e59ff5 227 }
DavidEGrayson 22:44c032e59ff5 228 }
DavidEGrayson 22:44c032e59ff5 229 }
DavidEGrayson 22:44c032e59ff5 230
DavidEGrayson 9:9734347b5756 231 void testEncoders()
DavidEGrayson 9:9734347b5756 232 {
DavidEGrayson 9:9734347b5756 233 Pacer reportPacer(500000);
DavidEGrayson 10:e4dd36148539 234 led1 = 1;
DavidEGrayson 9:9734347b5756 235 while(1)
DavidEGrayson 9:9734347b5756 236 {
DavidEGrayson 9:9734347b5756 237 while(encoderBuffer.hasEvents())
DavidEGrayson 9:9734347b5756 238 {
DavidEGrayson 9:9734347b5756 239 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 9:9734347b5756 240 }
DavidEGrayson 9:9734347b5756 241
DavidEGrayson 9:9734347b5756 242 if(reportPacer.pace())
DavidEGrayson 9:9734347b5756 243 {
DavidEGrayson 9:9734347b5756 244 led2 = 1;
DavidEGrayson 9:9734347b5756 245 pc.printf("%8d %8d\n", encoderLeft.getCount(), encoderRight.getCount());
DavidEGrayson 9:9734347b5756 246 led2 = 0;
DavidEGrayson 10:e4dd36148539 247 }
DavidEGrayson 9:9734347b5756 248 }
DavidEGrayson 9:9734347b5756 249 }
DavidEGrayson 9:9734347b5756 250
DavidEGrayson 8:78b1ff957cba 251 void testMotors()
DavidEGrayson 8:78b1ff957cba 252 {
DavidEGrayson 8:78b1ff957cba 253 led1 = 1;
DavidEGrayson 8:78b1ff957cba 254 led2 = 0;
DavidEGrayson 8:78b1ff957cba 255 led3 = 0;
DavidEGrayson 8:78b1ff957cba 256 while(1)
DavidEGrayson 8:78b1ff957cba 257 {
DavidEGrayson 9:9734347b5756 258 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 259 led2 = 0;
DavidEGrayson 8:78b1ff957cba 260 led3 = 0;
DavidEGrayson 8:78b1ff957cba 261 wait(2);
DavidEGrayson 8:78b1ff957cba 262
DavidEGrayson 9:9734347b5756 263 motorsSpeedSet(300, 300);
DavidEGrayson 8:78b1ff957cba 264 wait(2);
DavidEGrayson 8:78b1ff957cba 265
DavidEGrayson 9:9734347b5756 266 motorsSpeedSet(-300, 300);
DavidEGrayson 8:78b1ff957cba 267 wait(2);
DavidEGrayson 8:78b1ff957cba 268
DavidEGrayson 9:9734347b5756 269 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 270 led2 = 1;
DavidEGrayson 8:78b1ff957cba 271 wait(2);
DavidEGrayson 8:78b1ff957cba 272
DavidEGrayson 9:9734347b5756 273 motorsSpeedSet(600, 600);
DavidEGrayson 8:78b1ff957cba 274 wait(2);
DavidEGrayson 8:78b1ff957cba 275
DavidEGrayson 9:9734347b5756 276 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 277 led3 = 1;
DavidEGrayson 8:78b1ff957cba 278 wait(2);
DavidEGrayson 8:78b1ff957cba 279
DavidEGrayson 9:9734347b5756 280 motorsSpeedSet(1200, 1200);
DavidEGrayson 8:78b1ff957cba 281 wait(2);
DavidEGrayson 8:78b1ff957cba 282 }
DavidEGrayson 10:e4dd36148539 283 }
DavidEGrayson 10:e4dd36148539 284
DavidEGrayson 20:dbec34f0e76b 285 void infiniteReckonerReportLoop()
DavidEGrayson 20:dbec34f0e76b 286 {
DavidEGrayson 20:dbec34f0e76b 287 Pacer reportPacer(200000);
DavidEGrayson 20:dbec34f0e76b 288 while(1)
DavidEGrayson 20:dbec34f0e76b 289 {
DavidEGrayson 20:dbec34f0e76b 290 if(reportPacer.pace())
DavidEGrayson 20:dbec34f0e76b 291 {
DavidEGrayson 20:dbec34f0e76b 292 led4 = 1;
DavidEGrayson 20:dbec34f0e76b 293 pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
DavidEGrayson 20:dbec34f0e76b 294 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 20:dbec34f0e76b 295 determinant(), dotProduct());
DavidEGrayson 20:dbec34f0e76b 296 led4 = 0;
DavidEGrayson 20:dbec34f0e76b 297 }
DavidEGrayson 20:dbec34f0e76b 298 }
DavidEGrayson 20:dbec34f0e76b 299
DavidEGrayson 20:dbec34f0e76b 300 }
DavidEGrayson 20:dbec34f0e76b 301
DavidEGrayson 10:e4dd36148539 302 void printBar(const char * name, uint16_t adcResult)
DavidEGrayson 10:e4dd36148539 303 {
DavidEGrayson 10:e4dd36148539 304 pc.printf("%-2s %5d |", name, adcResult);
DavidEGrayson 10:e4dd36148539 305 uint8_t width = adcResult >> 10;
DavidEGrayson 10:e4dd36148539 306 uint8_t i;
DavidEGrayson 10:e4dd36148539 307 for(i = 0; i < width; i++){ pc.putc('#'); }
DavidEGrayson 10:e4dd36148539 308 for(; i < 63; i++){ pc.putc(' '); }
DavidEGrayson 10:e4dd36148539 309 pc.putc('|');
DavidEGrayson 10:e4dd36148539 310 pc.putc('\n');
DavidEGrayson 8:78b1ff957cba 311 }