Diff: robot.cpp

Revision:

5:74c8ce39334b

Parent:

4:adc885f4ab75

Child:

6:62d498ee97cf

diff -r adc885f4ab75 -r 74c8ce39334b robot.cpp
--- a/robot.cpp	Sun Nov 17 21:04:47 2013 +0000
+++ b/robot.cpp	Sun Nov 17 21:48:30 2013 +0000
@@ -13,7 +13,7 @@
     ifac=0.00000001;
     dfac=0.000001;
     
-    angfac=0.0000001;
+    angfac=0.0000016;
     //blah
 }
 
@@ -23,7 +23,7 @@
     const int* constbuf = bigenc.getVals();
     int prev[4]={0,0,0,0};
     int distTraveled=0, i;
-    double maxPow;
+    double maxPow=0.5;
     int loopcount=0;
     double minmain=0.05;
     double minalt=0.05;
@@ -37,6 +37,7 @@
     if(distance<=0) {
         invfactor = 2025000;
     }
+    double realfac=0;
     
     int pmain=distance;
     int imain=0;
@@ -48,7 +49,7 @@
     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)){
+    while(/*(pterm+pmain <= -ptol || pterm >= ptol) || (dterm <= -dtol || dterm >= dtol) || */(pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol)|| (realfac <= -0.05 || realfac >= 0.05) ){
         //maxPow=MAX(double(distance-distTraveled-2000)/15000.0,minspeed);
         //left.setPower(MIN(power,maxPow));
         //right.setPower(MIN(power,maxPow));
@@ -58,24 +59,25 @@
         //wait(0.05);
         constbuf = bigenc.getVals();
         
-        pmain = distance - constbuf[0];
-        dmain = constbuf[0] - prev[0];
+        pmain = distance - (constbuf[0]+constbuf[1])/2;
+        dmain = ((constbuf[0]+constbuf[1]) - (prev[0]+prev[1]))/2;
         imain += pmain;
         
-        pterm = distance - constbuf[1];
-        dterm = constbuf[1] - prev[1];
-        iterm += pterm;
+        //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);
+        //DBGPRINT("%d like %f\n\r", gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159)),angle*180/3.14159);
+        
+        realfac = (gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159)))*angfac;
         
-        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)){
+        double leftpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain,maxPow),-maxPow)-realfac;
+        double rightpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain,maxPow),-maxPow)+realfac;
+        if((pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol) || (realfac <= -0.05 || realfac >= 0.05) ){
             if (leftpow>0){
                 if (leftpow<minalt){
                     leftpow=minalt;
@@ -83,9 +85,6 @@
             }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;
@@ -103,19 +102,25 @@
         distTraveled+=deltaTraveled;
         loopcount++;
         if((dmain<5&&dmain>-5)&&minmain<0.2){
-            minmain+=0.001;
+            minmain+=0.003;
         } else if (minmain>=0.05) {
-            minmain-=0.001;
+            minmain-=0.003;
         }
         if((dterm<5&&dterm>-5)&&minalt<0.2){
-            minalt+=0.001;
+            minalt+=0.003;
         } else if (minalt>=0.05) {
-            minalt-=0.001;
+            minalt-=0.003;
         }
     }
     DBGPRINT("Loops: %d\r\n",loopcount);
     left.brake();
     right.brake();
+    wait(0.5);
+    for(i=0;i<4;i++)
+        prev[i]=constbuf[i];
+    //wait(0.05);
+    constbuf = bigenc.getVals();
+    DBGPRINT("loss of %d and %d\n\r",constbuf[0]-prev[0],constbuf[1]-prev[1]);
     return 0;
 }