Diff: main.cpp

Revision:

0:d4bb648b7f2c

diff -r 000000000000 -r d4bb648b7f2c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 14 21:46:17 2016 +0000
@@ -0,0 +1,210 @@
+#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");   
+    }
+}
+
+