File content as of revision 0:8038ea3ee241:

#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);
}