Diff: robot.cpp

Revision:

12:925f52da3ba9

Parent:

11:967469d7e01c

Child:

13:c2d14bf733a5

diff -r 967469d7e01c -r 925f52da3ba9 robot.cpp
--- a/robot.cpp	Wed Feb 12 06:00:07 2014 +0000
+++ b/robot.cpp	Wed Feb 12 06:41:29 2014 +0000
@@ -28,8 +28,9 @@
 int robot::absDriveForward(double desangle, int distance){
     Timer tim;
     int dir=1;
+    // if we are traveling forward a negative distance, go backwards
     if(distance < 0){
-        dir = -1;
+        dir = -1; //is multiplied by the final output
         distance = -distance;
     }
     int i,move;
@@ -39,6 +40,8 @@
     int distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
     int stopRaise,startFall;
     if(distance<distToMax*2){
+        // can't get up to full speed before needing to slow down
+        //find new max speed, time to get there, and distance there
         maxSpeed =  ((2*stepsPerInc)+sqrt((double)(2*stepsPerInc)*((2*stepsPerInc)+8*distance)))/(4*stepsPerInc);
         stepsToMax=(maxSpeed-1)*stepsPerInc;
         distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
@@ -46,33 +49,45 @@
         startFall = stopRaise+(distance-distToMax*2)/(maxSpeed)+1;
         DBGPRINT("Insufficent Ramp-Up\r\n",1);
     } else {
+        // can get up to full speed, so we will
         stopRaise=stepsToMax;
-        startFall=stopRaise+(distance-distToMax*2)/maxSpeed;
+        startFall=stopRaise+(distance-distToMax*2)/maxSpeed+1;
         DBGPRINT("Sufficent Ramp-Up\r\n",1);
     }
     DBGPRINT("[%d %d %d] {%d, %d}\n\r",maxSpeed,stepsToMax,distToMax,stopRaise,startFall);
     tim.start();
     move = 0;
     int totalMove=0;
+    gyro.stop();
     for(i=0;i<startFall+stepsToMax;i++){
+        //start clock for this frame
         tim.reset();
         if(i<=stopRaise && i%stepsPerInc==0){
+            //increase speed every (stepsPerInc) steps up to maxSpeed
             move++;
         }
         if(i==startFall-1){
+            //calibration step before we start falling to get an exact result
             move = distance-totalMove-distToMax;
         }
         if(i>=startFall && (i-startFall)%stepsPerInc==0){
-            if(i==startFall)
+            if(i==startFall) //reset move after calibration step
                 move=maxSpeed;
+            
+            //decrement every stepsPerInc steps until stopped
             move--;
         }
         totalMove+=move;
         motors.moveForward(move*dir);
         //DBGPRINT("%d: %d\t%d\r\n",i,move,totalMove);
+        while(tim.read_ms()<5);
+        gyro.gyroUpkeep();
+        addforward(double(move*dir)*0.0035362);
+        //waits until 10ms have passed for the 100hz timing
         while(tim.read_ms()<10);
     }
-    return totalMove;
+    gyro.start();
+    return totalMove*dir;
 }
 
 //this is the main thing that both turns and goes forward