Diff: robot.cpp

Revision:

6:62d498ee97cf

Parent:

5:74c8ce39334b

Child:

7:3b2cf7efe5d1

--- a/robot.cpp	Sun Nov 17 21:48:30 2013 +0000
+++ b/robot.cpp	Mon Nov 18 23:45:12 2013 +0000
@@ -23,7 +23,7 @@
     const int* constbuf = bigenc.getVals();
     int prev[4]={0,0,0,0};
     int distTraveled=0, i;
-    double maxPow=0.5;
+    double maxPow=0.4;
     int loopcount=0;
     double minmain=0.05;
     double minalt=0.05;
@@ -31,13 +31,13 @@
     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;
+    double targx = x + double((distance==0)?10000:distance)*0.0035362*cos(angle)*1.5;
+    double targy = y + double((distance==0)?10000:distance)*0.0035362*sin(angle)*1.5;
     int invfactor = 0;
-    if(distance<=0) {
+    if(distance<0) {
         invfactor = 2025000;
     }
-    double realfac=0;
+    double realfac=0.1;
     
     int pmain=distance;
     int imain=0;
@@ -49,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)|| (realfac <= -0.05 || realfac >= 0.05) ){
+    while(/*(pterm+pmain <= -ptol || pterm >= ptol) || (dterm <= -dtol || dterm >= dtol) || */(pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol)|| (realfac <= -0.03 || realfac >= 0.03) /*|| (fmod(rot*180.0/3.14159-desangle+3600000.0,360.0) >= 1 && fmod(rot*180.0/3.14159-desangle+3600000.0,360.0) <= 359)*/){
         //maxPow=MAX(double(distance-distTraveled-2000)/15000.0,minspeed);
         //left.setPower(MIN(power,maxPow));
         //right.setPower(MIN(power,maxPow));
@@ -71,9 +71,10 @@
         //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("%f like %f [%f] {%d,%d,%f,%f}\n\r", gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159))*360.0/4050000.0,angle*180/3.14159,rot,pmain,dmain,fmod(rot*180.0/3.14159-desangle+3600000.0,360.0), realfac);\
         
         realfac = (gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159)))*angfac;
+        realfac = MAX(MIN(realfac,0.3),-0.3);
         
         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;
@@ -112,14 +113,15 @@
             minalt-=0.003;
         }
     }
-    DBGPRINT("Loops: %d\r\n",loopcount);
     left.brake();
     right.brake();
-    wait(0.5);
+    DBGPRINT("Loops: %d\r\n",loopcount);
+    wait(0.2);
     for(i=0;i<4;i++)
         prev[i]=constbuf[i];
     //wait(0.05);
     constbuf = bigenc.getVals();
+    addforward(double(constbuf[0]-prev[0]+constbuf[1]-prev[1])*0.0035362/2.0);
     DBGPRINT("loss of %d and %d\n\r",constbuf[0]-prev[0],constbuf[1]-prev[1]);
     return 0;
 }