Terrabots / Mbed 2 deprecated Tracker2

Made distance public

Dependencies:   QEI mbed

Fork of Tracker by Terrabots

Tracker.cpp@7:58157608cfbe, 2017-04-23 (annotated)

Committer:
jcallahan1
Date:
Sun Apr 23 18:57:31 2017 +0000
Revision:
7:58157608cfbe
Parent:
3:eb8680da1721
Child:
4:c38c00a6588c
Made "distance" public

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 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