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@20:dbec34f0e76b, 2014-02-27 (annotated)

Committer:
DavidEGrayson
Date:
Thu Feb 27 19:46:35 2014 +0000
Revision:
20:dbec34f0e76b
Parent:
19:a11ffc903774
Child:
21:c279c6a83671
Broke up driveHome into two functions.

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 9:9734347b5756 7 #include "test.h"
DavidEGrayson 8:78b1ff957cba 8 #include "leds.h"
DavidEGrayson 9:9734347b5756 9 #include "encoders.h"
DavidEGrayson 8:78b1ff957cba 10 #include "pc_serial.h"
DavidEGrayson 10:e4dd36148539 11 #include "line_sensors.h"
DavidEGrayson 12:835a4d24ae3b 12 #include "reckoner.h"
DavidEGrayson 16:8eaa5bc2bdb1 13 #include "buttons.h"
DavidEGrayson 16:8eaa5bc2bdb1 14
DavidEGrayson 20:dbec34f0e76b 15 void __attribute__((noreturn)) infiniteReckonerReportLoop();
DavidEGrayson 10:e4dd36148539 16 void printBar(const char * name, uint16_t adcResult);
DavidEGrayson 10:e4dd36148539 17
DavidEGrayson 17:2df9861f53ee 18 void testDriveHome()
DavidEGrayson 16:8eaa5bc2bdb1 19 {
DavidEGrayson 17:2df9861f53ee 20 led1 = 1;
DavidEGrayson 18:b65fbb795396 21 while(!button1DefinitelyPressed())
DavidEGrayson 17:2df9861f53ee 22 {
DavidEGrayson 17:2df9861f53ee 23 updateReckonerFromEncoders();
DavidEGrayson 17:2df9861f53ee 24 }
DavidEGrayson 17:2df9861f53ee 25 driveHome();
DavidEGrayson 20:dbec34f0e76b 26 infiniteReckonerReportLoop();
DavidEGrayson 20:dbec34f0e76b 27 }
DavidEGrayson 20:dbec34f0e76b 28
DavidEGrayson 20:dbec34f0e76b 29 void testFinalSettleIn()
DavidEGrayson 20:dbec34f0e76b 30 {
DavidEGrayson 20:dbec34f0e76b 31 led1 = 1;
DavidEGrayson 20:dbec34f0e76b 32 while(!button1DefinitelyPressed())
DavidEGrayson 20:dbec34f0e76b 33 {
DavidEGrayson 20:dbec34f0e76b 34 updateReckonerFromEncoders();
DavidEGrayson 20:dbec34f0e76b 35 }
DavidEGrayson 20:dbec34f0e76b 36 finalSettleIn();
DavidEGrayson 20:dbec34f0e76b 37 infiniteReckonerReportLoop();
DavidEGrayson 16:8eaa5bc2bdb1 38 }
DavidEGrayson 16:8eaa5bc2bdb1 39
DavidEGrayson 17:2df9861f53ee 40
DavidEGrayson 16:8eaa5bc2bdb1 41 void testButtons()
DavidEGrayson 16:8eaa5bc2bdb1 42 {
DavidEGrayson 16:8eaa5bc2bdb1 43 led1 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 44
DavidEGrayson 17:2df9861f53ee 45 while(!button1DefinitelyReleased());
DavidEGrayson 17:2df9861f53ee 46 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 47 led2 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 48
DavidEGrayson 16:8eaa5bc2bdb1 49 while(!button1DefinitelyReleased());
DavidEGrayson 16:8eaa5bc2bdb1 50 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 51 led3 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 52
DavidEGrayson 16:8eaa5bc2bdb1 53 while(!button1DefinitelyReleased());
DavidEGrayson 16:8eaa5bc2bdb1 54 while(!button1DefinitelyPressed());
DavidEGrayson 16:8eaa5bc2bdb1 55 led4 = 1;
DavidEGrayson 16:8eaa5bc2bdb1 56
DavidEGrayson 16:8eaa5bc2bdb1 57 while(1){};
DavidEGrayson 16:8eaa5bc2bdb1 58 }
DavidEGrayson 16:8eaa5bc2bdb1 59
DavidEGrayson 12:835a4d24ae3b 60 void testReckoner()
DavidEGrayson 12:835a4d24ae3b 61 {
DavidEGrayson 13:bba5b3abd13f 62 Pacer reportPacer(100000);
DavidEGrayson 12:835a4d24ae3b 63 while(1)
DavidEGrayson 12:835a4d24ae3b 64 {
DavidEGrayson 12:835a4d24ae3b 65 updateReckonerFromEncoders();
DavidEGrayson 13:bba5b3abd13f 66 led1 = (reckoner.cos > 0);
DavidEGrayson 13:bba5b3abd13f 67 led2 = (reckoner.sin > 0);
DavidEGrayson 13:bba5b3abd13f 68 led3 = (reckoner.x > 0);
DavidEGrayson 13:bba5b3abd13f 69 led4 = (reckoner.y > 0);
DavidEGrayson 12:835a4d24ae3b 70
DavidEGrayson 13:bba5b3abd13f 71 if (reportPacer.pace())
DavidEGrayson 13:bba5b3abd13f 72 {
DavidEGrayson 19:a11ffc903774 73 pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n",
DavidEGrayson 13:bba5b3abd13f 74 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 19:a11ffc903774 75 encoderLeft.getCount(), encoderRight.getCount(), determinant());
DavidEGrayson 13:bba5b3abd13f 76 }
DavidEGrayson 12:835a4d24ae3b 77 }
DavidEGrayson 12:835a4d24ae3b 78 }
DavidEGrayson 12:835a4d24ae3b 79
DavidEGrayson 10:e4dd36148539 80 void testLineSensors()
DavidEGrayson 10:e4dd36148539 81 {
DavidEGrayson 10:e4dd36148539 82 led1 = 1;
DavidEGrayson 10:e4dd36148539 83 Pacer reportPacer(100000);
DavidEGrayson 10:e4dd36148539 84 bool const printBarGraph = true;
DavidEGrayson 10:e4dd36148539 85 while (1)
DavidEGrayson 10:e4dd36148539 86 {
DavidEGrayson 10:e4dd36148539 87 if (reportPacer.pace())
DavidEGrayson 10:e4dd36148539 88 {
DavidEGrayson 10:e4dd36148539 89 uint16_t left = lineSensorsAnalog[0].read_u16();
DavidEGrayson 10:e4dd36148539 90 uint16_t middle = lineSensorsAnalog[1].read_u16();
DavidEGrayson 10:e4dd36148539 91 uint16_t right = lineSensorsAnalog[2].read_u16();
DavidEGrayson 10:e4dd36148539 92
DavidEGrayson 10:e4dd36148539 93 if (printBarGraph)
DavidEGrayson 10:e4dd36148539 94 {
DavidEGrayson 10:e4dd36148539 95 pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0"
DavidEGrayson 10:e4dd36148539 96 printBar("L", left);
DavidEGrayson 10:e4dd36148539 97 printBar("M", middle);
DavidEGrayson 10:e4dd36148539 98 printBar("R", right);
DavidEGrayson 10:e4dd36148539 99 }
DavidEGrayson 10:e4dd36148539 100 else
DavidEGrayson 10:e4dd36148539 101 {
DavidEGrayson 10:e4dd36148539 102 pc.printf("%8d %8d %8d\n", left, middle, right);
DavidEGrayson 10:e4dd36148539 103 }
DavidEGrayson 10:e4dd36148539 104 }
DavidEGrayson 10:e4dd36148539 105 }
DavidEGrayson 10:e4dd36148539 106 }
DavidEGrayson 8:78b1ff957cba 107
DavidEGrayson 9:9734347b5756 108 void testEncoders()
DavidEGrayson 9:9734347b5756 109 {
DavidEGrayson 9:9734347b5756 110 Pacer reportPacer(500000);
DavidEGrayson 10:e4dd36148539 111 led1 = 1;
DavidEGrayson 9:9734347b5756 112 while(1)
DavidEGrayson 9:9734347b5756 113 {
DavidEGrayson 9:9734347b5756 114 while(encoderBuffer.hasEvents())
DavidEGrayson 9:9734347b5756 115 {
DavidEGrayson 9:9734347b5756 116 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 9:9734347b5756 117 }
DavidEGrayson 9:9734347b5756 118
DavidEGrayson 9:9734347b5756 119 if(reportPacer.pace())
DavidEGrayson 9:9734347b5756 120 {
DavidEGrayson 9:9734347b5756 121 led2 = 1;
DavidEGrayson 9:9734347b5756 122 pc.printf("%8d %8d\n", encoderLeft.getCount(), encoderRight.getCount());
DavidEGrayson 9:9734347b5756 123 led2 = 0;
DavidEGrayson 10:e4dd36148539 124 }
DavidEGrayson 9:9734347b5756 125 }
DavidEGrayson 9:9734347b5756 126 }
DavidEGrayson 9:9734347b5756 127
DavidEGrayson 8:78b1ff957cba 128 void testMotors()
DavidEGrayson 8:78b1ff957cba 129 {
DavidEGrayson 8:78b1ff957cba 130 led1 = 1;
DavidEGrayson 8:78b1ff957cba 131 led2 = 0;
DavidEGrayson 8:78b1ff957cba 132 led3 = 0;
DavidEGrayson 8:78b1ff957cba 133 while(1)
DavidEGrayson 8:78b1ff957cba 134 {
DavidEGrayson 9:9734347b5756 135 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 136 led2 = 0;
DavidEGrayson 8:78b1ff957cba 137 led3 = 0;
DavidEGrayson 8:78b1ff957cba 138 wait(2);
DavidEGrayson 8:78b1ff957cba 139
DavidEGrayson 9:9734347b5756 140 motorsSpeedSet(300, 300);
DavidEGrayson 8:78b1ff957cba 141 wait(2);
DavidEGrayson 8:78b1ff957cba 142
DavidEGrayson 9:9734347b5756 143 motorsSpeedSet(-300, 300);
DavidEGrayson 8:78b1ff957cba 144 wait(2);
DavidEGrayson 8:78b1ff957cba 145
DavidEGrayson 9:9734347b5756 146 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 147 led2 = 1;
DavidEGrayson 8:78b1ff957cba 148 wait(2);
DavidEGrayson 8:78b1ff957cba 149
DavidEGrayson 9:9734347b5756 150 motorsSpeedSet(600, 600);
DavidEGrayson 8:78b1ff957cba 151 wait(2);
DavidEGrayson 8:78b1ff957cba 152
DavidEGrayson 9:9734347b5756 153 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 154 led3 = 1;
DavidEGrayson 8:78b1ff957cba 155 wait(2);
DavidEGrayson 8:78b1ff957cba 156
DavidEGrayson 9:9734347b5756 157 motorsSpeedSet(1200, 1200);
DavidEGrayson 8:78b1ff957cba 158 wait(2);
DavidEGrayson 8:78b1ff957cba 159 }
DavidEGrayson 10:e4dd36148539 160 }
DavidEGrayson 10:e4dd36148539 161
DavidEGrayson 20:dbec34f0e76b 162 void infiniteReckonerReportLoop()
DavidEGrayson 20:dbec34f0e76b 163 {
DavidEGrayson 20:dbec34f0e76b 164 Pacer reportPacer(200000);
DavidEGrayson 20:dbec34f0e76b 165 while(1)
DavidEGrayson 20:dbec34f0e76b 166 {
DavidEGrayson 20:dbec34f0e76b 167 if(reportPacer.pace())
DavidEGrayson 20:dbec34f0e76b 168 {
DavidEGrayson 20:dbec34f0e76b 169 led4 = 1;
DavidEGrayson 20:dbec34f0e76b 170 pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
DavidEGrayson 20:dbec34f0e76b 171 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 20:dbec34f0e76b 172 determinant(), dotProduct());
DavidEGrayson 20:dbec34f0e76b 173 led4 = 0;
DavidEGrayson 20:dbec34f0e76b 174 }
DavidEGrayson 20:dbec34f0e76b 175 }
DavidEGrayson 20:dbec34f0e76b 176
DavidEGrayson 20:dbec34f0e76b 177 }
DavidEGrayson 20:dbec34f0e76b 178
DavidEGrayson 10:e4dd36148539 179 void printBar(const char * name, uint16_t adcResult)
DavidEGrayson 10:e4dd36148539 180 {
DavidEGrayson 10:e4dd36148539 181 pc.printf("%-2s %5d |", name, adcResult);
DavidEGrayson 10:e4dd36148539 182 uint8_t width = adcResult >> 10;
DavidEGrayson 10:e4dd36148539 183 uint8_t i;
DavidEGrayson 10:e4dd36148539 184 for(i = 0; i < width; i++){ pc.putc('#'); }
DavidEGrayson 10:e4dd36148539 185 for(; i < 63; i++){ pc.putc(' '); }
DavidEGrayson 10:e4dd36148539 186 pc.putc('|');
DavidEGrayson 10:e4dd36148539 187 pc.putc('\n');
DavidEGrayson 8:78b1ff957cba 188 }