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