Alan Millard / Mbed 2 deprecated BeautifulMemeProjectBT

Bluetooth communication for flocking.

Dependencies:   mbed

Fork of BeautifulMemeProject by James Hilder

PsiSwarm/motors.cpp@27:7eb032772bc2, 2016-01-31 (annotated)

Committer:
alanmillard
Date:
Sun Jan 31 15:14:54 2016 +0000
Revision:
27:7eb032772bc2
Parent:
13:f5994956b1ba 
Flocking seems to work better now, however the robot controller sometimes "crashes" (unsure why).

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 13:f5994956b1ba 117 //Returns the limit of degrees available to turn in given time
jah128 13:f5994956b1ba 118 float get_maximum_turn_angle(int microseconds){
jah128 13:f5994956b1ba 119 //We can turn at about 270 degrees per second at full speed
jah128 13:f5994956b1ba 120 return (microseconds * 0.00027);
jah128 13:f5994956b1ba 121 }
jah128 13:f5994956b1ba 122
jah128 9:085e090e1ec1 123 //Return the time in microseconds that performing the turn will take
jah128 9:085e090e1ec1 124 int get_time_based_turn_time(float speed, float degrees)
jah128 9:085e090e1ec1 125 {
jah128 9:085e090e1ec1 126 //Check sign of degrees
jah128 9:085e090e1ec1 127 if(degrees < 0) degrees =- degrees;
jah128 9:085e090e1ec1 128
jah128 9:085e090e1ec1 129 //Main calculation for turn time
jah128 9:085e090e1ec1 130 float turn_time = degrees / ((290 * speed));
jah128 9:085e090e1ec1 131
jah128 9:085e090e1ec1 132 //Add a hard offset of 4ms to account for start\stop time
jah128 9:085e090e1ec1 133 if(degrees > 4) {
jah128 9:085e090e1ec1 134 turn_time += 0.004;
jah128 9:085e090e1ec1 135 } else turn_time += 0.002;
jah128 9:085e090e1ec1 136
jah128 9:085e090e1ec1 137 // Add offset for slow speed
jah128 9:085e090e1ec1 138 if(speed<0.31) {
jah128 9:085e090e1ec1 139 float mul_fact = 0.31 - speed;
jah128 9:085e090e1ec1 140 if(mul_fact < 0) mul_fact = 0;
jah128 9:085e090e1ec1 141 mul_fact /= 2;
jah128 9:085e090e1ec1 142 mul_fact += 1;
jah128 9:085e090e1ec1 143 turn_time *= mul_fact;
jah128 9:085e090e1ec1 144 }
jah128 9:085e090e1ec1 145
jah128 9:085e090e1ec1 146 // Add offset for short turns
jah128 9:085e090e1ec1 147 if(degrees < 360) {
jah128 9:085e090e1ec1 148 float short_offset_multiplier = 1.0 + (0.9 / degrees);
jah128 9:085e090e1ec1 149 turn_time *= short_offset_multiplier;
jah128 9:085e090e1ec1 150 }
jah128 9:085e090e1ec1 151
jah128 9:085e090e1ec1 152 // Convert to uS
jah128 9:085e090e1ec1 153 turn_time *= 1000000;
jah128 9:085e090e1ec1 154
jah128 9:085e090e1ec1 155 return (int) turn_time;
jah128 9:085e090e1ec1 156 }
jah128 9:085e090e1ec1 157
jah128 9:085e090e1ec1 158 //Turn the robot a set number of degrees [using time estimation to end turn]
jah128 9:085e090e1ec1 159 int time_based_turn_degrees(float speed, float degrees, char brake)
jah128 8:00558287a4ef 160 {
jah128 8:00558287a4ef 161 if(speed < 0 || speed > 1 || degrees == 0) {
jah128 9:085e090e1ec1 162 debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees);
jah128 9:085e090e1ec1 163 return 0;
jah128 8:00558287a4ef 164 } else {
jah128 9:085e090e1ec1 165 //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
jah128 9:085e090e1ec1 166 IF_check_time_for_existing_time_based_action();
jah128 8:00558287a4ef 167
jah128 9:085e090e1ec1 168 //Calculate turn time using get_time_based_turn_time
jah128 9:085e090e1ec1 169 int turn_time = get_time_based_turn_time(speed,degrees);
jah128 8:00558287a4ef 170
jah128 9:085e090e1ec1 171 //Set correct turn direction (-degrees is a counter-clockwise turn)
jah128 9:085e090e1ec1 172 if(degrees < 0) {
jah128 9:085e090e1ec1 173 degrees=-degrees;
jah128 9:085e090e1ec1 174 speed=-speed;
jah128 8:00558287a4ef 175 }
jah128 8:00558287a4ef 176
jah128 9:085e090e1ec1 177 //Start turning
jah128 9:085e090e1ec1 178 turn(speed);
jah128 8:00558287a4ef 179
jah128 9:085e090e1ec1 180 brake_when_done = brake;
jah128 9:085e090e1ec1 181 time_based_action_timeout.attach_us(&IF_end_time_based_action,turn_time);
jah128 9:085e090e1ec1 182 return turn_time;
jah128 9:085e090e1ec1 183 }
jah128 8:00558287a4ef 184 }
jah128 8:00558287a4ef 185
jah128 9:085e090e1ec1 186 //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
jah128 9:085e090e1ec1 187 void IF_check_time_for_existing_time_based_action()
jah128 9:085e090e1ec1 188 {
jah128 9:085e090e1ec1 189 if(time_based_motor_action == 1){
jah128 9:085e090e1ec1 190 time_based_action_timeout.detach();
jah128 9:085e090e1ec1 191 debug("WARNING: New time-based action called before previous action finished!\n");
jah128 9:085e090e1ec1 192 }
jah128 9:085e090e1ec1 193 else time_based_motor_action = 1;
jah128 9:085e090e1ec1 194 }
jah128 8:00558287a4ef 195
jah128 8:00558287a4ef 196 void IF_end_time_based_action()
jah128 8:00558287a4ef 197 {
jah128 9:085e090e1ec1 198 if(brake_when_done == 1)brake();
jah128 9:085e090e1ec1 199 else stop();
jah128 9:085e090e1ec1 200 time_based_motor_action = 0;
jah128 8:00558287a4ef 201 }
jah128 8:00558287a4ef 202
jah128 8:00558287a4ef 203 void IF_update_motors()
jah128 8:00558287a4ef 204 {
jah128 8:00558287a4ef 205 if(motor_left_speed > 1.0) {
jah128 0:8a5497a2e366 206 motor_left_speed = 1.0;
jah128 8:00558287a4ef 207 //Throw exception...
jah128 0:8a5497a2e366 208 }
jah128 8:00558287a4ef 209 if(motor_right_speed > 1.0) {
jah128 8:00558287a4ef 210 motor_right_speed = 1.0;
jah128 8:00558287a4ef 211 //Throw exception...
jah128 8:00558287a4ef 212 }
jah128 8:00558287a4ef 213 if(motor_left_speed < -1.0) {
jah128 8:00558287a4ef 214 motor_left_speed = -1.0;
jah128 8:00558287a4ef 215 //Throw exception...
jah128 0:8a5497a2e366 216 }
jah128 8:00558287a4ef 217 if(motor_right_speed < -1.0) {
jah128 8:00558287a4ef 218 motor_right_speed = -1.0;
jah128 0:8a5497a2e366 219 //Throw exception...
jah128 8:00558287a4ef 220 }
jah128 8:00558287a4ef 221 if(motor_left_brake) {
jah128 8:00558287a4ef 222 motor_left_f.write(1);
jah128 8:00558287a4ef 223 motor_left_r.write(1);
jah128 8:00558287a4ef 224 if(motor_left_speed!=0) {
jah128 8:00558287a4ef 225 motor_left_speed = 0;
jah128 8:00558287a4ef 226 //Throw exception...
jah128 8:00558287a4ef 227 }
jah128 8:00558287a4ef 228 } else {
jah128 8:00558287a4ef 229 if(motor_left_speed >= 0) {
jah128 0:8a5497a2e366 230 motor_left_f.write(0);
jah128 0:8a5497a2e366 231 motor_left_r.write(IF_calibrated_speed(motor_left_speed));
jah128 8:00558287a4ef 232
jah128 8:00558287a4ef 233 } else {
jah128 0:8a5497a2e366 234 motor_left_r.write(0);
jah128 0:8a5497a2e366 235 motor_left_f.write(IF_calibrated_speed(-motor_left_speed));
jah128 0:8a5497a2e366 236 }
jah128 0:8a5497a2e366 237 }
jah128 8:00558287a4ef 238 if(motor_right_brake) {
jah128 8:00558287a4ef 239 motor_right_f.write(1);
jah128 8:00558287a4ef 240 motor_right_r.write(1);
jah128 8:00558287a4ef 241 if(motor_right_speed!=0) {
jah128 8:00558287a4ef 242 motor_right_speed = 0;
jah128 8:00558287a4ef 243 //Throw exception...
jah128 8:00558287a4ef 244 }
jah128 8:00558287a4ef 245 } else {
jah128 8:00558287a4ef 246 if(motor_right_speed >= 0) {
jah128 0:8a5497a2e366 247 motor_right_f.write(0);
jah128 0:8a5497a2e366 248 motor_right_r.write(IF_calibrated_speed(motor_right_speed));
jah128 8:00558287a4ef 249 } else {
jah128 0:8a5497a2e366 250 motor_right_r.write(0);
jah128 0:8a5497a2e366 251 motor_right_f.write(IF_calibrated_speed(-motor_right_speed));
jah128 0:8a5497a2e366 252 }
jah128 0:8a5497a2e366 253 }
jah128 8:00558287a4ef 254
jah128 0:8a5497a2e366 255 }
jah128 0:8a5497a2e366 256
jah128 0:8a5497a2e366 257
jah128 8:00558287a4ef 258 float IF_calibrated_speed(float speed)
jah128 8:00558287a4ef 259 {
jah128 0:8a5497a2e366 260 if(speed == 0) return 0;
jah128 0:8a5497a2e366 261 //Converts an input value to take account of the stall speed of the motor; aims to produce a more linear speed
jah128 0:8a5497a2e366 262 float adjusted = speed;
jah128 8:00558287a4ef 263 if(OFFSET_MOTORS) {
jah128 8:00558287a4ef 264 adjusted*=0.8f;
jah128 8:00558287a4ef 265 adjusted+=0.2;
jah128 8:00558287a4ef 266 }
jah128 8:00558287a4ef 267 return adjusted;
jah128 0:8a5497a2e366 268 }
jah128 0:8a5497a2e366 269
jah128 8:00558287a4ef 270 void IF_init_motors()
jah128 8:00558287a4ef 271 {
jah128 0:8a5497a2e366 272 // Motor PWM outputs work optimally at 25kHz frequency
jah128 0:8a5497a2e366 273 motor_left_f.period_us(40);
jah128 8:00558287a4ef 274 motor_right_f.period_us(40);
jah128 0:8a5497a2e366 275 motor_left_r.period_us(40);
jah128 0:8a5497a2e366 276 motor_right_r.period_us(40);
jah128 0:8a5497a2e366 277 motor_left_speed = 0;
jah128 0:8a5497a2e366 278 motor_right_speed = 0;
jah128 0:8a5497a2e366 279 motor_left_brake = 0;
jah128 8:00558287a4ef 280 motor_right_brake = 0;
jah128 8:00558287a4ef 281 IF_update_motors();
jah128 0:8a5497a2e366 282 }