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@12:835a4d24ae3b, 2014-02-23 (annotated)

Committer:
DavidEGrayson
Date:
Sun Feb 23 22:23:34 2014 +0000
Revision:
12:835a4d24ae3b
Parent:
10:e4dd36148539
Child:
13:bba5b3abd13f
Made the Reckoner class and wrote a routine to help test it.;

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