Diff: PsiSwarm/motors.cpp

Revision:

9:085e090e1ec1

Parent:

8:00558287a4ef

Child:

13:f5994956b1ba

--- a/PsiSwarm/motors.cpp	Thu Oct 22 15:36:16 2015 +0000
+++ b/PsiSwarm/motors.cpp	Mon Oct 26 11:16:05 2015 +0000
@@ -14,6 +14,7 @@
 #include "psiswarm.h"
 
 Timeout time_based_action_timeout;
+char brake_when_done = 0;
 
 void set_left_motor_speed(float speed)
 {
@@ -89,52 +90,108 @@
     IF_update_motors();
 }
 
-void time_based_turn(float speed, int microseconds)
+//Forward for a set period of time
+void time_based_forward(float speed, int microseconds, char brake)
 {
+    //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
+    IF_check_time_for_existing_time_based_action();
+        
+    //Start moving
+    forward(speed);
+    brake_when_done = brake;
+    time_based_action_timeout.attach_us(&IF_end_time_based_action,microseconds);
+}
+
+//Turn for a set period of time
+void time_based_turn(float speed, int microseconds, char brake)
+{
+    //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
+    IF_check_time_for_existing_time_based_action();
+        
+    //Start turning
+    turn(speed);
+    brake_when_done = brake;
+    time_based_action_timeout.attach_us(&IF_end_time_based_action,microseconds);
 }
 
-void time_based_turn_degrees(float speed, float degrees)
+//Return the time in microseconds that performing the turn will take
+int get_time_based_turn_time(float speed, float degrees)
+{
+    //Check sign of degrees
+    if(degrees < 0) degrees =- degrees;
+     
+    //Main calculation for turn time
+    float turn_time = degrees / ((290 * speed));
+
+    //Add a hard offset of 4ms to account for start\stop time
+    if(degrees > 4) {
+       turn_time += 0.004;
+    } else turn_time += 0.002;
+
+    // Add offset for slow speed
+    if(speed<0.31) {
+        float mul_fact = 0.31 - speed;
+        if(mul_fact < 0) mul_fact = 0;
+        mul_fact /= 2;
+        mul_fact += 1;
+        turn_time *= mul_fact;
+    }
+
+    // Add offset for short turns
+    if(degrees < 360) {
+        float short_offset_multiplier = 1.0 + (0.9 / degrees);
+        turn_time *= short_offset_multiplier;
+    }
+    
+    // Convert to uS 
+    turn_time *= 1000000;
+    
+    return (int) turn_time;   
+}
+
+//Turn the robot a set number of degrees [using time estimation to end turn]
+int time_based_turn_degrees(float speed, float degrees, char brake)
 {
     if(speed < 0 || speed > 1 || degrees == 0) {
-    debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees);
+        debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees);
+        return 0;
     } else {
-        char invert = 0;
-        if(degrees < 0) {degrees=-degrees; invert = 1;}
-        
-        //Main calculation for turn time
-        float turn_time = degrees / ((290 * speed));
+        //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
+        IF_check_time_for_existing_time_based_action();
         
-        //Add a hard offset of 4ms to account for start\stop time
-        if(degrees > 4) {
-            turn_time += 0.004;  
-        } else turn_time += 0.002;
+        //Calculate turn time using get_time_based_turn_time
+        int turn_time = get_time_based_turn_time(speed,degrees);
         
-        // Add offset for slow speed
-        if(speed<0.31) {
-            float mul_fact = 0.31 - speed;
-            if(mul_fact < 0) mul_fact = 0;
-            mul_fact /= 2;
-            mul_fact += 1;
-            turn_time *= mul_fact;
+        //Set correct turn direction (-degrees is a counter-clockwise turn)
+       if(degrees < 0) {
+            degrees=-degrees;
+            speed=-speed;
         }
 
-        // Add offset for short turns
-        if(degrees < 360) {
-            float short_offset_multiplier = 1.0 + (0.9 / degrees);
-            turn_time *= short_offset_multiplier;
-        }
+        //Start turning
+        turn(speed);
         
-        //pc.printf("Speed: %f   Turn time: %f\n",speed,turn_time);
-        if(invert) speed=-speed;
-        turn(speed);
-        time_based_action_timeout.attach(&IF_end_time_based_action,turn_time);
-}
+        brake_when_done = brake;
+        time_based_action_timeout.attach_us(&IF_end_time_based_action,turn_time);
+        return turn_time;
+    }
 }
 
+//Check if a current time based action is running - if it is, throw a warning and cancel its timeout
+void IF_check_time_for_existing_time_based_action()
+{
+    if(time_based_motor_action == 1){
+        time_based_action_timeout.detach(); 
+        debug("WARNING: New time-based action called before previous action finished!\n");
+    }   
+    else time_based_motor_action = 1;
+}
 
 void IF_end_time_based_action()
 {
-        brake();
+    if(brake_when_done == 1)brake();
+    else stop();
+    time_based_motor_action = 0;
 }
 
 void IF_update_motors()