Diff: robot.cpp

Revision:

8:c23cd84befa2

Parent:

7:3b2cf7efe5d1

Child:

9:aff48e331147

--- a/robot.cpp	Fri Nov 22 18:00:18 2013 +0000
+++ b/robot.cpp	Fri Nov 22 21:28:52 2013 +0000
@@ -21,6 +21,8 @@
 }
 
 //this is the main thing that both turns and goes forward
+// desangle is a angle in degrees to head towards (this is relative to the direction the robot starts pointing in
+// distance is a distance to head in that direction in units of encoder ticks
 int robot::driveForward(double desangle, int distance){
     bigenc.resetAll();
     const int* constbuf = bigenc.getVals();
@@ -48,17 +50,12 @@
     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)|| (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));
+    while((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)*/){
+
         //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];
@@ -70,14 +67,6 @@
         dmain = ((constbuf[0]+constbuf[1]) - (prev[0]+prev[1]))/2;
         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("%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);\
         
         //finds the difference between the angle to the imaginary point we're headed to and the current angle and turns it into a power level
@@ -108,7 +97,6 @@
         }
         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);
         
         //how far we've moved in the last timestep
         int deltaTraveled=(constbuf[0]-prev[0]+constbuf[1]-prev[1])/2;