Diff: PsiSwarm/motors.cpp

Revision:

8:00558287a4ef

Parent:

6:ff3c66f7372b

Child:

9:085e090e1ec1

--- a/PsiSwarm/motors.cpp	Thu Oct 22 13:28:17 2015 +0000
+++ b/PsiSwarm/motors.cpp	Thu Oct 22 15:36:16 2015 +0000
@@ -1,18 +1,20 @@
 /* University of York Robotics Laboratory PsiSwarm Library: Motor Functions Source File
- * 
+ *
  * File: motors.cpp
  *
  * (C) Dept. Electronics & Computer Science, University of York
  * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
- * 
+ *
  * PsiSwarm Library Version: 0.3
  *
  * October 2015
  *
- */ 
+ */
 
 #include "psiswarm.h"
 
+Timeout time_based_action_timeout;
+
 void set_left_motor_speed(float speed)
 {
     motor_left_speed = speed;
@@ -84,80 +86,134 @@
     motor_right_speed = -speed;
     motor_left_brake = 0;
     motor_right_brake = 0;
-    IF_update_motors();   
+    IF_update_motors();
+}
+
+void time_based_turn(float speed, int microseconds)
+{
 }
 
-void IF_update_motors(){
-    if(motor_left_speed > 1.0){
+void time_based_turn_degrees(float speed, float degrees)
+{
+    if(speed < 0 || speed > 1 || degrees == 0) {
+    debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees);
+    } else {
+        char invert = 0;
+        if(degrees < 0) {degrees=-degrees; invert = 1;}
+        
+        //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;
+        }
+        
+        //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);
+}
+}
+
+
+void IF_end_time_based_action()
+{
+        brake();
+}
+
+void IF_update_motors()
+{
+    if(motor_left_speed > 1.0) {
         motor_left_speed = 1.0;
-        //Throw exception...   
-    }
-    if(motor_right_speed > 1.0){
-        motor_right_speed = 1.0;
-        //Throw exception...   
-    }
-    if(motor_left_speed < -1.0){
-        motor_left_speed = -1.0;
-        //Throw exception...   
+        //Throw exception...
     }
-    if(motor_right_speed < -1.0){
-        motor_right_speed = -1.0;
-        //Throw exception...   
+    if(motor_right_speed > 1.0) {
+        motor_right_speed = 1.0;
+        //Throw exception...
+    }
+    if(motor_left_speed < -1.0) {
+        motor_left_speed = -1.0;
+        //Throw exception...
     }
-    if(motor_left_brake){
-       motor_left_f.write(1);
-       motor_left_r.write(1);
-       if(motor_left_speed!=0){
-       motor_left_speed = 0;
+    if(motor_right_speed < -1.0) {
+        motor_right_speed = -1.0;
         //Throw exception...
-       }   
-    }else{
-        if(motor_left_speed >= 0){
+    }
+    if(motor_left_brake) {
+        motor_left_f.write(1);
+        motor_left_r.write(1);
+        if(motor_left_speed!=0) {
+            motor_left_speed = 0;
+            //Throw exception...
+        }
+    } else {
+        if(motor_left_speed >= 0) {
             motor_left_f.write(0);
             motor_left_r.write(IF_calibrated_speed(motor_left_speed));
-  
-        }else{
+
+        } else {
             motor_left_r.write(0);
             motor_left_f.write(IF_calibrated_speed(-motor_left_speed));
         }
     }
-    if(motor_right_brake){
-       motor_right_f.write(1);
-       motor_right_r.write(1);
-       if(motor_right_speed!=0){
-       motor_right_speed = 0;
-        //Throw exception...
-       }   
-    }else{
-        if(motor_right_speed >= 0){
+    if(motor_right_brake) {
+        motor_right_f.write(1);
+        motor_right_r.write(1);
+        if(motor_right_speed!=0) {
+            motor_right_speed = 0;
+            //Throw exception...
+        }
+    } else {
+        if(motor_right_speed >= 0) {
             motor_right_f.write(0);
             motor_right_r.write(IF_calibrated_speed(motor_right_speed));
-        }else{
+        } else {
             motor_right_r.write(0);
             motor_right_f.write(IF_calibrated_speed(-motor_right_speed));
         }
     }
-    
+
 }
 
 
-float IF_calibrated_speed(float speed){
+float IF_calibrated_speed(float speed)
+{
     if(speed == 0) return 0;
     //Converts an input value to take account of the stall speed of the motor; aims to produce a more linear speed
     float adjusted = speed;
-    if(OFFSET_MOTORS) {adjusted*=0.8f; adjusted+=0.2;}
-    return adjusted;   
+    if(OFFSET_MOTORS) {
+        adjusted*=0.8f;
+        adjusted+=0.2;
+    }
+    return adjusted;
 }
 
-void IF_init_motors(){
+void IF_init_motors()
+{
     // Motor PWM outputs work optimally at 25kHz frequency
     motor_left_f.period_us(40);
-    motor_right_f.period_us(40);   
+    motor_right_f.period_us(40);
     motor_left_r.period_us(40);
     motor_right_r.period_us(40);
     motor_left_speed = 0;
     motor_right_speed = 0;
     motor_left_brake = 0;
-    motor_right_brake = 0;  
-    IF_update_motors();    
+    motor_right_brake = 0;
+    IF_update_motors();
 }
\ No newline at end of file