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

Committer:
DavidEGrayson
Date:
Thu Feb 27 23:20:34 2014 +0000
Revision:
21:c279c6a83671
Parent:
20:dbec34f0e76b
Child:
22:44c032e59ff5
Wrote a whole bunch of code that could theoretically allow the robot to compete, but it has not been tested at all yet.

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 9:9734347b5756 110 void testEncoders()
DavidEGrayson 9:9734347b5756 111 {
DavidEGrayson 9:9734347b5756 112 Pacer reportPacer(500000);
DavidEGrayson 10:e4dd36148539 113 led1 = 1;
DavidEGrayson 9:9734347b5756 114 while(1)
DavidEGrayson 9:9734347b5756 115 {
DavidEGrayson 9:9734347b5756 116 while(encoderBuffer.hasEvents())
DavidEGrayson 9:9734347b5756 117 {
DavidEGrayson 9:9734347b5756 118 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 9:9734347b5756 119 }
DavidEGrayson 9:9734347b5756 120
DavidEGrayson 9:9734347b5756 121 if(reportPacer.pace())
DavidEGrayson 9:9734347b5756 122 {
DavidEGrayson 9:9734347b5756 123 led2 = 1;
DavidEGrayson 9:9734347b5756 124 pc.printf("%8d %8d\n", encoderLeft.getCount(), encoderRight.getCount());
DavidEGrayson 9:9734347b5756 125 led2 = 0;
DavidEGrayson 10:e4dd36148539 126 }
DavidEGrayson 9:9734347b5756 127 }
DavidEGrayson 9:9734347b5756 128 }
DavidEGrayson 9:9734347b5756 129
DavidEGrayson 8:78b1ff957cba 130 void testMotors()
DavidEGrayson 8:78b1ff957cba 131 {
DavidEGrayson 8:78b1ff957cba 132 led1 = 1;
DavidEGrayson 8:78b1ff957cba 133 led2 = 0;
DavidEGrayson 8:78b1ff957cba 134 led3 = 0;
DavidEGrayson 8:78b1ff957cba 135 while(1)
DavidEGrayson 8:78b1ff957cba 136 {
DavidEGrayson 9:9734347b5756 137 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 138 led2 = 0;
DavidEGrayson 8:78b1ff957cba 139 led3 = 0;
DavidEGrayson 8:78b1ff957cba 140 wait(2);
DavidEGrayson 8:78b1ff957cba 141
DavidEGrayson 9:9734347b5756 142 motorsSpeedSet(300, 300);
DavidEGrayson 8:78b1ff957cba 143 wait(2);
DavidEGrayson 8:78b1ff957cba 144
DavidEGrayson 9:9734347b5756 145 motorsSpeedSet(-300, 300);
DavidEGrayson 8:78b1ff957cba 146 wait(2);
DavidEGrayson 8:78b1ff957cba 147
DavidEGrayson 9:9734347b5756 148 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 149 led2 = 1;
DavidEGrayson 8:78b1ff957cba 150 wait(2);
DavidEGrayson 8:78b1ff957cba 151
DavidEGrayson 9:9734347b5756 152 motorsSpeedSet(600, 600);
DavidEGrayson 8:78b1ff957cba 153 wait(2);
DavidEGrayson 8:78b1ff957cba 154
DavidEGrayson 9:9734347b5756 155 motorsSpeedSet(0, 0);
DavidEGrayson 8:78b1ff957cba 156 led3 = 1;
DavidEGrayson 8:78b1ff957cba 157 wait(2);
DavidEGrayson 8:78b1ff957cba 158
DavidEGrayson 9:9734347b5756 159 motorsSpeedSet(1200, 1200);
DavidEGrayson 8:78b1ff957cba 160 wait(2);
DavidEGrayson 8:78b1ff957cba 161 }
DavidEGrayson 10:e4dd36148539 162 }
DavidEGrayson 10:e4dd36148539 163
DavidEGrayson 20:dbec34f0e76b 164 void infiniteReckonerReportLoop()
DavidEGrayson 20:dbec34f0e76b 165 {
DavidEGrayson 20:dbec34f0e76b 166 Pacer reportPacer(200000);
DavidEGrayson 20:dbec34f0e76b 167 while(1)
DavidEGrayson 20:dbec34f0e76b 168 {
DavidEGrayson 20:dbec34f0e76b 169 if(reportPacer.pace())
DavidEGrayson 20:dbec34f0e76b 170 {
DavidEGrayson 20:dbec34f0e76b 171 led4 = 1;
DavidEGrayson 20:dbec34f0e76b 172 pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
DavidEGrayson 20:dbec34f0e76b 173 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 20:dbec34f0e76b 174 determinant(), dotProduct());
DavidEGrayson 20:dbec34f0e76b 175 led4 = 0;
DavidEGrayson 20:dbec34f0e76b 176 }
DavidEGrayson 20:dbec34f0e76b 177 }
DavidEGrayson 20:dbec34f0e76b 178
DavidEGrayson 20:dbec34f0e76b 179 }
DavidEGrayson 20:dbec34f0e76b 180
DavidEGrayson 10:e4dd36148539 181 void printBar(const char * name, uint16_t adcResult)
DavidEGrayson 10:e4dd36148539 182 {
DavidEGrayson 10:e4dd36148539 183 pc.printf("%-2s %5d |", name, adcResult);
DavidEGrayson 10:e4dd36148539 184 uint8_t width = adcResult >> 10;
DavidEGrayson 10:e4dd36148539 185 uint8_t i;
DavidEGrayson 10:e4dd36148539 186 for(i = 0; i < width; i++){ pc.putc('#'); }
DavidEGrayson 10:e4dd36148539 187 for(; i < 63; i++){ pc.putc(' '); }
DavidEGrayson 10:e4dd36148539 188 pc.putc('|');
DavidEGrayson 10:e4dd36148539 189 pc.putc('\n');
DavidEGrayson 8:78b1ff957cba 190 }