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@16:8eaa5bc2bdb1, 2014-02-24 (annotated)

Committer:
DavidEGrayson
Date:
Mon Feb 24 01:26:00 2014 +0000
Revision:
16:8eaa5bc2bdb1
Parent:
14:c8cca3687e64
Child:
17:2df9861f53ee
Got debounced button pressing to work.

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