File content as of revision 0:d4bb648b7f2c:

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

/*
    Written by Milo Pan with the help of Jacob Callahan
*/

/*
Use X4 encoding.
QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
QEI constructor(CHANNEL A, CHANNEL B,PinName index (NC), int pulsesPerRev, Encoding encoding=X2_ENCODING) 
Use X2 encoding by default.
Encoding x4 = x4_ENCODING;
*/

/*
Measurement Instructions
in order to convert the numerical value (outputted by this program) to Inches traveled,
measurements need to be taken (i.e. have the truck travel 40" for 10 trials, average the values, and find the factor
*/

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;                                   //number of pulses detected by encoder
float encoding = 2;                                 //X2
float circumference = 13.25;                        //inches of circumference
float distance;

AnalogIn pot(p15);
float value;
float angle;
float zero;

float PI = 3.14159;

/* VERSION 1
float x = 0;
float dx;
float y = 0;
float dy;
float dispAngle;        // theta d
float r;                // r
float c;                // c, chord length

float segment;          // intermediary variable for ease of calculation

float heading;          // heading of front
*/


// VERSION 2
float df;
float db;
float circumference2;
float proportion;
float scaledDown;
float W;
float R;
int dir; //1 CCW, 2 CW, 0 straight
float heading = 0.0;
float dispAngleRad;
float dispAngleDeg;
float x;
float dx;
float y;
float dy;

float totalDistance;
int i;



void measure() {
    // raw measurements of pot, encoder
    //(pulse count / X * N) * (1 / PPI)
    pulseCount = (float) wheel.getPulses();
    /* conversion to relevant values: distance of arc traveled, angle of turn
        |   suppose the truck is a two-segment body
        /  <- angle between bottom leg and the vertical = angle of turn
    the 20 and 68 were arbitrary and determined from testing, respectively
    the distance is in inches
    */
    distance = pulseCount*circumference/encoding/20/68; 
    // with half inch correction after testing extensively
    /*if (distance > 0) {                                 
        distance-=0.5;
    } 
    */
}

/* VERSION 1
void calculateDisplacement() {
    /
        Premise:
        Suppose the arc s (distance), the distance traveled by the 
        turning truck, is defined by two connected points on 
        a circle with radius r.
    /
    
    // convert angle to radians
    angle = angle*PI/180;
    // r = s/angle, for distance of arc s         
    r = distance/(angle);
    // c = 2*r*sin(angle/2) for chord c
    c = 2*r*sin(angle/2);
    // intermediate value
    segment = (r*(1-cos(angle)));
    // calculation of angle of displacement
    dispAngle = acos(segment/c);
    // calculation of displacement
    dx = c*cos(dispAngle+heading);
    dy = c*sin(dispAngle+heading);
    x = x - r + dx;
    y = y + dy;
    wait(1.0);                                           
}
*/

void calculateDisplacement() {
    df = 4.5; // distance from pivot to front wheel
    db = 4.25; // distance from pivot to back wheel
    // W = 2*R*sin(0.5*angle);
    // W = shortest distance between wheels
    angle = angle*PI/180;
    W = sqrt(df*df+db*db-2*df*db*cos(PI-angle));
    R = W/(2*sin(0.5*angle));
    
    circumference2 = 2*PI*R; // inches
    proportion = distance/circumference2;
    scaledDown = proportion - floor(proportion);
    dispAngleRad = scaledDown*2*PI;
    dispAngleDeg = scaledDown*360;
    dx = R*cos(dispAngleRad + heading);
    dy = R*sin(dispAngleRad + heading);

    if(dir == 1) {               // CCW
        x = 0 - R + dx;
        y = dy;
    } else if (dir == 2) {                // CW
        y = dy;
        if((dispAngleDeg < 90) || (dispAngleDeg > 270)) {
        x = dx;
        } else {
            x = R - dx;
        }
    } else {
        x = 0;
        y = distance;
    }
    heading += dispAngleRad;
    wait(1.0);
}
/*
dir
and straight accuracy
*/

int main() {
    // required to set truck straight before start up to calibrate to zero
    zero = (float) pot;
    while(i<5) {
        printf("%d set angle \n\r",i);
        wait(1);
        i++;
    }
    if (totalDistance == 0) { // pot turns constant after the distance turns non zero
        value = (float) pot;
        // appropriate conversion from potentiometer float value to angle
        angle = (value-zero)*300;
        printf("%f",angle);
    }
    distance = 0;
    totalDistance = 0;
    heading = 0;
    while(1){
        measure(); 

        calculateDisplacement();
        
        totalDistance+=distance;
        if(totalDistance>0) {//@@@@@@@@@@check dir with angle vals@@@@@@@
            if (angle > 0.5) {
                dir = 1;//ccw
            } else if (angle < -0.5) {
                dir = 2;//cw
            } else {
                dir = 0;
            }
        }
        distance = 0;
        
        /*
            Notes:
            At startup:
            distance = 0
            angle =! 0
        */
        
        // for debugging
        pc.printf("Distance %f, Angle %f\n\r", totalDistance, angle*180/PI);     
        pc.printf("(X,Y): %f %f\n\r", x, y); 
        pc.printf("(DX,DY): %f %f\n\r", dx, dy);    
        pc.printf("heading: %f\n\r", heading);
        pc.printf("----------------------------\n\r");   
    }
}