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@14:c8cca3687e64, 2014-02-24 (annotated)

Committer:
DavidEGrayson
Date:
Mon Feb 24 00:07:54 2014 +0000
Revision:
14:c8cca3687e64
Parent:
13:bba5b3abd13f
Child:
16:8eaa5bc2bdb1
Started doing an experiment to see what dA really is.

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 14:c8cca3687e64 29 pc.printf("%11d %11d %11d %11d | %8d %8d\r\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 }