Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
PsiSwarm/motors.cpp@8:00558287a4ef, 2015-10-22 (annotated)
- Committer:
- jah128
- Date:
- Thu Oct 22 15:36:16 2015 +0000
- Revision:
- 8:00558287a4ef
- Parent:
- 6:ff3c66f7372b
- Child:
- 9:085e090e1ec1
Added time_based_turn_degrees
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 | 8:00558287a4ef | 17 | |
jah128 | 0:8a5497a2e366 | 18 | void set_left_motor_speed(float speed) |
jah128 | 0:8a5497a2e366 | 19 | { |
jah128 | 0:8a5497a2e366 | 20 | motor_left_speed = speed; |
jah128 | 0:8a5497a2e366 | 21 | motor_left_brake = 0; |
jah128 | 0:8a5497a2e366 | 22 | IF_update_motors(); |
jah128 | 0:8a5497a2e366 | 23 | } |
jah128 | 0:8a5497a2e366 | 24 | |
jah128 | 0:8a5497a2e366 | 25 | void set_right_motor_speed(float speed) |
jah128 | 0:8a5497a2e366 | 26 | { |
jah128 | 0:8a5497a2e366 | 27 | motor_right_speed = speed; |
jah128 | 0:8a5497a2e366 | 28 | motor_right_brake = 0; |
jah128 | 0:8a5497a2e366 | 29 | IF_update_motors(); |
jah128 | 0:8a5497a2e366 | 30 | } |
jah128 | 0:8a5497a2e366 | 31 | |
jah128 | 0:8a5497a2e366 | 32 | void brake_left_motor() |
jah128 | 0:8a5497a2e366 | 33 | { |
jah128 | 0:8a5497a2e366 | 34 | motor_left_speed = 0; |
jah128 | 0:8a5497a2e366 | 35 | motor_left_brake = 1; |
jah128 | 0:8a5497a2e366 | 36 | IF_update_motors(); |
jah128 | 0:8a5497a2e366 | 37 | } |
jah128 | 0:8a5497a2e366 | 38 | |
jah128 | 0:8a5497a2e366 | 39 | void brake_right_motor() |
jah128 | 0:8a5497a2e366 | 40 | { |
jah128 | 0:8a5497a2e366 | 41 | motor_right_speed = 0; |
jah128 | 0:8a5497a2e366 | 42 | motor_right_brake = 1; |
jah128 | 0:8a5497a2e366 | 43 | IF_update_motors(); |
jah128 | 0:8a5497a2e366 | 44 | } |
jah128 | 0:8a5497a2e366 | 45 | |
jah128 | 0:8a5497a2e366 | 46 | void brake() |
jah128 | 0:8a5497a2e366 | 47 | { |
jah128 | 0:8a5497a2e366 | 48 | motor_left_speed = 0; |
jah128 | 0:8a5497a2e366 | 49 | motor_right_speed = 0; |
jah128 | 0:8a5497a2e366 | 50 | motor_left_brake = 1; |
jah128 | 0:8a5497a2e366 | 51 | motor_right_brake = 1; |
jah128 | 0:8a5497a2e366 | 52 | IF_update_motors(); |
jah128 | 0:8a5497a2e366 | 53 | } |
jah128 | 0:8a5497a2e366 | 54 | |
jah128 | 0:8a5497a2e366 | 55 | void stop() |
jah128 | 0:8a5497a2e366 | 56 | { |
jah128 | 0:8a5497a2e366 | 57 | motor_left_speed = 0; |
jah128 | 0:8a5497a2e366 | 58 | motor_right_speed = 0; |
jah128 | 0:8a5497a2e366 | 59 | motor_left_brake = 0; |
jah128 | 0:8a5497a2e366 | 60 | motor_right_brake = 0; |
jah128 | 0:8a5497a2e366 | 61 | IF_update_motors(); |
jah128 | 0:8a5497a2e366 | 62 | } |
jah128 | 0:8a5497a2e366 | 63 | |
jah128 | 0:8a5497a2e366 | 64 | void forward(float speed) |
jah128 | 0:8a5497a2e366 | 65 | { |
jah128 | 0:8a5497a2e366 | 66 | motor_left_speed = speed; |
jah128 | 0:8a5497a2e366 | 67 | motor_right_speed = speed; |
jah128 | 0:8a5497a2e366 | 68 | motor_left_brake = 0; |
jah128 | 0:8a5497a2e366 | 69 | motor_right_brake = 0; |
jah128 | 0:8a5497a2e366 | 70 | IF_update_motors(); |
jah128 | 0:8a5497a2e366 | 71 | } |
jah128 | 0:8a5497a2e366 | 72 | |
jah128 | 0:8a5497a2e366 | 73 | void backward(float speed) |
jah128 | 0:8a5497a2e366 | 74 | { |
jah128 | 0:8a5497a2e366 | 75 | motor_left_speed = -speed; |
jah128 | 0:8a5497a2e366 | 76 | motor_right_speed = -speed; |
jah128 | 0:8a5497a2e366 | 77 | motor_left_brake = 0; |
jah128 | 0:8a5497a2e366 | 78 | motor_right_brake = 0; |
jah128 | 0:8a5497a2e366 | 79 | IF_update_motors(); |
jah128 | 0:8a5497a2e366 | 80 | } |
jah128 | 0:8a5497a2e366 | 81 | |
jah128 | 0:8a5497a2e366 | 82 | void turn(float speed) |
jah128 | 0:8a5497a2e366 | 83 | { |
jah128 | 0:8a5497a2e366 | 84 | //A positive turn is clockwise |
jah128 | 0:8a5497a2e366 | 85 | motor_left_speed = speed; |
jah128 | 0:8a5497a2e366 | 86 | motor_right_speed = -speed; |
jah128 | 0:8a5497a2e366 | 87 | motor_left_brake = 0; |
jah128 | 0:8a5497a2e366 | 88 | motor_right_brake = 0; |
jah128 | 8:00558287a4ef | 89 | IF_update_motors(); |
jah128 | 8:00558287a4ef | 90 | } |
jah128 | 8:00558287a4ef | 91 | |
jah128 | 8:00558287a4ef | 92 | void time_based_turn(float speed, int microseconds) |
jah128 | 8:00558287a4ef | 93 | { |
jah128 | 0:8a5497a2e366 | 94 | } |
jah128 | 0:8a5497a2e366 | 95 | |
jah128 | 8:00558287a4ef | 96 | void time_based_turn_degrees(float speed, float degrees) |
jah128 | 8:00558287a4ef | 97 | { |
jah128 | 8:00558287a4ef | 98 | if(speed < 0 || speed > 1 || degrees == 0) { |
jah128 | 8:00558287a4ef | 99 | debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees); |
jah128 | 8:00558287a4ef | 100 | } else { |
jah128 | 8:00558287a4ef | 101 | char invert = 0; |
jah128 | 8:00558287a4ef | 102 | if(degrees < 0) {degrees=-degrees; invert = 1;} |
jah128 | 8:00558287a4ef | 103 | |
jah128 | 8:00558287a4ef | 104 | //Main calculation for turn time |
jah128 | 8:00558287a4ef | 105 | float turn_time = degrees / ((290 * speed)); |
jah128 | 8:00558287a4ef | 106 | |
jah128 | 8:00558287a4ef | 107 | //Add a hard offset of 4ms to account for start\stop time |
jah128 | 8:00558287a4ef | 108 | if(degrees > 4) { |
jah128 | 8:00558287a4ef | 109 | turn_time += 0.004; |
jah128 | 8:00558287a4ef | 110 | } else turn_time += 0.002; |
jah128 | 8:00558287a4ef | 111 | |
jah128 | 8:00558287a4ef | 112 | // Add offset for slow speed |
jah128 | 8:00558287a4ef | 113 | if(speed<0.31) { |
jah128 | 8:00558287a4ef | 114 | float mul_fact = 0.31 - speed; |
jah128 | 8:00558287a4ef | 115 | if(mul_fact < 0) mul_fact = 0; |
jah128 | 8:00558287a4ef | 116 | mul_fact /= 2; |
jah128 | 8:00558287a4ef | 117 | mul_fact += 1; |
jah128 | 8:00558287a4ef | 118 | turn_time *= mul_fact; |
jah128 | 8:00558287a4ef | 119 | } |
jah128 | 8:00558287a4ef | 120 | |
jah128 | 8:00558287a4ef | 121 | // Add offset for short turns |
jah128 | 8:00558287a4ef | 122 | if(degrees < 360) { |
jah128 | 8:00558287a4ef | 123 | float short_offset_multiplier = 1.0 + (0.9 / degrees); |
jah128 | 8:00558287a4ef | 124 | turn_time *= short_offset_multiplier; |
jah128 | 8:00558287a4ef | 125 | } |
jah128 | 8:00558287a4ef | 126 | |
jah128 | 8:00558287a4ef | 127 | //pc.printf("Speed: %f Turn time: %f\n",speed,turn_time); |
jah128 | 8:00558287a4ef | 128 | if(invert) speed=-speed; |
jah128 | 8:00558287a4ef | 129 | turn(speed); |
jah128 | 8:00558287a4ef | 130 | time_based_action_timeout.attach(&IF_end_time_based_action,turn_time); |
jah128 | 8:00558287a4ef | 131 | } |
jah128 | 8:00558287a4ef | 132 | } |
jah128 | 8:00558287a4ef | 133 | |
jah128 | 8:00558287a4ef | 134 | |
jah128 | 8:00558287a4ef | 135 | void IF_end_time_based_action() |
jah128 | 8:00558287a4ef | 136 | { |
jah128 | 8:00558287a4ef | 137 | brake(); |
jah128 | 8:00558287a4ef | 138 | } |
jah128 | 8:00558287a4ef | 139 | |
jah128 | 8:00558287a4ef | 140 | void IF_update_motors() |
jah128 | 8:00558287a4ef | 141 | { |
jah128 | 8:00558287a4ef | 142 | if(motor_left_speed > 1.0) { |
jah128 | 0:8a5497a2e366 | 143 | motor_left_speed = 1.0; |
jah128 | 8:00558287a4ef | 144 | //Throw exception... |
jah128 | 0:8a5497a2e366 | 145 | } |
jah128 | 8:00558287a4ef | 146 | if(motor_right_speed > 1.0) { |
jah128 | 8:00558287a4ef | 147 | motor_right_speed = 1.0; |
jah128 | 8:00558287a4ef | 148 | //Throw exception... |
jah128 | 8:00558287a4ef | 149 | } |
jah128 | 8:00558287a4ef | 150 | if(motor_left_speed < -1.0) { |
jah128 | 8:00558287a4ef | 151 | motor_left_speed = -1.0; |
jah128 | 8:00558287a4ef | 152 | //Throw exception... |
jah128 | 0:8a5497a2e366 | 153 | } |
jah128 | 8:00558287a4ef | 154 | if(motor_right_speed < -1.0) { |
jah128 | 8:00558287a4ef | 155 | motor_right_speed = -1.0; |
jah128 | 0:8a5497a2e366 | 156 | //Throw exception... |
jah128 | 8:00558287a4ef | 157 | } |
jah128 | 8:00558287a4ef | 158 | if(motor_left_brake) { |
jah128 | 8:00558287a4ef | 159 | motor_left_f.write(1); |
jah128 | 8:00558287a4ef | 160 | motor_left_r.write(1); |
jah128 | 8:00558287a4ef | 161 | if(motor_left_speed!=0) { |
jah128 | 8:00558287a4ef | 162 | motor_left_speed = 0; |
jah128 | 8:00558287a4ef | 163 | //Throw exception... |
jah128 | 8:00558287a4ef | 164 | } |
jah128 | 8:00558287a4ef | 165 | } else { |
jah128 | 8:00558287a4ef | 166 | if(motor_left_speed >= 0) { |
jah128 | 0:8a5497a2e366 | 167 | motor_left_f.write(0); |
jah128 | 0:8a5497a2e366 | 168 | motor_left_r.write(IF_calibrated_speed(motor_left_speed)); |
jah128 | 8:00558287a4ef | 169 | |
jah128 | 8:00558287a4ef | 170 | } else { |
jah128 | 0:8a5497a2e366 | 171 | motor_left_r.write(0); |
jah128 | 0:8a5497a2e366 | 172 | motor_left_f.write(IF_calibrated_speed(-motor_left_speed)); |
jah128 | 0:8a5497a2e366 | 173 | } |
jah128 | 0:8a5497a2e366 | 174 | } |
jah128 | 8:00558287a4ef | 175 | if(motor_right_brake) { |
jah128 | 8:00558287a4ef | 176 | motor_right_f.write(1); |
jah128 | 8:00558287a4ef | 177 | motor_right_r.write(1); |
jah128 | 8:00558287a4ef | 178 | if(motor_right_speed!=0) { |
jah128 | 8:00558287a4ef | 179 | motor_right_speed = 0; |
jah128 | 8:00558287a4ef | 180 | //Throw exception... |
jah128 | 8:00558287a4ef | 181 | } |
jah128 | 8:00558287a4ef | 182 | } else { |
jah128 | 8:00558287a4ef | 183 | if(motor_right_speed >= 0) { |
jah128 | 0:8a5497a2e366 | 184 | motor_right_f.write(0); |
jah128 | 0:8a5497a2e366 | 185 | motor_right_r.write(IF_calibrated_speed(motor_right_speed)); |
jah128 | 8:00558287a4ef | 186 | } else { |
jah128 | 0:8a5497a2e366 | 187 | motor_right_r.write(0); |
jah128 | 0:8a5497a2e366 | 188 | motor_right_f.write(IF_calibrated_speed(-motor_right_speed)); |
jah128 | 0:8a5497a2e366 | 189 | } |
jah128 | 0:8a5497a2e366 | 190 | } |
jah128 | 8:00558287a4ef | 191 | |
jah128 | 0:8a5497a2e366 | 192 | } |
jah128 | 0:8a5497a2e366 | 193 | |
jah128 | 0:8a5497a2e366 | 194 | |
jah128 | 8:00558287a4ef | 195 | float IF_calibrated_speed(float speed) |
jah128 | 8:00558287a4ef | 196 | { |
jah128 | 0:8a5497a2e366 | 197 | if(speed == 0) return 0; |
jah128 | 0:8a5497a2e366 | 198 | //Converts an input value to take account of the stall speed of the motor; aims to produce a more linear speed |
jah128 | 0:8a5497a2e366 | 199 | float adjusted = speed; |
jah128 | 8:00558287a4ef | 200 | if(OFFSET_MOTORS) { |
jah128 | 8:00558287a4ef | 201 | adjusted*=0.8f; |
jah128 | 8:00558287a4ef | 202 | adjusted+=0.2; |
jah128 | 8:00558287a4ef | 203 | } |
jah128 | 8:00558287a4ef | 204 | return adjusted; |
jah128 | 0:8a5497a2e366 | 205 | } |
jah128 | 0:8a5497a2e366 | 206 | |
jah128 | 8:00558287a4ef | 207 | void IF_init_motors() |
jah128 | 8:00558287a4ef | 208 | { |
jah128 | 0:8a5497a2e366 | 209 | // Motor PWM outputs work optimally at 25kHz frequency |
jah128 | 0:8a5497a2e366 | 210 | motor_left_f.period_us(40); |
jah128 | 8:00558287a4ef | 211 | motor_right_f.period_us(40); |
jah128 | 0:8a5497a2e366 | 212 | motor_left_r.period_us(40); |
jah128 | 0:8a5497a2e366 | 213 | motor_right_r.period_us(40); |
jah128 | 0:8a5497a2e366 | 214 | motor_left_speed = 0; |
jah128 | 0:8a5497a2e366 | 215 | motor_right_speed = 0; |
jah128 | 0:8a5497a2e366 | 216 | motor_left_brake = 0; |
jah128 | 8:00558287a4ef | 217 | motor_right_brake = 0; |
jah128 | 8:00558287a4ef | 218 | IF_update_motors(); |
jah128 | 0:8a5497a2e366 | 219 | } |