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@13:bba5b3abd13f, 2014-02-23 (annotated)

Committer:
DavidEGrayson
Date:
Sun Feb 23 23:49:58 2014 +0000
Revision:
13:bba5b3abd13f
Parent:
12:835a4d24ae3b
Child:
14:c8cca3687e64
testReckoner routine works great!  I can roll my robot around and it always knows what quadrant it is pointing to and knows what quadrant it is in!

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