Diff: motors.cpp

Revision:

0:d6269d17c8cf

Child:

2:c6986ee3c7c5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motors.cpp	Thu Feb 04 21:48:54 2016 +0000
@@ -0,0 +1,283 @@
+/* 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, Alexander Horsfield, Homero Elizondo, Jon Timmis
+ *
+ * PsiSwarm Library Version: 0.4
+ *
+ * February 2016
+ *
+ *
+ */
+
+#include "psiswarm.h"
+
+Timeout time_based_action_timeout;
+char brake_when_done = 0;
+
+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();
+}
+
+//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);
+}
+
+//Returns the limit of degrees available to turn in given time
+float get_maximum_turn_angle(int microseconds){
+    //We can turn at about 270 degrees per second at full speed
+    return (microseconds * 0.00027);
+}
+
+//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);
+        return 0;
+    } else {
+        //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();
+        
+        //Calculate turn time using get_time_based_turn_time
+        int turn_time = get_time_based_turn_time(speed,degrees);
+        
+        //Set correct turn direction (-degrees is a counter-clockwise turn)
+       if(degrees < 0) {
+            degrees=-degrees;
+            speed=-speed;
+        }
+
+        //Start turning
+        turn(speed);
+        
+        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()
+{
+    if(brake_when_done == 1)brake();
+    else stop();
+    time_based_motor_action = 0;
+}
+
+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