Terrabots
/
Tracker2
Made distance public
Fork of Tracker by
Tracker.cpp
- Committer:
- simplyellow
- Date:
- 2017-01-26
- Revision:
- 1:008116b6eb46
- Parent:
- 0:8038ea3ee241
- Child:
- 3:eb8680da1721
File content as of revision 1:008116b6eb46:
#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 } /* // Example Code #include "mbed.h" #include "Tracker.h" Tracker foo(p29, p30, p15); int main() { foo.clear(); foo.potSetup(); foo.setDirection(); foo.checkToStart(); while(true) { foo.calcDisplacement(); } } */ /*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); }