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@0:8038ea3ee241, 2017-01-26 (annotated)

Committer:
simplyellow
Date:
Thu Jan 26 20:06:55 2017 +0000
Revision:
0:8038ea3ee241
Child:
1:008116b6eb46
official library and example code for tracking the X and Y positions of the dump truck

Who changed what in which revision?

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