Diff: robot.cpp

Revision:

4:adc885f4ab75

Parent:

3:a223b0bf8256

Child:

5:74c8ce39334b

--- a/robot.cpp	Sun Nov 17 04:15:18 2013 +0000
+++ b/robot.cpp	Sun Nov 17 21:04:47 2013 +0000
@@ -9,37 +9,118 @@
     bigenc.setDirections(1,-1,1,1);
     left.setReversed(1);
     x=y=rot=0;
+    pfac=0.00035;
+    ifac=0.00000001;
+    dfac=0.000001;
+    
+    angfac=0.0000001;
     //blah
 }
 
 
-int robot::driveForward(double power, int distance){
+int robot::driveForward(double desangle, 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);
+    int loopcount=0;
+    double minmain=0.05;
+    double minalt=0.05;
+    
+    int targetang = desangle*4050000.0/360.0;//gyro.getZ();
+    int startang = targetang;
+    double angle=double(startang)*2*3.14159/4050000.0;
+    double targx = x + double(distance)*0.0035362*cos(angle)*1.5;
+    double targy = y + double(distance)*0.0035362*sin(angle)*1.5;
+    int invfactor = 0;
+    if(distance<=0) {
+        invfactor = 2025000;
+    }
+    
+    int pmain=distance;
+    int imain=0;
+    int dmain=0;
+    int plast=distance;
+    
+    int pterm=distance;
+    int iterm=0;
+    int dterm=0;
+    const int ptol = 75;
+    const int dtol = 10;
+    while((pterm+pmain <= -ptol || pterm >= ptol) || (dterm <= -dtol || dterm >= dtol) || (pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol)){
+        //maxPow=MAX(double(distance-distTraveled-2000)/15000.0,minspeed);
+        //left.setPower(MIN(power,maxPow));
+        //right.setPower(MIN(power,maxPow));
+        //DBGPRINT("=%d of %d [%f] (%d, %d) \t{%f,\t%f,/t%f}",distTraveled,distance, maxPow, constbuf[0], constbuf[1],x,y,rot*180.0/3.14159);
         for(i=0;i<4;i++)
             prev[i]=constbuf[i];
         //wait(0.05);
         constbuf = bigenc.getVals();
+        
+        pmain = distance - constbuf[0];
+        dmain = constbuf[0] - prev[0];
+        imain += pmain;
+        
+        pterm = distance - constbuf[1];
+        dterm = constbuf[1] - prev[1];
+        iterm += pterm;
+        
+        //pterm = constbuf[0] - constbuf[1];
+        //dterm = pterm - (prev[0] - prev[1]);
+        //iterm += pterm;
+        
+        DBGPRINT("%d like %f\n\r", gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159)),angle*180/3.14159);
+        
+        double realfac = (gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159)))*angfac;
+        
+        double leftpow = MAX(MIN(pfac*pterm+ifac*iterm+dfac*dterm-realfac,0.6),-0.6);
+        if((pterm <= -ptol || pterm >= ptol) || (dterm <= -dtol || dterm >= dtol)){
+            if (leftpow>0){
+                if (leftpow<minalt){
+                    leftpow=minalt;
+                }
+            }else if (leftpow>-minalt){
+                leftpow=-minalt;
+            }
+        }
+        double rightpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain+realfac,0.6),-0.6);
+        if((pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol)){
+            if (rightpow>0){
+                if (rightpow<minmain){
+                    rightpow=minmain;
+                }
+            }else if (rightpow>-minmain){
+                rightpow=-minmain;
+            }
+        }
+        left.setPower(leftpow);
+        right.setPower(rightpow);
         //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);
+        int deltaTraveled=(constbuf[0]-prev[0]+constbuf[1]-prev[1])/2;
+        //DBGPRINT("\t %d\r\n",deltaTraveled);
         addforward(double(deltaTraveled)*0.0035362);
         distTraveled+=deltaTraveled;
+        loopcount++;
+        if((dmain<5&&dmain>-5)&&minmain<0.2){
+            minmain+=0.001;
+        } else if (minmain>=0.05) {
+            minmain-=0.001;
+        }
+        if((dterm<5&&dterm>-5)&&minalt<0.2){
+            minalt+=0.001;
+        } else if (minalt>=0.05) {
+            minalt-=0.001;
+        }
     }
+    DBGPRINT("Loops: %d\r\n",loopcount);
     left.brake();
     right.brake();
     return 0;
 }
 
-int robot::addforward(double dist){
-    double angle=0;//double(gyro.getZ())*2*3.14159/4050000.0;
+void robot::addforward(double dist){
+    double angle=double(gyro.getZ())*2*3.14159/4050000.0;
     x+=dist*cos(angle);
     y+=dist*sin(angle);
     rot=angle;
@@ -50,7 +131,7 @@
     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;
+    double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
     bigenc.resetAll();
     const int* constbuf = bigenc.getVals();
     int prev[4]={0,0,0,0};
@@ -59,7 +140,7 @@
     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;
+        currangle=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();
@@ -88,7 +169,7 @@
 }
 
 int robot::turntowards(double xInches, double yInches){
-    double currangle=0;//double(gyro.getZ())*2*3.14159/4050000.0;
+    double currangle=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
@@ -111,7 +192,7 @@
     const int* constbuf = bigenc.getVals();
     int prev[4]={0,0,0,0};
     int startz;
-    startz=0;//gyro.getZ();
+    startz=gyro.getZ();
     int gyroticks=(degrees*4050000)/360;
     double maxPow;
     int nowz=startz;
@@ -145,7 +226,7 @@
             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();
+        nowz=gyro.getZ();
     }
     right.brake();
     left.brake();