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