Terrabots / Mbed 2 deprecated Tracker

official library and example code for tracking X and Y positions on the dump truck

Dependencies:   QEI mbed

Dependents:   DUMP_TRUCK_TEST_V1 DUMP_TRUCK_SPR2017

Tracker.cpp@6:50f337affc44, 2017-02-14 (annotated)

Committer:
simplyellow
Date:
Tue Feb 14 21:18:43 2017 +0000
Revision:
6:50f337affc44
Parent:
4:c38c00a6588c 
added doxygen tags for some variables

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simplyellow 0:8038ea3ee241 1 #include "Tracker.h"
simplyellow 0:8038ea3ee241 2
simplyellow 4:c38c00a6588c 3 //test edit
simplyellow 4:c38c00a6588c 4
simplyellow 0:8038ea3ee241 5 Tracker::Tracker(PinName _chA, PinName _chB, PinName _pot) : chA(_chA), chB(_chB), pot(_pot)
simplyellow 0:8038ea3ee241 6 {
simplyellow 0:8038ea3ee241 7 //chA = p29, chB = p30, pot = p15
simplyellow 3:eb8680da1721 8 wheel = new QEI(chA, chB, NC, 128, QEI::X4_ENCODING); //orange, yellow wires, VU 5 VOLTS, BOTTOM 2x2 pins on WW02
simplyellow 0:8038ea3ee241 9 encoding = 2; // X2 encoding
simplyellow 0:8038ea3ee241 10 circumference = 13.25; // circumference of wheel
simplyellow 0:8038ea3ee241 11 constant = 1360; // obtained from tests
simplyellow 0:8038ea3ee241 12 dir = 3; // starting dir
simplyellow 0:8038ea3ee241 13 df = 4.50; // distance from pivot to front wheel
simplyellow 0:8038ea3ee241 14 db = 4.25; // distance from pivot to back wheel
simplyellow 0:8038ea3ee241 15 }
simplyellow 0:8038ea3ee241 16
simplyellow 1:008116b6eb46 17 /*
simplyellow 1:008116b6eb46 18 // Example Code
simplyellow 1:008116b6eb46 19 #include "mbed.h"
simplyellow 1:008116b6eb46 20 #include "Tracker.h"
simplyellow 1:008116b6eb46 21
simplyellow 1:008116b6eb46 22 Tracker foo(p29, p30, p15);
simplyellow 1:008116b6eb46 23
simplyellow 1:008116b6eb46 24 int main() {
simplyellow 1:008116b6eb46 25 foo.clear();
simplyellow 1:008116b6eb46 26 foo.potSetup();
simplyellow 1:008116b6eb46 27 foo.setDirection();
simplyellow 1:008116b6eb46 28 foo.checkToStart();
simplyellow 1:008116b6eb46 29 while(true) {
simplyellow 1:008116b6eb46 30 foo.calcDisplacement();
simplyellow 1:008116b6eb46 31 }
simplyellow 1:008116b6eb46 32 }
simplyellow 1:008116b6eb46 33 */
simplyellow 1:008116b6eb46 34
simplyellow 0:8038ea3ee241 35 /*Tracker::~Tracker()
simplyellow 0:8038ea3ee241 36 {
simplyellow 0:8038ea3ee241 37 if(wheel != NULL) {
simplyellow 0:8038ea3ee241 38 delete wheel;
simplyellow 0:8038ea3ee241 39 }
simplyellow 0:8038ea3ee241 40 }*/
simplyellow 0:8038ea3ee241 41
simplyellow 0:8038ea3ee241 42 void Tracker::clear() {
simplyellow 0:8038ea3ee241 43 distance = 0;
simplyellow 0:8038ea3ee241 44 x = 0;
simplyellow 0:8038ea3ee241 45 y = 0;
simplyellow 0:8038ea3ee241 46 }
simplyellow 0:8038ea3ee241 47
simplyellow 0:8038ea3ee241 48 void Tracker::potSetup() {
simplyellow 0:8038ea3ee241 49 zeroAngle = (float) pot;
simplyellow 0:8038ea3ee241 50 // USE A BETTER CALIBRATION PERIOD
simplyellow 0:8038ea3ee241 51 for(int i = 5; i > 0; i--) {
simplyellow 0:8038ea3ee241 52 printf("%d seconds to set turning angle.\n\r", i);
simplyellow 0:8038ea3ee241 53 wait(1);
simplyellow 0:8038ea3ee241 54 }
simplyellow 0:8038ea3ee241 55 turnAngle = ((float) pot - zeroAngle) * 300;
simplyellow 0:8038ea3ee241 56 }
simplyellow 0:8038ea3ee241 57
simplyellow 0:8038ea3ee241 58 void Tracker::setDirection() {
simplyellow 0:8038ea3ee241 59 if (turnAngle > 0.5) { // +/- 0.5 leeway
simplyellow 0:8038ea3ee241 60 printf("DIRECTION SET TO CCW with turnAngle %f\n\r", turnAngle);
simplyellow 0:8038ea3ee241 61 dir = CCW;
simplyellow 0:8038ea3ee241 62 printf("%d is direction\n\r", dir);
simplyellow 0:8038ea3ee241 63 } else if (turnAngle < -0.5) {
simplyellow 0:8038ea3ee241 64 printf("DIRECTION SET TO CW with turnAngle %f\n\r", turnAngle);
simplyellow 0:8038ea3ee241 65 dir = CW;
simplyellow 0:8038ea3ee241 66 } else {
simplyellow 0:8038ea3ee241 67 printf("DIRECTION SET TO STRAIGHT with turnAngle %f\n\r", turnAngle);
simplyellow 0:8038ea3ee241 68 dir = STRAIGHT;
simplyellow 0:8038ea3ee241 69 }
simplyellow 0:8038ea3ee241 70 turnAngle = abs(turnAngle);
simplyellow 0:8038ea3ee241 71 }
simplyellow 0:8038ea3ee241 72
simplyellow 0:8038ea3ee241 73 void Tracker::checkToStart() { // start tracking when distance > 0
simplyellow 0:8038ea3ee241 74 while(true) {
simplyellow 0:8038ea3ee241 75 pulseCount = (float) wheel->getPulses();
simplyellow 0:8038ea3ee241 76 distance = pulseCount*circumference/encoding/constant;
simplyellow 0:8038ea3ee241 77 if (distance > 0) {
simplyellow 0:8038ea3ee241 78 return;
simplyellow 0:8038ea3ee241 79 } else {
simplyellow 0:8038ea3ee241 80 wait(0.0001);
simplyellow 0:8038ea3ee241 81 }
simplyellow 0:8038ea3ee241 82 }
simplyellow 0:8038ea3ee241 83 }
simplyellow 0:8038ea3ee241 84
simplyellow 0:8038ea3ee241 85 void Tracker::calcDisplacement() {
simplyellow 0:8038ea3ee241 86 pulseCount = (float) wheel->getPulses();
simplyellow 0:8038ea3ee241 87 distance = pulseCount*circumference/encoding/constant;
simplyellow 0:8038ea3ee241 88
simplyellow 0:8038ea3ee241 89 W = sqrt(df*df+db*db-2*df*db*cos(PI-turnAngle*PI/180));
simplyellow 0:8038ea3ee241 90 R = W/(2*sin(0.5*turnAngle*PI/180));
simplyellow 0:8038ea3ee241 91 dispAngle = distance/R;
simplyellow 0:8038ea3ee241 92
simplyellow 0:8038ea3ee241 93 if (dir == STRAIGHT) {
simplyellow 0:8038ea3ee241 94 x = 0;
simplyellow 0:8038ea3ee241 95 y = distance;
simplyellow 0:8038ea3ee241 96 } else if (dir == CCW) {
simplyellow 0:8038ea3ee241 97 x = -1*(R*(1-cos(dispAngle)));
simplyellow 0:8038ea3ee241 98 y = R*sin(dispAngle);
simplyellow 0:8038ea3ee241 99 } else if (dir == CW) {
simplyellow 0:8038ea3ee241 100 x = R*(1-cos(dispAngle));
simplyellow 0:8038ea3ee241 101 y = R*sin(dispAngle);
simplyellow 0:8038ea3ee241 102 } else {
simplyellow 0:8038ea3ee241 103 printf("Direction not set. TERMINATED. \n\r");
simplyellow 0:8038ea3ee241 104 while(true) {}
simplyellow 0:8038ea3ee241 105 }
simplyellow 0:8038ea3ee241 106
simplyellow 0:8038ea3ee241 107 wait(.0001);
simplyellow 0:8038ea3ee241 108 printf("----------------------\n\r");
simplyellow 0:8038ea3ee241 109 printf("x: %f\n\r", x);
simplyellow 0:8038ea3ee241 110 printf("y: %f\n\r", y);
simplyellow 0:8038ea3ee241 111 printf("distance: %f\n\r", distance);
simplyellow 0:8038ea3ee241 112 printf("turnAngle: %f\n\r", turnAngle);
simplyellow 0:8038ea3ee241 113 printf("dispAngle: %f\n\r", dispAngle);
simplyellow 0:8038ea3ee241 114 printf("direction: %d\n\r", dir);
simplyellow 0:8038ea3ee241 115 printf("W: %f\n\r", W);
simplyellow 0:8038ea3ee241 116 printf("R: %f\n\r", R);
simplyellow 0:8038ea3ee241 117 }
simplyellow 0:8038ea3ee241 118
simplyellow 0:8038ea3ee241 119