Diff: robot.cpp

Revision:

10:926f142f16a3

Parent:

9:aff48e331147

Child:

11:967469d7e01c

--- a/robot.cpp	Mon Feb 03 20:37:35 2014 +0000
+++ b/robot.cpp	Wed Feb 12 05:38:52 2014 +0000
@@ -20,6 +20,52 @@
     angfac=0.0000016;
 }
 
+//driveforward, but set up so that 
+int robot::absDriveForward(double desangle, int distance){
+    Timer tim;
+    int i,move;
+    int maxSpeed=41;
+    int stepsPerInc=3;
+    int stepsToMax=(maxSpeed-1)*stepsPerInc;
+    int distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
+    int stopRaise,startFall;
+    if(distance<distToMax*2){
+        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 {
+        stopRaise=stepsToMax;
+        startFall=stopRaise+(distance-distToMax*2)/maxSpeed;
+        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;
+    for(i=0;i<startFall+stepsToMax;i++){
+        tim.reset();
+        if(i<=stopRaise && i%stepsPerInc==0){
+            move++;
+        }
+        if(i==startFall-1){
+            move = distance-totalMove-distToMax;
+        }
+        if(i>=startFall && (i-startFall)%stepsPerInc==0){
+            if(i==startFall)
+                move=maxSpeed;
+            move--;
+        }
+        totalMove+=move;
+        motors.moveForward(move);
+        DBGPRINT("%d: %d\t%d\r\n",i,move,totalMove);
+        while(tim.read_ms()<10);
+    }
+    return totalMove;
+}
+
 //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