Diff: robot.cpp

Revision:

13:c2d14bf733a5

Parent:

12:925f52da3ba9

Child:

14:a30aa3b29a2e

--- a/robot.cpp	Wed Feb 12 06:41:29 2014 +0000
+++ b/robot.cpp	Thu Feb 27 00:38:13 2014 +0000
@@ -26,7 +26,8 @@
 
 //driveforward, but set up so that 
 int robot::absDriveForward(double desangle, int distance){
-    Timer tim;
+    return smoothMove(distance, 1, 5);
+    /*Timer tim;
     int dir=1;
     // if we are traveling forward a negative distance, go backwards
     if(distance < 0){
@@ -35,7 +36,7 @@
     }
     int i,move;
     int maxSpeed=41;
-    int stepsPerInc=3;
+    int stepsPerInc=2;
     int stepsToMax=(maxSpeed-1)*stepsPerInc;
     int distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
     int stopRaise,startFall;
@@ -88,6 +89,77 @@
     }
     gyro.start();
     return totalMove*dir;
+    */
+}
+
+int robot::smoothMove(int distance, int rotate, int maxSpeed){
+    Timer tim;
+    int dir=1;
+    // if we are traveling forward a negative distance, go backwards
+    if(distance < 0){
+        dir = -1; //is multiplied by the final output
+        distance = -distance;
+    }
+    if(rotate)
+        rotate=-1;
+    else 
+        rotate=1;
+    int i,move;
+    //int maxSpeed=41;
+    int stepsPerInc=2;
+    int stepsToMax=(maxSpeed-1)*stepsPerInc;
+    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;
+        stopRaise = stepsToMax;
+        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+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) //reset move after calibration step
+                move=maxSpeed;
+            
+            //decrement every stepsPerInc steps until stopped
+            move--;
+        }
+        totalMove+=move;
+        //motors.moveForward(move*dir);
+        motors.moveWheels(move*dir,move*dir*rotate);
+        //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);
+    }
+    gyro.start();
+    return totalMove*dir;
 }
 
 //this is the main thing that both turns and goes forward