Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
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?
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 | 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 | } |