Diff: robot.cpp

Revision:

1:c28fac16a109

Child:

2:7c6b494f9005

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.cpp	Sun Nov 17 03:05:35 2013 +0000
@@ -0,0 +1,153 @@
+#include "robot.h"
+
+#define MIN(a,b) ((b<a)?(b):(a))
+#define MAX(a,b) ((b>a)?(b):(a))
+
+robot::robot() : spi(PTD2, PTD3, PTD1), bigenc(spi,PTD0), /*gyro(p9, p10),*/
+            right(bigenc,0,1,PTA4), left(bigenc,2,3,PTA5){
+    
+    bigenc.setDirections(1,-1,1,1);
+    left.setReversed(1);
+    x=y=rot=0;
+    //blah
+}
+
+
+int robot::driveForward(double power, int distance){
+    bigenc.resetAll();
+    const int* constbuf = bigenc.getVals();
+    int prev[4]={0,0,0,0};
+    int distTraveled=0, i;
+    double maxPow;
+    while(distTraveled<distance){
+        maxPow=MAX(double(distance-distTraveled-2000)/15000.0,0.1);
+        left.setPower(MIN(power,maxPow));
+        right.setPower(MIN(power,maxPow));
+        DBGPRINT("=%d of %d [%f] (%d, %d, %d, %d) \t{%f,\t%f,/t%f}\r\n",distTraveled,distance, maxPow, constbuf[0], constbuf[1], constbuf[2], constbuf[3],x,y,rot*180.0/3.14159);
+        for(i=0;i<4;i++)
+            prev[i]=constbuf[i];
+        //wait(0.05);
+        constbuf = bigenc.getVals();
+        //int deltaTraveled=MAX(MIN(MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1]),MIN(constbuf[2]-prev[2],constbuf[3]-prev[3])),0);
+        int deltaTraveled=MAX((MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1])+MIN(constbuf[2]-prev[2],constbuf[3]-prev[3]))/2,0);
+        addforward(double(deltaTraveled)*0.0035362);
+        distTraveled+=deltaTraveled;
+    }
+    left.brake();
+    right.brake();
+    return 0;
+}
+
+int robot::addforward(double dist){
+    double angle=0;//double(gyro.getZ())*2*3.14159/4050000.0;
+    x+=dist*cos(angle);
+    y+=dist*sin(angle);
+    rot=angle;
+}
+
+int robot::moveTo(double xInches, double yInches){
+    double power=.2;
+    turntowards(xInches,yInches);
+    double distance=sqrt(pow(xInches-x,2)+pow(yInches-y,2))/0.0035362;
+    double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
+    double currangle=0;//double(gyro.getZ())*2*3.14159/4050000.0;
+    bigenc.resetAll();
+    const int* constbuf = bigenc.getVals();
+    int prev[4]={0,0,0,0};
+    int distTraveled=0, i;
+    double maxPow;
+    DBGPRINT("going %f at angle %f from current of %f\r\n",distance,angle,currangle);
+    while(distTraveled<distance){
+        angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
+        currangle=0;//double(gyro.getZ())*2*3.14159/4050000.0;
+        maxPow=MAX(double(distance-distTraveled-2000)/15000.0,0.1);
+        if(currangle>angle+2.0){ //too far to the right, brake left
+            left.brake();
+        } else {
+            left.setPower(MIN(power,maxPow));
+        }
+        if(currangle<angle-2){
+            right.brake();
+        } else {
+            right.setPower(MIN(power,maxPow));
+        }
+        DBGPRINT("=%d of %d [%f] (%d, %d, %d, %d) \t{%f,\t%f,\t%f}\r\n",distTraveled,distance, maxPow, constbuf[0], constbuf[1], constbuf[2], constbuf[3],x,y,rot*180.0/3.14159);
+        for(i=0;i<4;i++)
+            prev[i]=constbuf[i];
+        //wait(0.05);
+        constbuf = bigenc.getVals();
+        int deltaTraveled=MAX((MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1])+MIN(constbuf[2]-prev[2],constbuf[3]-prev[3]))/2,0);
+        addforward(double(deltaTraveled)*0.0035362);
+        distTraveled+=deltaTraveled;
+        //angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
+    }
+    left.brake();
+    right.brake();
+    return 0;
+    
+}
+
+int robot::turntowards(double xInches, double yInches){
+    double currangle=0;//double(gyro.getZ())*2*3.14159/4050000.0;
+    double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
+    double finangle=angle;
+    /*if(int(currangle-angle)%360>180){ //needs to turn positive degrees
+        finangle=currangle+double(int(angle-currangle)%360);
+    } else {//negative degrees
+        finangle=currangle-double(int(currangle-angle)%360);
+    }*/
+    double acc=turn(0.4,finangle);
+    if(acc<-0.75 && acc>0.75){
+        acc=turn(0.3,finangle);
+    }
+        
+    
+    return 1;
+}
+    
+
+double robot::turn(double power, double degrees){
+    bigenc.resetAll();
+    const int* constbuf = bigenc.getVals();
+    int prev[4]={0,0,0,0};
+    int startz;
+    startz=0;//gyro.getZ();
+    int gyroticks=(degrees*4050000)/360;
+    double maxPow;
+    int nowz=startz;
+    int dir=0;
+    if(gyroticks>startz){
+        right.setPower(-power);
+        left.setPower(power);
+        dir=1;
+    } else {
+        right.setPower(power);
+        left.setPower(-power);
+        dir=-1;
+    }
+    while((gyroticks-nowz)*dir>0){
+        maxPow=MAX(double(abs(gyroticks-nowz))/4050000.0,0.25);
+        if(gyroticks<nowz){
+            right.setPower(-MIN(power,maxPow));
+            left.setPower(MIN(power,maxPow));
+        } else {
+            right.setPower(MIN(power,maxPow));
+            left.setPower(-MIN(power,maxPow));
+        }
+        DBGPRINT("_%d of %d {%f, %f, %f}\r\n",(gyroticks-nowz)*dir, gyroticks,x,y,rot*180.0/3.14159);
+        for(int i=0;i<4;i++)
+            prev[i]=constbuf[i];
+        constbuf = bigenc.getVals();
+        int deltaTraveled;
+        if(gyroticks<nowz){
+            deltaTraveled=(MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1])-MIN(-constbuf[2]+prev[2],-constbuf[3]+prev[3]))/2;
+        } else {
+            deltaTraveled=(-MIN(constbuf[0]-prev[0],constbuf[1]-prev[1])+MIN(constbuf[2]-prev[2],constbuf[3]-prev[3]))/2;
+        }
+        addforward(double(deltaTraveled)*0.0035362);
+        nowz=0;//gyro.getZ();
+    }
+    right.brake();
+    left.brake();
+    return (gyroticks-nowz)*dir;
+}
\ No newline at end of file