James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

PsiSwarm/motors.cpp@9:085e090e1ec1, 2015-10-26 (annotated)

Committer:
jah128
Date:
Mon Oct 26 11:16:05 2015 +0000
Revision:
9:085e090e1ec1
Parent:
8:00558287a4ef
Child:
13:f5994956b1ba
Fork for variable frequency beacon

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:8a5497a2e366 1 /* University of York Robotics Laboratory PsiSwarm Library: Motor Functions Source File
jah128 8:00558287a4ef 2 *
jah128 0:8a5497a2e366 3 * File: motors.cpp
jah128 0:8a5497a2e366 4 *
jah128 6:ff3c66f7372b 5 * (C) Dept. Electronics & Computer Science, University of York
jah128 6:ff3c66f7372b 6 * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
jah128 8:00558287a4ef 7 *
jah128 6:ff3c66f7372b 8 * PsiSwarm Library Version: 0.3
jah128 0:8a5497a2e366 9 *
jah128 6:ff3c66f7372b 10 * October 2015
jah128 0:8a5497a2e366 11 *
jah128 8:00558287a4ef 12 */
jah128 0:8a5497a2e366 13
jah128 0:8a5497a2e366 14 #include "psiswarm.h"
jah128 0:8a5497a2e366 15
jah128 8:00558287a4ef 16 Timeout time_based_action_timeout;
jah128 9:085e090e1ec1 17 char brake_when_done = 0;
jah128 8:00558287a4ef 18
jah128 0:8a5497a2e366 19 void set_left_motor_speed(float speed)
jah128 0:8a5497a2e366 20 {
jah128 0:8a5497a2e366 21 motor_left_speed = speed;
jah128 0:8a5497a2e366 22 motor_left_brake = 0;
jah128 0:8a5497a2e366 23 IF_update_motors();
jah128 0:8a5497a2e366 24 }
jah128 0:8a5497a2e366 25
jah128 0:8a5497a2e366 26 void set_right_motor_speed(float speed)
jah128 0:8a5497a2e366 27 {
jah128 0:8a5497a2e366 28 motor_right_speed = speed;
jah128 0:8a5497a2e366 29 motor_right_brake = 0;
jah128 0:8a5497a2e366 30 IF_update_motors();
jah128 0:8a5497a2e366 31 }
jah128 0:8a5497a2e366 32
jah128 0:8a5497a2e366 33 void brake_left_motor()
jah128 0:8a5497a2e366 34 {
jah128 0:8a5497a2e366 35 motor_left_speed = 0;
jah128 0:8a5497a2e366 36 motor_left_brake = 1;
jah128 0:8a5497a2e366 37 IF_update_motors();
jah128 0:8a5497a2e366 38 }
jah128 0:8a5497a2e366 39
jah128 0:8a5497a2e366 40 void brake_right_motor()
jah128 0:8a5497a2e366 41 {
jah128 0:8a5497a2e366 42 motor_right_speed = 0;
jah128 0:8a5497a2e366 43 motor_right_brake = 1;
jah128 0:8a5497a2e366 44 IF_update_motors();
jah128 0:8a5497a2e366 45 }
jah128 0:8a5497a2e366 46
jah128 0:8a5497a2e366 47 void brake()
jah128 0:8a5497a2e366 48 {
jah128 0:8a5497a2e366 49 motor_left_speed = 0;
jah128 0:8a5497a2e366 50 motor_right_speed = 0;
jah128 0:8a5497a2e366 51 motor_left_brake = 1;
jah128 0:8a5497a2e366 52 motor_right_brake = 1;
jah128 0:8a5497a2e366 53 IF_update_motors();
jah128 0:8a5497a2e366 54 }
jah128 0:8a5497a2e366 55
jah128 0:8a5497a2e366 56 void stop()
jah128 0:8a5497a2e366 57 {
jah128 0:8a5497a2e366 58 motor_left_speed = 0;
jah128 0:8a5497a2e366 59 motor_right_speed = 0;
jah128 0:8a5497a2e366 60 motor_left_brake = 0;
jah128 0:8a5497a2e366 61 motor_right_brake = 0;
jah128 0:8a5497a2e366 62 IF_update_motors();
jah128 0:8a5497a2e366 63 }
jah128 0:8a5497a2e366 64
jah128 0:8a5497a2e366 65 void forward(float speed)
jah128 0:8a5497a2e366 66 {
jah128 0:8a5497a2e366 67 motor_left_speed = speed;
jah128 0:8a5497a2e366 68 motor_right_speed = speed;
jah128 0:8a5497a2e366 69 motor_left_brake = 0;
jah128 0:8a5497a2e366 70 motor_right_brake = 0;
jah128 0:8a5497a2e366 71 IF_update_motors();
jah128 0:8a5497a2e366 72 }
jah128 0:8a5497a2e366 73
jah128 0:8a5497a2e366 74 void backward(float speed)
jah128 0:8a5497a2e366 75 {
jah128 0:8a5497a2e366 76 motor_left_speed = -speed;
jah128 0:8a5497a2e366 77 motor_right_speed = -speed;
jah128 0:8a5497a2e366 78 motor_left_brake = 0;
jah128 0:8a5497a2e366 79 motor_right_brake = 0;
jah128 0:8a5497a2e366 80 IF_update_motors();
jah128 0:8a5497a2e366 81 }
jah128 0:8a5497a2e366 82
jah128 0:8a5497a2e366 83 void turn(float speed)
jah128 0:8a5497a2e366 84 {
jah128 0:8a5497a2e366 85 //A positive turn is clockwise
jah128 0:8a5497a2e366 86 motor_left_speed = speed;
jah128 0:8a5497a2e366 87 motor_right_speed = -speed;
jah128 0:8a5497a2e366 88 motor_left_brake = 0;
jah128 0:8a5497a2e366 89 motor_right_brake = 0;
jah128 8:00558287a4ef 90 IF_update_motors();
jah128 8:00558287a4ef 91 }
jah128 8:00558287a4ef 92
jah128 9:085e090e1ec1 93 //Forward for a set period of time
jah128 9:085e090e1ec1 94 void time_based_forward(float speed, int microseconds, char brake)
jah128 8:00558287a4ef 95 {
jah128 9:085e090e1ec1 96 //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
jah128 9:085e090e1ec1 97 IF_check_time_for_existing_time_based_action();
jah128 9:085e090e1ec1 98
jah128 9:085e090e1ec1 99 //Start moving
jah128 9:085e090e1ec1 100 forward(speed);
jah128 9:085e090e1ec1 101 brake_when_done = brake;
jah128 9:085e090e1ec1 102 time_based_action_timeout.attach_us(&IF_end_time_based_action,microseconds);
jah128 9:085e090e1ec1 103 }
jah128 9:085e090e1ec1 104
jah128 9:085e090e1ec1 105 //Turn for a set period of time
jah128 9:085e090e1ec1 106 void time_based_turn(float speed, int microseconds, char brake)
jah128 9:085e090e1ec1 107 {
jah128 9:085e090e1ec1 108 //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
jah128 9:085e090e1ec1 109 IF_check_time_for_existing_time_based_action();
jah128 9:085e090e1ec1 110
jah128 9:085e090e1ec1 111 //Start turning
jah128 9:085e090e1ec1 112 turn(speed);
jah128 9:085e090e1ec1 113 brake_when_done = brake;
jah128 9:085e090e1ec1 114 time_based_action_timeout.attach_us(&IF_end_time_based_action,microseconds);
jah128 0:8a5497a2e366 115 }
jah128 0:8a5497a2e366 116
jah128 9:085e090e1ec1 117 //Return the time in microseconds that performing the turn will take
jah128 9:085e090e1ec1 118 int get_time_based_turn_time(float speed, float degrees)
jah128 9:085e090e1ec1 119 {
jah128 9:085e090e1ec1 120 //Check sign of degrees
jah128 9:085e090e1ec1 121 if(degrees < 0) degrees =- degrees;
jah128 9:085e090e1ec1 122
jah128 9:085e090e1ec1 123 //Main calculation for turn time
jah128 9:085e090e1ec1 124 float turn_time = degrees / ((290 * speed));
jah128 9:085e090e1ec1 125
jah128 9:085e090e1ec1 126 //Add a hard offset of 4ms to account for start\stop time
jah128 9:085e090e1ec1 127 if(degrees > 4) {
jah128 9:085e090e1ec1 128 turn_time += 0.004;
jah128 9:085e090e1ec1 129 } else turn_time += 0.002;
jah128 9:085e090e1ec1 130
jah128 9:085e090e1ec1 131 // Add offset for slow speed
jah128 9:085e090e1ec1 132 if(speed<0.31) {
jah128 9:085e090e1ec1 133 float mul_fact = 0.31 - speed;
jah128 9:085e090e1ec1 134 if(mul_fact < 0) mul_fact = 0;
jah128 9:085e090e1ec1 135 mul_fact /= 2;
jah128 9:085e090e1ec1 136 mul_fact += 1;
jah128 9:085e090e1ec1 137 turn_time *= mul_fact;
jah128 9:085e090e1ec1 138 }
jah128 9:085e090e1ec1 139
jah128 9:085e090e1ec1 140 // Add offset for short turns
jah128 9:085e090e1ec1 141 if(degrees < 360) {
jah128 9:085e090e1ec1 142 float short_offset_multiplier = 1.0 + (0.9 / degrees);
jah128 9:085e090e1ec1 143 turn_time *= short_offset_multiplier;
jah128 9:085e090e1ec1 144 }
jah128 9:085e090e1ec1 145
jah128 9:085e090e1ec1 146 // Convert to uS
jah128 9:085e090e1ec1 147 turn_time *= 1000000;
jah128 9:085e090e1ec1 148
jah128 9:085e090e1ec1 149 return (int) turn_time;
jah128 9:085e090e1ec1 150 }
jah128 9:085e090e1ec1 151
jah128 9:085e090e1ec1 152 //Turn the robot a set number of degrees [using time estimation to end turn]
jah128 9:085e090e1ec1 153 int time_based_turn_degrees(float speed, float degrees, char brake)
jah128 8:00558287a4ef 154 {
jah128 8:00558287a4ef 155 if(speed < 0 || speed > 1 || degrees == 0) {
jah128 9:085e090e1ec1 156 debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees);
jah128 9:085e090e1ec1 157 return 0;
jah128 8:00558287a4ef 158 } else {
jah128 9:085e090e1ec1 159 //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
jah128 9:085e090e1ec1 160 IF_check_time_for_existing_time_based_action();
jah128 8:00558287a4ef 161
jah128 9:085e090e1ec1 162 //Calculate turn time using get_time_based_turn_time
jah128 9:085e090e1ec1 163 int turn_time = get_time_based_turn_time(speed,degrees);
jah128 8:00558287a4ef 164
jah128 9:085e090e1ec1 165 //Set correct turn direction (-degrees is a counter-clockwise turn)
jah128 9:085e090e1ec1 166 if(degrees < 0) {
jah128 9:085e090e1ec1 167 degrees=-degrees;
jah128 9:085e090e1ec1 168 speed=-speed;
jah128 8:00558287a4ef 169 }
jah128 8:00558287a4ef 170
jah128 9:085e090e1ec1 171 //Start turning
jah128 9:085e090e1ec1 172 turn(speed);
jah128 8:00558287a4ef 173
jah128 9:085e090e1ec1 174 brake_when_done = brake;
jah128 9:085e090e1ec1 175 time_based_action_timeout.attach_us(&IF_end_time_based_action,turn_time);
jah128 9:085e090e1ec1 176 return turn_time;
jah128 9:085e090e1ec1 177 }
jah128 8:00558287a4ef 178 }
jah128 8:00558287a4ef 179
jah128 9:085e090e1ec1 180 //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
jah128 9:085e090e1ec1 181 void IF_check_time_for_existing_time_based_action()
jah128 9:085e090e1ec1 182 {
jah128 9:085e090e1ec1 183 if(time_based_motor_action == 1){
jah128 9:085e090e1ec1 184 time_based_action_timeout.detach();
jah128 9:085e090e1ec1 185 debug("WARNING: New time-based action called before previous action finished!\n");
jah128 9:085e090e1ec1 186 }
jah128 9:085e090e1ec1 187 else time_based_motor_action = 1;
jah128 9:085e090e1ec1 188 }
jah128 8:00558287a4ef 189
jah128 8:00558287a4ef 190 void IF_end_time_based_action()
jah128 8:00558287a4ef 191 {
jah128 9:085e090e1ec1 192 if(brake_when_done == 1)brake();
jah128 9:085e090e1ec1 193 else stop();
jah128 9:085e090e1ec1 194 time_based_motor_action = 0;
jah128 8:00558287a4ef 195 }
jah128 8:00558287a4ef 196
jah128 8:00558287a4ef 197 void IF_update_motors()
jah128 8:00558287a4ef 198 {
jah128 8:00558287a4ef 199 if(motor_left_speed > 1.0) {
jah128 0:8a5497a2e366 200 motor_left_speed = 1.0;
jah128 8:00558287a4ef 201 //Throw exception...
jah128 0:8a5497a2e366 202 }
jah128 8:00558287a4ef 203 if(motor_right_speed > 1.0) {
jah128 8:00558287a4ef 204 motor_right_speed = 1.0;
jah128 8:00558287a4ef 205 //Throw exception...
jah128 8:00558287a4ef 206 }
jah128 8:00558287a4ef 207 if(motor_left_speed < -1.0) {
jah128 8:00558287a4ef 208 motor_left_speed = -1.0;
jah128 8:00558287a4ef 209 //Throw exception...
jah128 0:8a5497a2e366 210 }
jah128 8:00558287a4ef 211 if(motor_right_speed < -1.0) {
jah128 8:00558287a4ef 212 motor_right_speed = -1.0;
jah128 0:8a5497a2e366 213 //Throw exception...
jah128 8:00558287a4ef 214 }
jah128 8:00558287a4ef 215 if(motor_left_brake) {
jah128 8:00558287a4ef 216 motor_left_f.write(1);
jah128 8:00558287a4ef 217 motor_left_r.write(1);
jah128 8:00558287a4ef 218 if(motor_left_speed!=0) {
jah128 8:00558287a4ef 219 motor_left_speed = 0;
jah128 8:00558287a4ef 220 //Throw exception...
jah128 8:00558287a4ef 221 }
jah128 8:00558287a4ef 222 } else {
jah128 8:00558287a4ef 223 if(motor_left_speed >= 0) {
jah128 0:8a5497a2e366 224 motor_left_f.write(0);
jah128 0:8a5497a2e366 225 motor_left_r.write(IF_calibrated_speed(motor_left_speed));
jah128 8:00558287a4ef 226
jah128 8:00558287a4ef 227 } else {
jah128 0:8a5497a2e366 228 motor_left_r.write(0);
jah128 0:8a5497a2e366 229 motor_left_f.write(IF_calibrated_speed(-motor_left_speed));
jah128 0:8a5497a2e366 230 }
jah128 0:8a5497a2e366 231 }
jah128 8:00558287a4ef 232 if(motor_right_brake) {
jah128 8:00558287a4ef 233 motor_right_f.write(1);
jah128 8:00558287a4ef 234 motor_right_r.write(1);
jah128 8:00558287a4ef 235 if(motor_right_speed!=0) {
jah128 8:00558287a4ef 236 motor_right_speed = 0;
jah128 8:00558287a4ef 237 //Throw exception...
jah128 8:00558287a4ef 238 }
jah128 8:00558287a4ef 239 } else {
jah128 8:00558287a4ef 240 if(motor_right_speed >= 0) {
jah128 0:8a5497a2e366 241 motor_right_f.write(0);
jah128 0:8a5497a2e366 242 motor_right_r.write(IF_calibrated_speed(motor_right_speed));
jah128 8:00558287a4ef 243 } else {
jah128 0:8a5497a2e366 244 motor_right_r.write(0);
jah128 0:8a5497a2e366 245 motor_right_f.write(IF_calibrated_speed(-motor_right_speed));
jah128 0:8a5497a2e366 246 }
jah128 0:8a5497a2e366 247 }
jah128 8:00558287a4ef 248
jah128 0:8a5497a2e366 249 }
jah128 0:8a5497a2e366 250
jah128 0:8a5497a2e366 251
jah128 8:00558287a4ef 252 float IF_calibrated_speed(float speed)
jah128 8:00558287a4ef 253 {
jah128 0:8a5497a2e366 254 if(speed == 0) return 0;
jah128 0:8a5497a2e366 255 //Converts an input value to take account of the stall speed of the motor; aims to produce a more linear speed
jah128 0:8a5497a2e366 256 float adjusted = speed;
jah128 8:00558287a4ef 257 if(OFFSET_MOTORS) {
jah128 8:00558287a4ef 258 adjusted*=0.8f;
jah128 8:00558287a4ef 259 adjusted+=0.2;
jah128 8:00558287a4ef 260 }
jah128 8:00558287a4ef 261 return adjusted;
jah128 0:8a5497a2e366 262 }
jah128 0:8a5497a2e366 263
jah128 8:00558287a4ef 264 void IF_init_motors()
jah128 8:00558287a4ef 265 {
jah128 0:8a5497a2e366 266 // Motor PWM outputs work optimally at 25kHz frequency
jah128 0:8a5497a2e366 267 motor_left_f.period_us(40);
jah128 8:00558287a4ef 268 motor_right_f.period_us(40);
jah128 0:8a5497a2e366 269 motor_left_r.period_us(40);
jah128 0:8a5497a2e366 270 motor_right_r.period_us(40);
jah128 0:8a5497a2e366 271 motor_left_speed = 0;
jah128 0:8a5497a2e366 272 motor_right_speed = 0;
jah128 0:8a5497a2e366 273 motor_left_brake = 0;
jah128 8:00558287a4ef 274 motor_right_brake = 0;
jah128 8:00558287a4ef 275 IF_update_motors();
jah128 0:8a5497a2e366 276 }