motors.cpp

00001 /* University of York Robotics Laboratory PsiSwarm Library: Motor Functions Source File
00002  *
00003  * Copyright 2016 University of York
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
00007  * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
00008  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00009  * See the License for the specific language governing permissions and limitations under the License.
00010  *
00011  * File: motors.cpp
00012  *
00013  * (C) Dept. Electronics & Computer Science, University of York
00014  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
00015  *
00016  * PsiSwarm Library Version: 0.8
00017  *
00018  * October 2016
00019  *
00020  *
00021  */
00022 
00023 #include "psiswarm.h"
00024 
00025 Timeout time_based_action_timeout;
00026 char brake_when_done = 0;
00027 
00028 void Motors::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 Motors::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 Motors::brake_left_motor()
00043 {
00044     motor_left_speed = 0;
00045     motor_left_brake = 1;
00046     IF_update_motors();
00047 }
00048 
00049 void Motors::brake_right_motor()
00050 {
00051     motor_right_speed = 0;
00052     motor_right_brake = 1;
00053     IF_update_motors();
00054 }
00055 
00056 void Motors::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 Motors::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 Motors::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 Motors::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 Motors::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 Motors::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(this,&Motors::IF_end_time_based_action,microseconds);
00112 }
00113 
00114 //Turn for a set period of time
00115 void Motors::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(this,&Motors::IF_end_time_based_action,microseconds);
00124 }
00125 
00126 //Returns the limit of degrees available to turn in given time
00127 float Motors::get_maximum_turn_angle(int microseconds)
00128 {
00129     //We can turn at about 270 degrees per second at full speed
00130     return (microseconds * 0.00027);
00131 }
00132 
00133 //Return the time in microseconds that performing the turn will take
00134 int Motors::get_time_based_turn_time(float speed, float degrees)
00135 {
00136     //Check sign of degrees
00137     if(degrees < 0) degrees =- degrees;
00138 
00139     //Main calculation for turn time
00140     float turn_time = degrees / ((290 * speed));
00141 
00142     //Add a hard offset of 4ms to account for start\stop time
00143     if(degrees > 4) {
00144         turn_time += 0.004;
00145     } else turn_time += 0.002;
00146 
00147     // Add offset for slow speed
00148     if(speed<0.31) {
00149         float mul_fact = 0.31 - speed;
00150         if(mul_fact < 0) mul_fact = 0;
00151         mul_fact /= 2;
00152         mul_fact += 1;
00153         turn_time *= mul_fact;
00154     }
00155 
00156     // Add offset for short turns
00157     if(degrees < 360) {
00158         float short_offset_multiplier = 1.0 + (0.9 / degrees);
00159         turn_time *= short_offset_multiplier;
00160     }
00161 
00162     // Convert to uS
00163     turn_time *= 1000000;
00164 
00165     return (int) turn_time;
00166 }
00167 
00168 //Turn the robot a set number of degrees [using time estimation to end turn]
00169 int Motors::time_based_turn_degrees(float speed, float degrees, char brake)
00170 {
00171     if(speed < 0 || speed > 1 || degrees == 0) {
00172         psi.debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees);
00173         return 0;
00174     } else {
00175         //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
00176         IF_check_time_for_existing_time_based_action();
00177 
00178         //Calculate turn time using get_time_based_turn_time
00179         int turn_time = get_time_based_turn_time(speed,degrees);
00180 
00181         //Set correct turn direction (-degrees is a counter-clockwise turn)
00182         if(degrees < 0) {
00183             degrees=-degrees;
00184             speed=-speed;
00185         }
00186 
00187         //Start turning
00188         turn(speed);
00189 
00190         brake_when_done = brake;
00191         time_based_action_timeout.attach_us(this,&Motors::IF_end_time_based_action,turn_time);
00192         return turn_time;
00193     }
00194 }
00195 
00196 //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
00197 void Motors::IF_check_time_for_existing_time_based_action()
00198 {
00199     if(time_based_motor_action == 1) {
00200         time_based_action_timeout.detach();
00201         psi.debug("WARNING: New time-based action called before previous action finished!\n");
00202     } else time_based_motor_action = 1;
00203 }
00204 
00205 void Motors::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 Motors::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 Motors::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 Motors::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 Motors::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 Motors::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 }