motors.cpp

00001 /* University of York Robotics Laboratory PsiSwarm Library: Motor Functions Source File
00002  *
00003  * File: motors.cpp
00004  *
00005  * (C) Dept. Electronics & Computer Science, University of York
00006  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
00007  *
00008  * PsiSwarm Library Version: 0.5
00009  *
00010  * April 2016
00011  *
00012  *
00013  */
00014 
00015 #include "psiswarm.h"
00016 
00017 Timeout time_based_action_timeout;
00018 char brake_when_done = 0;
00019 
00020 void set_motor_speed(float left_motor_speed, float right_motor_speed)
00021 {
00022     motor_left_speed = left_motor_speed;
00023     motor_right_speed = right_motor_speed;
00024     motor_left_brake = 0;
00025     motor_right_brake = 0;
00026     IF_update_motors();
00027 }
00028 void set_left_motor_speed(float speed)
00029 {
00030     motor_left_speed = speed;
00031     motor_left_brake = 0;
00032     IF_update_motors();
00033 }
00034 
00035 void set_right_motor_speed(float speed)
00036 {
00037     motor_right_speed = speed;
00038     motor_right_brake = 0;
00039     IF_update_motors();
00040 }
00041 
00042 void brake_left_motor()
00043 {
00044     motor_left_speed = 0;
00045     motor_left_brake = 1;
00046     IF_update_motors();
00047 }
00048 
00049 void brake_right_motor()
00050 {
00051     motor_right_speed = 0;
00052     motor_right_brake = 1;
00053     IF_update_motors();
00054 }
00055 
00056 void brake()
00057 {
00058     motor_left_speed = 0;
00059     motor_right_speed = 0;
00060     motor_left_brake = 1;
00061     motor_right_brake = 1;
00062     IF_update_motors();
00063 }
00064 
00065 void stop()
00066 {
00067     motor_left_speed = 0;
00068     motor_right_speed = 0;
00069     motor_left_brake = 0;
00070     motor_right_brake = 0;
00071     IF_update_motors();
00072 }
00073 
00074 void forward(float speed)
00075 {
00076     motor_left_speed = speed;
00077     motor_right_speed = speed;
00078     motor_left_brake = 0;
00079     motor_right_brake = 0;
00080     IF_update_motors();
00081 }
00082 
00083 void backward(float speed)
00084 {
00085     motor_left_speed = -speed;
00086     motor_right_speed = -speed;
00087     motor_left_brake = 0;
00088     motor_right_brake = 0;
00089     IF_update_motors();
00090 }
00091 
00092 void turn(float speed)
00093 {
00094     //A positive turn is clockwise
00095     motor_left_speed = speed;
00096     motor_right_speed = -speed;
00097     motor_left_brake = 0;
00098     motor_right_brake = 0;
00099     IF_update_motors();
00100 }
00101 
00102 //Forward for a set period of time
00103 void time_based_forward(float speed, int microseconds, char brake)
00104 {
00105     //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
00106     IF_check_time_for_existing_time_based_action();
00107         
00108     //Start moving
00109     forward(speed);
00110     brake_when_done = brake;
00111     time_based_action_timeout.attach_us(&IF_end_time_based_action,microseconds);
00112 }
00113 
00114 //Turn for a set period of time
00115 void time_based_turn(float speed, int microseconds, char brake)
00116 {
00117     //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
00118     IF_check_time_for_existing_time_based_action();
00119         
00120     //Start turning
00121     turn(speed);
00122     brake_when_done = brake;
00123     time_based_action_timeout.attach_us(&IF_end_time_based_action,microseconds);
00124 }
00125 
00126 //Returns the limit of degrees available to turn in given time
00127 float get_maximum_turn_angle(int microseconds){
00128     //We can turn at about 270 degrees per second at full speed
00129     return (microseconds * 0.00027);
00130 }
00131 
00132 //Return the time in microseconds that performing the turn will take
00133 int get_time_based_turn_time(float speed, float degrees)
00134 {
00135     //Check sign of degrees
00136     if(degrees < 0) degrees =- degrees;
00137      
00138     //Main calculation for turn time
00139     float turn_time = degrees / ((290 * speed));
00140 
00141     //Add a hard offset of 4ms to account for start\stop time
00142     if(degrees > 4) {
00143        turn_time += 0.004;
00144     } else turn_time += 0.002;
00145 
00146     // Add offset for slow speed
00147     if(speed<0.31) {
00148         float mul_fact = 0.31 - speed;
00149         if(mul_fact < 0) mul_fact = 0;
00150         mul_fact /= 2;
00151         mul_fact += 1;
00152         turn_time *= mul_fact;
00153     }
00154 
00155     // Add offset for short turns
00156     if(degrees < 360) {
00157         float short_offset_multiplier = 1.0 + (0.9 / degrees);
00158         turn_time *= short_offset_multiplier;
00159     }
00160     
00161     // Convert to uS 
00162     turn_time *= 1000000;
00163     
00164     return (int) turn_time;   
00165 }
00166 
00167 //Turn the robot a set number of degrees [using time estimation to end turn]
00168 int time_based_turn_degrees(float speed, float degrees, char brake)
00169 {
00170     if(speed < 0 || speed > 1 || degrees == 0) {
00171         debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees);
00172         return 0;
00173     } else {
00174         //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
00175         IF_check_time_for_existing_time_based_action();
00176         
00177         //Calculate turn time using get_time_based_turn_time
00178         int turn_time = get_time_based_turn_time(speed,degrees);
00179         
00180         //Set correct turn direction (-degrees is a counter-clockwise turn)
00181        if(degrees < 0) {
00182             degrees=-degrees;
00183             speed=-speed;
00184         }
00185 
00186         //Start turning
00187         turn(speed);
00188         
00189         brake_when_done = brake;
00190         time_based_action_timeout.attach_us(&IF_end_time_based_action,turn_time);
00191         return turn_time;
00192     }
00193 }
00194 
00195 //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
00196 void IF_check_time_for_existing_time_based_action()
00197 {
00198     if(time_based_motor_action == 1){
00199         time_based_action_timeout.detach(); 
00200         debug("WARNING: New time-based action called before previous action finished!\n");
00201     }   
00202     else time_based_motor_action = 1;
00203 }
00204 
00205 void IF_end_time_based_action()
00206 {
00207     if(brake_when_done == 1)brake();
00208     else stop();
00209     time_based_motor_action = 0;
00210 }
00211 
00212 void IF_update_motors()
00213 {
00214     if(motor_left_speed > 1.0) {
00215         motor_left_speed = 1.0;
00216         //Throw exception...
00217     }
00218     if(motor_right_speed > 1.0) {
00219         motor_right_speed = 1.0;
00220         //Throw exception...
00221     }
00222     if(motor_left_speed < -1.0) {
00223         motor_left_speed = -1.0;
00224         //Throw exception...
00225     }
00226     if(motor_right_speed < -1.0) {
00227         motor_right_speed = -1.0;
00228         //Throw exception...
00229     }
00230     if(motor_left_brake) {
00231         motor_left_f.write(1);
00232         motor_left_r.write(1);
00233         if(motor_left_speed!=0) {
00234             motor_left_speed = 0;
00235             //Throw exception...
00236         }
00237     } else {
00238         if(motor_left_speed >= 0) {
00239             motor_left_f.write(0);
00240             motor_left_r.write(IF_calibrated_left_speed(motor_left_speed));
00241 
00242         } else {
00243             motor_left_r.write(0);
00244             motor_left_f.write(IF_calibrated_left_speed(-motor_left_speed));
00245         }
00246     }
00247     if(motor_right_brake) {
00248         motor_right_f.write(1);
00249         motor_right_r.write(1);
00250         if(motor_right_speed!=0) {
00251             motor_right_speed = 0;
00252             //Throw exception...
00253         }
00254     } else {
00255         if(motor_right_speed >= 0) {
00256             motor_right_f.write(0);
00257             motor_right_r.write(IF_calibrated_right_speed(motor_right_speed));
00258         } else {
00259             motor_right_r.write(0);
00260             motor_right_f.write(IF_calibrated_right_speed(-motor_right_speed));
00261         }
00262     }
00263 
00264 }
00265 
00266 
00267 float IF_calibrated_left_speed(float speed)
00268 {
00269     //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
00270     float adjusted  = IF_calibrated_speed(speed);
00271     if(use_motor_calibration){
00272         adjusted *= left_motor_calibration_value;
00273     }
00274     return adjusted;
00275 }
00276 
00277 float IF_calibrated_right_speed(float speed)
00278 {
00279     //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
00280     float adjusted  = IF_calibrated_speed(speed);
00281     if(use_motor_calibration){
00282         adjusted *= right_motor_calibration_value;
00283     }
00284     return adjusted;
00285 }
00286 
00287 float IF_calibrated_speed(float speed)
00288 {
00289     if(speed == 0) return 0;
00290     //Converts an input value to take account of the stall speed of the motor; aims to produce a more linear speed
00291     float adjusted = speed;
00292     if(OFFSET_MOTORS) {
00293         adjusted*=0.8f;
00294         adjusted+=0.2;
00295     }
00296     return adjusted;
00297 }
00298 
00299 void IF_init_motors()
00300 {
00301     // Motor PWM outputs work optimally at 25kHz frequency
00302     motor_left_f.period_us(40);
00303     motor_right_f.period_us(40);
00304     motor_left_r.period_us(40);
00305     motor_right_r.period_us(40);
00306     motor_left_speed = 0;
00307     motor_right_speed = 0;
00308     motor_left_brake = 0;
00309     motor_right_brake = 0;
00310     IF_update_motors();
00311 }