Terrabots
/
Tracker2
Made distance public
Fork of Tracker by
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?
User | Revision | Line number | New 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 |