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@22:44c032e59ff5, 2014-02-28 (annotated)

Committer:
DavidEGrayson
Date:
Fri Feb 28 00:16:49 2014 +0000
Revision:
22:44c032e59ff5
Parent:
21:c279c6a83671
Child:
24:fc01d9125d3b
Fixed the code for calibrating.  Added testCalibrate.

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 22:44c032e59ff5 110 // If the calibration stops working, we could just use these values from David's office in the day time:
DavidEGrayson 22:44c032e59ff5 111 // # calmin calmax
DavidEGrayson 22:44c032e59ff5 112 // 0 34872 59726
DavidEGrayson 22:44c032e59ff5 113 // 1 29335 60110
DavidEGrayson 22:44c032e59ff5 114 // 2 23845 58446
DavidEGrayson 22:44c032e59ff5 115 void testCalibrate()
DavidEGrayson 22:44c032e59ff5 116 {
DavidEGrayson 22:44c032e59ff5 117 Timer timer;
DavidEGrayson 22:44c032e59ff5 118 timer.start();
DavidEGrayson 22:44c032e59ff5 119
DavidEGrayson 22:44c032e59ff5 120 Pacer reportPacer(200000);
DavidEGrayson 22:44c032e59ff5 121
DavidEGrayson 22:44c032e59ff5 122 while(1)
DavidEGrayson 22:44c032e59ff5 123 {
DavidEGrayson 22:44c032e59ff5 124 lineTracker.read();
DavidEGrayson 22:44c032e59ff5 125 lineTracker.updateCalibration();
DavidEGrayson 22:44c032e59ff5 126 led4 = calibrationLooksGood();
DavidEGrayson 22:44c032e59ff5 127
DavidEGrayson 22:44c032e59ff5 128 if (reportPacer.pace())
DavidEGrayson 22:44c032e59ff5 129 {
DavidEGrayson 22:44c032e59ff5 130 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 22:44c032e59ff5 131 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 22:44c032e59ff5 132 {
DavidEGrayson 22:44c032e59ff5 133 pc.printf("%-2d %5d %5d %5d\r\n", s, lineTracker.calibratedMinimum[s], lineTracker.rawValues[s], lineTracker.calibratedMaximum[s]);
DavidEGrayson 22:44c032e59ff5 134 }
DavidEGrayson 22:44c032e59ff5 135 if (calibrationLooksGood())
DavidEGrayson 22:44c032e59ff5 136 {
DavidEGrayson 22:44c032e59ff5 137 pc.puts("Good. \r\n");
DavidEGrayson 22:44c032e59ff5 138 }
DavidEGrayson 22:44c032e59ff5 139 else
DavidEGrayson 22:44c032e59ff5 140 {
DavidEGrayson 22:44c032e59ff5 141 pc.puts("Not good yet.\r\n");
DavidEGrayson 22:44c032e59ff5 142 }
DavidEGrayson 22:44c032e59ff5 143 }
DavidEGrayson 22:44c032e59ff5 144 }
DavidEGrayson 22:44c032e59ff5 145 }
DavidEGrayson 22:44c032e59ff5 146
DavidEGrayson 9:9734347b5756 147 void testEncoders()
DavidEGrayson 9:9734347b5756 148 {
DavidEGrayson 9:9734347b5756 149 Pacer reportPacer(500000);
DavidEGrayson 10:e4dd36148539 150 led1 = 1;
DavidEGrayson 9:9734347b5756 151 while(1)
DavidEGrayson 9:9734347b5756 152 {
DavidEGrayson 9:9734347b5756 153 while(encoderBuffer.hasEvents())
DavidEGrayson 9:9734347b5756 154 {
DavidEGrayson 9:9734347b5756 155 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 9:9734347b5756 156 }
DavidEGrayson 9:9734347b5756 157
DavidEGrayson 9:9734347b5756 158 if(reportPacer.pace())
DavidEGrayson 9:9734347b5756 159 {
DavidEGrayson 9:9734347b5756 160 led2 = 1;
DavidEGrayson 9:9734347b5756 161 pc.printf("%8d %8d\n", encoderLeft.getCount(), encoderRight.getCount());
DavidEGrayson 9:9734347b5756 162 led2 = 0;
DavidEGrayson 10:e4dd36148539 163 }
DavidEGrayson 9:9734347b5756 164 }
DavidEGrayson 9:9734347b5756 165 }
DavidEGrayson 9:9734347b5756 166
DavidEGrayson 8:78b1ff957cba 167 void testMotors()
DavidEGrayson 8:78b1ff957cba 168 {
DavidEGrayson 8:78b1ff957cba 169 led1 = 1;
DavidEGrayson 8:78b1ff957cba 170 led2 = 0;
DavidEGrayson 8:78b1ff957cba 171 led3 = 0;
DavidEGrayson 8:78b1ff957cba 172 while(1)
DavidEGrayson 8:78b1ff957cba 173 {
DavidEGrayson 9:9734347b5756 174 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 175 led2 = 0;
DavidEGrayson 8:78b1ff957cba 176 led3 = 0;
DavidEGrayson 8:78b1ff957cba 177 wait(2);
DavidEGrayson 8:78b1ff957cba 178
DavidEGrayson 9:9734347b5756 179 motorsSpeedSet(300, 300);
DavidEGrayson 8:78b1ff957cba 180 wait(2);
DavidEGrayson 8:78b1ff957cba 181
DavidEGrayson 9:9734347b5756 182 motorsSpeedSet(-300, 300);
DavidEGrayson 8:78b1ff957cba 183 wait(2);
DavidEGrayson 8:78b1ff957cba 184
DavidEGrayson 9:9734347b5756 185 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 186 led2 = 1;
DavidEGrayson 8:78b1ff957cba 187 wait(2);
DavidEGrayson 8:78b1ff957cba 188
DavidEGrayson 9:9734347b5756 189 motorsSpeedSet(600, 600);
DavidEGrayson 8:78b1ff957cba 190 wait(2);
DavidEGrayson 8:78b1ff957cba 191
DavidEGrayson 9:9734347b5756 192 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 193 led3 = 1;
DavidEGrayson 8:78b1ff957cba 194 wait(2);
DavidEGrayson 8:78b1ff957cba 195
DavidEGrayson 9:9734347b5756 196 motorsSpeedSet(1200, 1200);
DavidEGrayson 8:78b1ff957cba 197 wait(2);
DavidEGrayson 8:78b1ff957cba 198 }
DavidEGrayson 10:e4dd36148539 199 }
DavidEGrayson 10:e4dd36148539 200
DavidEGrayson 20:dbec34f0e76b 201 void infiniteReckonerReportLoop()
DavidEGrayson 20:dbec34f0e76b 202 {
DavidEGrayson 20:dbec34f0e76b 203 Pacer reportPacer(200000);
DavidEGrayson 20:dbec34f0e76b 204 while(1)
DavidEGrayson 20:dbec34f0e76b 205 {
DavidEGrayson 20:dbec34f0e76b 206 if(reportPacer.pace())
DavidEGrayson 20:dbec34f0e76b 207 {
DavidEGrayson 20:dbec34f0e76b 208 led4 = 1;
DavidEGrayson 20:dbec34f0e76b 209 pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
DavidEGrayson 20:dbec34f0e76b 210 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 20:dbec34f0e76b 211 determinant(), dotProduct());
DavidEGrayson 20:dbec34f0e76b 212 led4 = 0;
DavidEGrayson 20:dbec34f0e76b 213 }
DavidEGrayson 20:dbec34f0e76b 214 }
DavidEGrayson 20:dbec34f0e76b 215
DavidEGrayson 20:dbec34f0e76b 216 }
DavidEGrayson 20:dbec34f0e76b 217
DavidEGrayson 10:e4dd36148539 218 void printBar(const char * name, uint16_t adcResult)
DavidEGrayson 10:e4dd36148539 219 {
DavidEGrayson 10:e4dd36148539 220 pc.printf("%-2s %5d |", name, adcResult);
DavidEGrayson 10:e4dd36148539 221 uint8_t width = adcResult >> 10;
DavidEGrayson 10:e4dd36148539 222 uint8_t i;
DavidEGrayson 10:e4dd36148539 223 for(i = 0; i < width; i++){ pc.putc('#'); }
DavidEGrayson 10:e4dd36148539 224 for(; i < 63; i++){ pc.putc(' '); }
DavidEGrayson 10:e4dd36148539 225 pc.putc('|');
DavidEGrayson 10:e4dd36148539 226 pc.putc('\n');
DavidEGrayson 8:78b1ff957cba 227 }