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@10:e4dd36148539, 2014-02-22 (annotated)

Committer:
DavidEGrayson
Date:
Sat Feb 22 04:44:44 2014 +0000
Revision:
10:e4dd36148539
Parent:
9:9734347b5756
Child:
12:835a4d24ae3b
Successfully tested the encoders, printing out a nice bar graph.

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