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@24:fc01d9125d3b, 2014-02-28 (annotated)

Committer:
DavidEGrayson
Date:
Fri Feb 28 01:07:14 2014 +0000
Revision:
24:fc01d9125d3b
Parent:
22:44c032e59ff5
Child:
28:4374035df5e0
Fixed the problem with LineTracker always thinking the line was visible.

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