Diff: PsiSwarm/motors.cpp

Revision:

0:8a5497a2e366

Child:

6:ff3c66f7372b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PsiSwarm/motors.cpp	Sat Oct 03 22:48:50 2015 +0000
@@ -0,0 +1,162 @@
+/* University of York Robotics Laboratory PsiSwarm Library: Motor Functions Source File
+ * 
+ * File: motors.cpp
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ * 
+ * PsiSwarm Library Version: 0.2
+ *
+ * September 2015
+ *
+ */ 
+
+#include "psiswarm.h"
+
+void set_left_motor_speed(float speed)
+{
+    motor_left_speed = speed;
+    motor_left_brake = 0;
+    IF_update_motors();
+}
+
+void set_right_motor_speed(float speed)
+{
+    motor_right_speed = speed;
+    motor_right_brake = 0;
+    IF_update_motors();
+}
+
+void brake_left_motor()
+{
+    motor_left_speed = 0;
+    motor_left_brake = 1;
+    IF_update_motors();
+}
+
+void brake_right_motor()
+{
+    motor_right_speed = 0;
+    motor_right_brake = 1;
+    IF_update_motors();
+}
+
+void brake()
+{
+    motor_left_speed = 0;
+    motor_right_speed = 0;
+    motor_left_brake = 1;
+    motor_right_brake = 1;
+    IF_update_motors();
+}
+
+void stop()
+{
+    motor_left_speed = 0;
+    motor_right_speed = 0;
+    motor_left_brake = 0;
+    motor_right_brake = 0;
+    IF_update_motors();
+}
+
+void forward(float speed)
+{
+    motor_left_speed = speed;
+    motor_right_speed = speed;
+    motor_left_brake = 0;
+    motor_right_brake = 0;
+    IF_update_motors();
+}
+
+void backward(float speed)
+{
+    motor_left_speed = -speed;
+    motor_right_speed = -speed;
+    motor_left_brake = 0;
+    motor_right_brake = 0;
+    IF_update_motors();
+}
+
+void turn(float speed)
+{
+    //A positive turn is clockwise
+    motor_left_speed = speed;
+    motor_right_speed = -speed;
+    motor_left_brake = 0;
+    motor_right_brake = 0;
+    IF_update_motors();   
+}
+
+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...   
+    }
+    if(motor_right_speed < -1.0){
+        motor_right_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;
+        //Throw exception...
+       }   
+    }else{
+        if(motor_left_speed >= 0){
+            motor_left_f.write(0);
+            motor_left_r.write(IF_calibrated_speed(motor_left_speed));
+  
+        }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){
+            motor_right_f.write(0);
+            motor_right_r.write(IF_calibrated_speed(motor_right_speed));
+        }else{
+            motor_right_r.write(0);
+            motor_right_f.write(IF_calibrated_speed(-motor_right_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;   
+}
+
+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_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();    
+}
\ No newline at end of file