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@29:cfcf08d8ac79, 2014-03-04 (annotated)

Committer:
DavidEGrayson
Date:
Tue Mar 04 00:46:18 2014 +0000
Revision:
29:cfcf08d8ac79
Parent:
28:4374035df5e0
Child:
30:84be2d602dc0
trying to figure out the analog problem;

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