File content as of revision 1:17db90f61f6c:

#include "QEI.h"
#include "mbed.h"
#include "Motor.h"

Serial pc(USBTX, USBRX);

QEI wheel (p29, p30, NC, 128, QEI::X4_ENCODING);
//orange, yellow wires, VU 5 VOLTS, BOTTOM 2x2 pins on WW02

float pulseCount;
float encoding = 2;
float circumference = 13.25;
float constant = 1360;              // constant = 20*68

float distance;

float x;
float y;

AnalogIn pot(p15);
float value;
float turnAngle;
float zeroAngle;

float PI = 3.14159265;

#define STRAIGHT 0
#define CCW 1
#define CW 2

int dir = 3;

float df = 4.50;                    // distance from pivot to front wheel
float db = 4.25;                    // distance from pivot to back wheel
float W;
float R;
float dispAngle;

#define Q1 1
#define Q2 2
#define Q3 3
#define Q4 4

int quadrant = 0;

void clear() {
    distance = 0;
    x = 0;
    y = 0;
}

void potSetup() {
    zeroAngle = (float) pot;
    for(int i = 5; i > 0; i--) {
        pc.printf("%d seconds to set turning angle.\n\r", i);
        wait(1);
    }
    turnAngle = ((float) pot - zeroAngle) * 300;
    pc.printf("---------------------------------\n\r");
    //debug
    /*
    for(int i = 5; i > 0; i--) {
        pc.printf("%f is the turning Angle.\n\r", turnAngle);
        pc.printf("%f is the zero value.\n\r", zeroAngle);
        pc.printf("%f is the pot value.\n\r", (float) pot);
        wait(1);
    }*/
}

void setDirection() {
    if (turnAngle > 0.5) {          // +/- 0.5 leeway
        pc.printf("DIRECTION SET TO CCW with turnAngle %f\n\r", turnAngle);
        dir = CCW;
        pc.printf("%d is direction\n\r", dir);
    } else if (turnAngle < -0.5) {
        pc.printf("DIRECTION SET TO CW with turnAngle %f\n\r", turnAngle);
        dir = CW;
    } else {
        pc.printf("DIRECTION SET TO STRAIGHT with turnAngle %f\n\r", turnAngle);
        dir = STRAIGHT;
    }
    turnAngle = abs(turnAngle);
}

void setQuadrant() {
    if ((turnAngle > 0) && (turnAngle < 90)) {
        quadrant = Q1;
    } else if ((turnAngle > 90) && (turnAngle < 180)) {
        quadrant = Q2;
    } else if ((turnAngle > 180) && (turnAngle < 270)) {
        quadrant = Q3;
    } else if ((turnAngle > 270) && (turnAngle < 360)) {
        quadrant = Q4;
    } else if (turnAngle == 0) {
        //don't worry
    } else {
        pc.printf("turnAngle not valid. TERMINATED. \n\r");
        while(true) {}
    }
}

void 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);
        }
    }
}
/*
AXES
when robot is facing forward
forward is +x
left is +y
*/


void 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);
        /*
        switch(quadrant) {
            case Q1:
                x = R*(cos(dispAngle)-1);
                y = R*sin(dispAngle);
                break;
            case Q2:
                x = -1*R*sin(dispAngle);
                y = R*(cos(dispAngle)-1);
                break;
            case Q3:
                x = R*(cos(dispAngle)-1);
                y = -1*R*sin(dispAngle);
                break;
            case Q4:
                x = R*sin(dispAngle);
                y = R*(1-cos(dispAngle));
                break;
            default:
                pc.printf("default \n\r");
        }*/
    } else if (dir == CW) {
        x = R*(1-cos(dispAngle));
        y = R*sin(dispAngle);
        /*
        switch(quadrant) {
            case Q1:
                x = R*sin(dispAngle);
                y = R*(cos(dispAngle)-1);
                break;
            case Q2:
                x = R*(cos(dispAngle)-1);
                y = R*sin(dispAngle);
                break;
            case Q3:
                x = -1*R*sin(dispAngle);
                y = R*(1-cos(dispAngle));
                break;
            case Q4:
                x = R*(cos(dispAngle)-1);
                y = -1*R*sin(dispAngle);
                break;
            default:
                pc.printf("default \n\r");
        }*/
    } else {
        pc.printf("Direction not set. TERMINATED. \n\r");
        while(true) {}
    }

    wait(.0001);
}

int main() {
    clear();
    potSetup();
    setDirection();
    setQuadrant();
    checkToStart();
    while(true) {
        calcDisplacement();
        pc.printf("----------------------\n\r");
        pc.printf("x: %f\n\r", x);
        pc.printf("y: %f\n\r", y);
        pc.printf("distance: %f\n\r", distance);
        pc.printf("turnAngle: %f\n\r", turnAngle);
        pc.printf("dispAngle: %f\n\r", dispAngle);
        pc.printf("direction: %d\n\r", dir);
        pc.printf("quadrant: %d\n\r", quadrant);
        pc.printf("W: %f\n\r", W);
        pc.printf("R: %f\n\r", R);
    }
}