Terrabots
/
Tracker2
Made distance public
Fork of Tracker by
Tracker.cpp@3:eb8680da1721, 2017-01-26 (annotated)
- Committer:
- simplyellow
- Date:
- Thu Jan 26 21:02:05 2017 +0000
- Revision:
- 3:eb8680da1721
- Parent:
- 1:008116b6eb46
- Child:
- 4:c38c00a6588c
.
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 | 3:eb8680da1721 | 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 | 1:008116b6eb46 | 15 | /* |
simplyellow | 1:008116b6eb46 | 16 | // Example Code |
simplyellow | 1:008116b6eb46 | 17 | #include "mbed.h" |
simplyellow | 1:008116b6eb46 | 18 | #include "Tracker.h" |
simplyellow | 1:008116b6eb46 | 19 | |
simplyellow | 1:008116b6eb46 | 20 | Tracker foo(p29, p30, p15); |
simplyellow | 1:008116b6eb46 | 21 | |
simplyellow | 1:008116b6eb46 | 22 | int main() { |
simplyellow | 1:008116b6eb46 | 23 | foo.clear(); |
simplyellow | 1:008116b6eb46 | 24 | foo.potSetup(); |
simplyellow | 1:008116b6eb46 | 25 | foo.setDirection(); |
simplyellow | 1:008116b6eb46 | 26 | foo.checkToStart(); |
simplyellow | 1:008116b6eb46 | 27 | while(true) { |
simplyellow | 1:008116b6eb46 | 28 | foo.calcDisplacement(); |
simplyellow | 1:008116b6eb46 | 29 | } |
simplyellow | 1:008116b6eb46 | 30 | } |
simplyellow | 1:008116b6eb46 | 31 | */ |
simplyellow | 1:008116b6eb46 | 32 | |
simplyellow | 0:8038ea3ee241 | 33 | /*Tracker::~Tracker() |
simplyellow | 0:8038ea3ee241 | 34 | { |
simplyellow | 0:8038ea3ee241 | 35 | if(wheel != NULL) { |
simplyellow | 0:8038ea3ee241 | 36 | delete wheel; |
simplyellow | 0:8038ea3ee241 | 37 | } |
simplyellow | 0:8038ea3ee241 | 38 | }*/ |
simplyellow | 0:8038ea3ee241 | 39 | |
simplyellow | 0:8038ea3ee241 | 40 | void Tracker::clear() { |
simplyellow | 0:8038ea3ee241 | 41 | distance = 0; |
simplyellow | 0:8038ea3ee241 | 42 | x = 0; |
simplyellow | 0:8038ea3ee241 | 43 | y = 0; |
simplyellow | 0:8038ea3ee241 | 44 | } |
simplyellow | 0:8038ea3ee241 | 45 | |
simplyellow | 0:8038ea3ee241 | 46 | void Tracker::potSetup() { |
simplyellow | 0:8038ea3ee241 | 47 | zeroAngle = (float) pot; |
simplyellow | 0:8038ea3ee241 | 48 | // USE A BETTER CALIBRATION PERIOD |
simplyellow | 0:8038ea3ee241 | 49 | for(int i = 5; i > 0; i--) { |
simplyellow | 0:8038ea3ee241 | 50 | printf("%d seconds to set turning angle.\n\r", i); |
simplyellow | 0:8038ea3ee241 | 51 | wait(1); |
simplyellow | 0:8038ea3ee241 | 52 | } |
simplyellow | 0:8038ea3ee241 | 53 | turnAngle = ((float) pot - zeroAngle) * 300; |
simplyellow | 0:8038ea3ee241 | 54 | } |
simplyellow | 0:8038ea3ee241 | 55 | |
simplyellow | 0:8038ea3ee241 | 56 | void Tracker::setDirection() { |
simplyellow | 0:8038ea3ee241 | 57 | if (turnAngle > 0.5) { // +/- 0.5 leeway |
simplyellow | 0:8038ea3ee241 | 58 | printf("DIRECTION SET TO CCW with turnAngle %f\n\r", turnAngle); |
simplyellow | 0:8038ea3ee241 | 59 | dir = CCW; |
simplyellow | 0:8038ea3ee241 | 60 | printf("%d is direction\n\r", dir); |
simplyellow | 0:8038ea3ee241 | 61 | } else if (turnAngle < -0.5) { |
simplyellow | 0:8038ea3ee241 | 62 | printf("DIRECTION SET TO CW with turnAngle %f\n\r", turnAngle); |
simplyellow | 0:8038ea3ee241 | 63 | dir = CW; |
simplyellow | 0:8038ea3ee241 | 64 | } else { |
simplyellow | 0:8038ea3ee241 | 65 | printf("DIRECTION SET TO STRAIGHT with turnAngle %f\n\r", turnAngle); |
simplyellow | 0:8038ea3ee241 | 66 | dir = STRAIGHT; |
simplyellow | 0:8038ea3ee241 | 67 | } |
simplyellow | 0:8038ea3ee241 | 68 | turnAngle = abs(turnAngle); |
simplyellow | 0:8038ea3ee241 | 69 | } |
simplyellow | 0:8038ea3ee241 | 70 | |
simplyellow | 0:8038ea3ee241 | 71 | void Tracker::checkToStart() { // start tracking when distance > 0 |
simplyellow | 0:8038ea3ee241 | 72 | while(true) { |
simplyellow | 0:8038ea3ee241 | 73 | pulseCount = (float) wheel->getPulses(); |
simplyellow | 0:8038ea3ee241 | 74 | distance = pulseCount*circumference/encoding/constant; |
simplyellow | 0:8038ea3ee241 | 75 | if (distance > 0) { |
simplyellow | 0:8038ea3ee241 | 76 | return; |
simplyellow | 0:8038ea3ee241 | 77 | } else { |
simplyellow | 0:8038ea3ee241 | 78 | wait(0.0001); |
simplyellow | 0:8038ea3ee241 | 79 | } |
simplyellow | 0:8038ea3ee241 | 80 | } |
simplyellow | 0:8038ea3ee241 | 81 | } |
simplyellow | 0:8038ea3ee241 | 82 | |
simplyellow | 0:8038ea3ee241 | 83 | void Tracker::calcDisplacement() { |
simplyellow | 0:8038ea3ee241 | 84 | pulseCount = (float) wheel->getPulses(); |
simplyellow | 0:8038ea3ee241 | 85 | distance = pulseCount*circumference/encoding/constant; |
simplyellow | 0:8038ea3ee241 | 86 | |
simplyellow | 0:8038ea3ee241 | 87 | W = sqrt(df*df+db*db-2*df*db*cos(PI-turnAngle*PI/180)); |
simplyellow | 0:8038ea3ee241 | 88 | R = W/(2*sin(0.5*turnAngle*PI/180)); |
simplyellow | 0:8038ea3ee241 | 89 | dispAngle = distance/R; |
simplyellow | 0:8038ea3ee241 | 90 | |
simplyellow | 0:8038ea3ee241 | 91 | if (dir == STRAIGHT) { |
simplyellow | 0:8038ea3ee241 | 92 | x = 0; |
simplyellow | 0:8038ea3ee241 | 93 | y = distance; |
simplyellow | 0:8038ea3ee241 | 94 | } else if (dir == CCW) { |
simplyellow | 0:8038ea3ee241 | 95 | x = -1*(R*(1-cos(dispAngle))); |
simplyellow | 0:8038ea3ee241 | 96 | y = R*sin(dispAngle); |
simplyellow | 0:8038ea3ee241 | 97 | } else if (dir == CW) { |
simplyellow | 0:8038ea3ee241 | 98 | x = R*(1-cos(dispAngle)); |
simplyellow | 0:8038ea3ee241 | 99 | y = R*sin(dispAngle); |
simplyellow | 0:8038ea3ee241 | 100 | } else { |
simplyellow | 0:8038ea3ee241 | 101 | printf("Direction not set. TERMINATED. \n\r"); |
simplyellow | 0:8038ea3ee241 | 102 | while(true) {} |
simplyellow | 0:8038ea3ee241 | 103 | } |
simplyellow | 0:8038ea3ee241 | 104 | |
simplyellow | 0:8038ea3ee241 | 105 | wait(.0001); |
simplyellow | 0:8038ea3ee241 | 106 | printf("----------------------\n\r"); |
simplyellow | 0:8038ea3ee241 | 107 | printf("x: %f\n\r", x); |
simplyellow | 0:8038ea3ee241 | 108 | printf("y: %f\n\r", y); |
simplyellow | 0:8038ea3ee241 | 109 | printf("distance: %f\n\r", distance); |
simplyellow | 0:8038ea3ee241 | 110 | printf("turnAngle: %f\n\r", turnAngle); |
simplyellow | 0:8038ea3ee241 | 111 | printf("dispAngle: %f\n\r", dispAngle); |
simplyellow | 0:8038ea3ee241 | 112 | printf("direction: %d\n\r", dir); |
simplyellow | 0:8038ea3ee241 | 113 | printf("W: %f\n\r", W); |
simplyellow | 0:8038ea3ee241 | 114 | printf("R: %f\n\r", R); |
simplyellow | 0:8038ea3ee241 | 115 | } |
simplyellow | 0:8038ea3ee241 | 116 | |
simplyellow | 0:8038ea3ee241 | 117 |