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