![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
official library and example code for tracking X and Y positions on the dump truck
Dependents: DUMP_TRUCK_TEST_V1 DUMP_TRUCK_SPR2017
Diff: Tracker.cpp
- Revision:
- 0:8038ea3ee241
- Child:
- 1:008116b6eb46
diff -r 000000000000 -r 8038ea3ee241 Tracker.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Tracker.cpp Thu Jan 26 20:06:55 2017 +0000 @@ -0,0 +1,99 @@ +#include "Tracker.h" + +Tracker::Tracker(PinName _chA, PinName _chB, PinName _pot) : chA(_chA), chB(_chB), pot(_pot) +{ + //chA = p29, chB = p30, pot = p15 + wheel = new QEI(_chA, _chB, NC, 128, QEI::X4_ENCODING); //orange, yellow wires, VU 5 VOLTS, BOTTOM 2x2 pins on WW02 + encoding = 2; // X2 encoding + circumference = 13.25; // circumference of wheel + constant = 1360; // obtained from tests + dir = 3; // starting dir + df = 4.50; // distance from pivot to front wheel + db = 4.25; // distance from pivot to back wheel +} + +/*Tracker::~Tracker() +{ + if(wheel != NULL) { + delete wheel; + } +}*/ + +void Tracker::clear() { + distance = 0; + x = 0; + y = 0; +} + +void Tracker::potSetup() { + zeroAngle = (float) pot; + // USE A BETTER CALIBRATION PERIOD + for(int i = 5; i > 0; i--) { + printf("%d seconds to set turning angle.\n\r", i); + wait(1); + } + turnAngle = ((float) pot - zeroAngle) * 300; +} + +void Tracker::setDirection() { + if (turnAngle > 0.5) { // +/- 0.5 leeway + printf("DIRECTION SET TO CCW with turnAngle %f\n\r", turnAngle); + dir = CCW; + printf("%d is direction\n\r", dir); + } else if (turnAngle < -0.5) { + printf("DIRECTION SET TO CW with turnAngle %f\n\r", turnAngle); + dir = CW; + } else { + printf("DIRECTION SET TO STRAIGHT with turnAngle %f\n\r", turnAngle); + dir = STRAIGHT; + } + turnAngle = abs(turnAngle); +} + +void Tracker::checkToStart() { // start tracking when distance > 0 + while(true) { + pulseCount = (float) wheel->getPulses(); + distance = pulseCount*circumference/encoding/constant; + if (distance > 0) { + return; + } else { + wait(0.0001); + } + } +} + +void Tracker::calcDisplacement() { + pulseCount = (float) wheel->getPulses(); + distance = pulseCount*circumference/encoding/constant; + + W = sqrt(df*df+db*db-2*df*db*cos(PI-turnAngle*PI/180)); + R = W/(2*sin(0.5*turnAngle*PI/180)); + dispAngle = distance/R; + + if (dir == STRAIGHT) { + x = 0; + y = distance; + } else if (dir == CCW) { + x = -1*(R*(1-cos(dispAngle))); + y = R*sin(dispAngle); + } else if (dir == CW) { + x = R*(1-cos(dispAngle)); + y = R*sin(dispAngle); + } else { + printf("Direction not set. TERMINATED. \n\r"); + while(true) {} + } + + wait(.0001); + printf("----------------------\n\r"); + printf("x: %f\n\r", x); + printf("y: %f\n\r", y); + printf("distance: %f\n\r", distance); + printf("turnAngle: %f\n\r", turnAngle); + printf("dispAngle: %f\n\r", dispAngle); + printf("direction: %d\n\r", dir); + printf("W: %f\n\r", W); + printf("R: %f\n\r", R); +} + +