Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
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?
User | Revision | Line number | New 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 | } |