Diff: motors.cpp

Revision:

4:1c621cb8cf0d

Parent:

2:c6986ee3c7c5

Child:

5:3cdd1a37cdd7

diff -r 7c0d1f581757 -r 1c621cb8cf0d motors.cpp
--- a/motors.cpp	Tue Mar 15 00:58:09 2016 +0000
+++ b/motors.cpp	Wed Aug 03 12:25:23 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
  *
  *
  */
@@ -229,11 +229,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 +246,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;