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@19:a11ffc903774, 2014-02-25 (annotated)

Committer:
DavidEGrayson
Date:
Tue Feb 25 02:58:16 2014 +0000
Revision:
19:a11ffc903774
Parent:
18:b65fbb795396
Child:
20:dbec34f0e76b
Got the robot to get back to its home position and stop.

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