Diff: motors.cpp

Revision:

4:dc77a25f29de

Parent:

2:c6986ee3c7c5

--- a/motors.cpp	Tue Mar 15 00:58:09 2016 +0000
+++ b/motors.cpp	Mon Jun 20 13:35:06 2016 +0000
@@ -5,9 +5,9 @@
  * (C) Dept. Electronics & Computer Science, University of York
  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
  *
- * PsiSwarm Library Version: 0.41
+ * PsiSwarm Library Version: 0.5
  *
- * March 2016
+ * April 2016
  *
  *
  */
@@ -17,6 +17,14 @@
 Timeout time_based_action_timeout;
 char brake_when_done = 0;
 
+void set_motor_speed(float left_motor_speed, float right_motor_speed)
+{
+    motor_left_speed = left_motor_speed;
+    motor_right_speed = right_motor_speed;
+    motor_left_brake = 0;
+    motor_right_brake = 0;
+    IF_update_motors();
+}
 void set_left_motor_speed(float speed)
 {
     motor_left_speed = speed;
@@ -229,11 +237,11 @@
     } else {
         if(motor_left_speed >= 0) {
             motor_left_f.write(0);
-            motor_left_r.write(IF_calibrated_speed(motor_left_speed));
+            motor_left_r.write(IF_calibrated_left_speed(motor_left_speed));
 
         } else {
             motor_left_r.write(0);
-            motor_left_f.write(IF_calibrated_speed(-motor_left_speed));
+            motor_left_f.write(IF_calibrated_left_speed(-motor_left_speed));
         }
     }
     if(motor_right_brake) {
@@ -246,16 +254,36 @@
     } else {
         if(motor_right_speed >= 0) {
             motor_right_f.write(0);
-            motor_right_r.write(IF_calibrated_speed(motor_right_speed));
+            motor_right_r.write(IF_calibrated_right_speed(motor_right_speed));
         } else {
             motor_right_r.write(0);
-            motor_right_f.write(IF_calibrated_speed(-motor_right_speed));
+            motor_right_f.write(IF_calibrated_right_speed(-motor_right_speed));
         }
     }
 
 }
 
 
+float IF_calibrated_left_speed(float speed)
+{
+    //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
+    float adjusted  = IF_calibrated_speed(speed);
+    if(use_motor_calibration){
+        adjusted *= left_motor_calibration_value;
+    }
+    return adjusted;
+}
+
+float IF_calibrated_right_speed(float speed)
+{
+    //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
+    float adjusted  = IF_calibrated_speed(speed);
+    if(use_motor_calibration){
+        adjusted *= right_motor_calibration_value;
+    }
+    return adjusted;
+}
+
 float IF_calibrated_speed(float speed)
 {
     if(speed == 0) return 0;