Psi Swarm robot library version 0.9
Fork of PsiSwarmV9 by
motors.cpp@20:1bc6c6dc477b, 2018-05-14 (annotated)
- Committer:
- jah128
- Date:
- Mon May 14 15:35:38 2018 +0000
- Revision:
- 20:1bc6c6dc477b
- Parent:
- 18:9204f74069b4
Updated?
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 | 8:6c92789d5f87 | 2 | * |
jah128 | 16:50686c07ad07 | 3 | * Copyright 2017 University of York |
jah128 | 6:b340a527add9 | 4 | * |
jah128 | 8:6c92789d5f87 | 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 | 8:6c92789d5f87 | 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 | 16:50686c07ad07 | 16 | * PsiSwarm Library Version: 0.9 |
jah128 | 0:d6269d17c8cf | 17 | * |
jah128 | 16:50686c07ad07 | 18 | * June 2017 |
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 | 8:6c92789d5f87 | 28 | void Motors::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 | 8:6c92789d5f87 | 35 | void Motors::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 | 8:6c92789d5f87 | 42 | void Motors::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 | 8:6c92789d5f87 | 49 | void Motors::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 | 8:6c92789d5f87 | 56 | void Motors::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 | 8:6c92789d5f87 | 65 | void Motors::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 | 8:6c92789d5f87 | 74 | void Motors::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 | 8:6c92789d5f87 | 83 | void Motors::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 | 8:6c92789d5f87 | 92 | void Motors::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 | 8:6c92789d5f87 | 103 | void Motors::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 | 8:6c92789d5f87 | 107 | |
jah128 | 0:d6269d17c8cf | 108 | //Start moving |
jah128 | 0:d6269d17c8cf | 109 | forward(speed); |
jah128 | 0:d6269d17c8cf | 110 | brake_when_done = brake; |
jah128 | 8:6c92789d5f87 | 111 | time_based_action_timeout.attach_us(this,&Motors::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 | 8:6c92789d5f87 | 115 | void Motors::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 | 8:6c92789d5f87 | 119 | |
jah128 | 0:d6269d17c8cf | 120 | //Start turning |
jah128 | 0:d6269d17c8cf | 121 | turn(speed); |
jah128 | 0:d6269d17c8cf | 122 | brake_when_done = brake; |
jah128 | 8:6c92789d5f87 | 123 | time_based_action_timeout.attach_us(this,&Motors::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 | 8:6c92789d5f87 | 127 | float Motors::get_maximum_turn_angle(int microseconds) |
jah128 | 8:6c92789d5f87 | 128 | { |
jah128 | 0:d6269d17c8cf | 129 | //We can turn at about 270 degrees per second at full speed |
jah128 | 0:d6269d17c8cf | 130 | return (microseconds * 0.00027); |
jah128 | 0:d6269d17c8cf | 131 | } |
jah128 | 0:d6269d17c8cf | 132 | |
jah128 | 0:d6269d17c8cf | 133 | //Return the time in microseconds that performing the turn will take |
jah128 | 8:6c92789d5f87 | 134 | int Motors::get_time_based_turn_time(float speed, float degrees) |
jah128 | 0:d6269d17c8cf | 135 | { |
jah128 | 0:d6269d17c8cf | 136 | //Check sign of degrees |
jah128 | 0:d6269d17c8cf | 137 | if(degrees < 0) degrees =- degrees; |
jah128 | 8:6c92789d5f87 | 138 | |
jah128 | 0:d6269d17c8cf | 139 | //Main calculation for turn time |
jah128 | 0:d6269d17c8cf | 140 | float turn_time = degrees / ((290 * speed)); |
jah128 | 0:d6269d17c8cf | 141 | |
jah128 | 0:d6269d17c8cf | 142 | //Add a hard offset of 4ms to account for start\stop time |
jah128 | 0:d6269d17c8cf | 143 | if(degrees > 4) { |
jah128 | 8:6c92789d5f87 | 144 | turn_time += 0.004; |
jah128 | 0:d6269d17c8cf | 145 | } else turn_time += 0.002; |
jah128 | 0:d6269d17c8cf | 146 | |
jah128 | 0:d6269d17c8cf | 147 | // Add offset for slow speed |
jah128 | 0:d6269d17c8cf | 148 | if(speed<0.31) { |
jah128 | 0:d6269d17c8cf | 149 | float mul_fact = 0.31 - speed; |
jah128 | 0:d6269d17c8cf | 150 | if(mul_fact < 0) mul_fact = 0; |
jah128 | 0:d6269d17c8cf | 151 | mul_fact /= 2; |
jah128 | 0:d6269d17c8cf | 152 | mul_fact += 1; |
jah128 | 0:d6269d17c8cf | 153 | turn_time *= mul_fact; |
jah128 | 0:d6269d17c8cf | 154 | } |
jah128 | 0:d6269d17c8cf | 155 | |
jah128 | 0:d6269d17c8cf | 156 | // Add offset for short turns |
jah128 | 0:d6269d17c8cf | 157 | if(degrees < 360) { |
jah128 | 0:d6269d17c8cf | 158 | float short_offset_multiplier = 1.0 + (0.9 / degrees); |
jah128 | 0:d6269d17c8cf | 159 | turn_time *= short_offset_multiplier; |
jah128 | 0:d6269d17c8cf | 160 | } |
jah128 | 8:6c92789d5f87 | 161 | |
jah128 | 8:6c92789d5f87 | 162 | // Convert to uS |
jah128 | 0:d6269d17c8cf | 163 | turn_time *= 1000000; |
jah128 | 8:6c92789d5f87 | 164 | |
jah128 | 8:6c92789d5f87 | 165 | return (int) turn_time; |
jah128 | 0:d6269d17c8cf | 166 | } |
jah128 | 0:d6269d17c8cf | 167 | |
jah128 | 0:d6269d17c8cf | 168 | //Turn the robot a set number of degrees [using time estimation to end turn] |
jah128 | 8:6c92789d5f87 | 169 | int Motors::time_based_turn_degrees(float speed, float degrees, char brake) |
jah128 | 0:d6269d17c8cf | 170 | { |
jah128 | 0:d6269d17c8cf | 171 | if(speed < 0 || speed > 1 || degrees == 0) { |
jah128 | 12:878c6e9d9e60 | 172 | psi.debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees); |
jah128 | 0:d6269d17c8cf | 173 | return 0; |
jah128 | 0:d6269d17c8cf | 174 | } else { |
jah128 | 0:d6269d17c8cf | 175 | //Check if a current time based action is running - if it is, throw a warning and cancel its timeout |
jah128 | 0:d6269d17c8cf | 176 | IF_check_time_for_existing_time_based_action(); |
jah128 | 8:6c92789d5f87 | 177 | |
jah128 | 0:d6269d17c8cf | 178 | //Calculate turn time using get_time_based_turn_time |
jah128 | 0:d6269d17c8cf | 179 | int turn_time = get_time_based_turn_time(speed,degrees); |
jah128 | 8:6c92789d5f87 | 180 | |
jah128 | 0:d6269d17c8cf | 181 | //Set correct turn direction (-degrees is a counter-clockwise turn) |
jah128 | 8:6c92789d5f87 | 182 | if(degrees < 0) { |
jah128 | 0:d6269d17c8cf | 183 | degrees=-degrees; |
jah128 | 0:d6269d17c8cf | 184 | speed=-speed; |
jah128 | 0:d6269d17c8cf | 185 | } |
jah128 | 0:d6269d17c8cf | 186 | |
jah128 | 0:d6269d17c8cf | 187 | //Start turning |
jah128 | 0:d6269d17c8cf | 188 | turn(speed); |
jah128 | 8:6c92789d5f87 | 189 | |
jah128 | 0:d6269d17c8cf | 190 | brake_when_done = brake; |
jah128 | 8:6c92789d5f87 | 191 | time_based_action_timeout.attach_us(this,&Motors::IF_end_time_based_action,turn_time); |
jah128 | 0:d6269d17c8cf | 192 | return turn_time; |
jah128 | 0:d6269d17c8cf | 193 | } |
jah128 | 0:d6269d17c8cf | 194 | } |
jah128 | 0:d6269d17c8cf | 195 | |
jah128 | 0:d6269d17c8cf | 196 | //Check if a current time based action is running - if it is, throw a warning and cancel its timeout |
jah128 | 8:6c92789d5f87 | 197 | void Motors::IF_check_time_for_existing_time_based_action() |
jah128 | 0:d6269d17c8cf | 198 | { |
jah128 | 8:6c92789d5f87 | 199 | if(time_based_motor_action == 1) { |
jah128 | 8:6c92789d5f87 | 200 | time_based_action_timeout.detach(); |
jah128 | 12:878c6e9d9e60 | 201 | psi.debug("WARNING: New time-based action called before previous action finished!\n"); |
jah128 | 8:6c92789d5f87 | 202 | } else time_based_motor_action = 1; |
jah128 | 0:d6269d17c8cf | 203 | } |
jah128 | 0:d6269d17c8cf | 204 | |
jah128 | 8:6c92789d5f87 | 205 | void Motors::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 | 8:6c92789d5f87 | 212 | void Motors::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 | 8:6c92789d5f87 | 267 | float Motors::IF_calibrated_left_speed(float speed) |
jah128 | 4:1c621cb8cf0d | 268 | { |
jah128 | 18:9204f74069b4 | 269 | if(speed==0) return 0; |
jah128 | 4:1c621cb8cf0d | 270 | //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled |
jah128 | 18:9204f74069b4 | 271 | float adjusted = speed; |
jah128 | 18:9204f74069b4 | 272 | if(use_motor_calibration == 1) { |
jah128 | 4:1c621cb8cf0d | 273 | adjusted *= left_motor_calibration_value; |
jah128 | 4:1c621cb8cf0d | 274 | } |
jah128 | 18:9204f74069b4 | 275 | if(OFFSET_MOTORS == 1 || use_motor_calibration == 1){ |
jah128 | 18:9204f74069b4 | 276 | adjusted*=(1.0-left_motor_stall_offset); |
jah128 | 18:9204f74069b4 | 277 | adjusted+=left_motor_stall_offset; |
jah128 | 18:9204f74069b4 | 278 | } |
jah128 | 18:9204f74069b4 | 279 | if(adjusted<0) return 0; |
jah128 | 18:9204f74069b4 | 280 | if(adjusted>1) return 1; |
jah128 | 4:1c621cb8cf0d | 281 | return adjusted; |
jah128 | 4:1c621cb8cf0d | 282 | } |
jah128 | 4:1c621cb8cf0d | 283 | |
jah128 | 8:6c92789d5f87 | 284 | float Motors::IF_calibrated_right_speed(float speed) |
jah128 | 4:1c621cb8cf0d | 285 | { |
jah128 | 18:9204f74069b4 | 286 | if(speed==0) return 0; |
jah128 | 4:1c621cb8cf0d | 287 | //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled |
jah128 | 18:9204f74069b4 | 288 | float adjusted = speed; |
jah128 | 18:9204f74069b4 | 289 | if(use_motor_calibration == 1) { |
jah128 | 4:1c621cb8cf0d | 290 | adjusted *= right_motor_calibration_value; |
jah128 | 4:1c621cb8cf0d | 291 | } |
jah128 | 18:9204f74069b4 | 292 | if(OFFSET_MOTORS == 1 || use_motor_calibration == 1){ |
jah128 | 18:9204f74069b4 | 293 | adjusted*=(1.0-right_motor_stall_offset); |
jah128 | 18:9204f74069b4 | 294 | adjusted+=right_motor_stall_offset; |
jah128 | 18:9204f74069b4 | 295 | } |
jah128 | 18:9204f74069b4 | 296 | |
jah128 | 18:9204f74069b4 | 297 | if(adjusted<0) return 0; |
jah128 | 18:9204f74069b4 | 298 | if(adjusted>1) return 1; |
jah128 | 4:1c621cb8cf0d | 299 | return adjusted; |
jah128 | 4:1c621cb8cf0d | 300 | } |
jah128 | 4:1c621cb8cf0d | 301 | |
jah128 | 18:9204f74069b4 | 302 | /* Remove from version 0.9 as each motor has unique offset |
jah128 | 8:6c92789d5f87 | 303 | float Motors::IF_calibrated_speed(float speed) |
jah128 | 0:d6269d17c8cf | 304 | { |
jah128 | 0:d6269d17c8cf | 305 | if(speed == 0) return 0; |
jah128 | 0:d6269d17c8cf | 306 | //Converts an input value to take account of the stall speed of the motor; aims to produce a more linear speed |
jah128 | 0:d6269d17c8cf | 307 | float adjusted = speed; |
jah128 | 0:d6269d17c8cf | 308 | if(OFFSET_MOTORS) { |
jah128 | 0:d6269d17c8cf | 309 | adjusted*=0.8f; |
jah128 | 0:d6269d17c8cf | 310 | adjusted+=0.2; |
jah128 | 0:d6269d17c8cf | 311 | } |
jah128 | 0:d6269d17c8cf | 312 | return adjusted; |
jah128 | 0:d6269d17c8cf | 313 | } |
jah128 | 18:9204f74069b4 | 314 | */ |
jah128 | 18:9204f74069b4 | 315 | |
jah128 | 18:9204f74069b4 | 316 | |
jah128 | 18:9204f74069b4 | 317 | void Motors::calibrate_motors (void) |
jah128 | 18:9204f74069b4 | 318 | { |
jah128 | 18:9204f74069b4 | 319 | char restore_use_motor_calibration = use_motor_calibration; |
jah128 | 18:9204f74069b4 | 320 | //Turn off motor calibration whilst testing values! |
jah128 | 18:9204f74069b4 | 321 | use_motor_calibration = 0; |
jah128 | 18:9204f74069b4 | 322 | |
jah128 | 18:9204f74069b4 | 323 | |
jah128 | 18:9204f74069b4 | 324 | pc.printf("\nMotor Calibration Routine Started\n"); |
jah128 | 18:9204f74069b4 | 325 | display.clear_display(); |
jah128 | 18:9204f74069b4 | 326 | display.write_string("Starting motor"); |
jah128 | 18:9204f74069b4 | 327 | display.set_position(1,0); |
jah128 | 18:9204f74069b4 | 328 | display.write_string("calibration..."); |
jah128 | 18:9204f74069b4 | 329 | wait(1); |
jah128 | 18:9204f74069b4 | 330 | char left_offset = 0; |
jah128 | 18:9204f74069b4 | 331 | char right_offset = 0; |
jah128 | 18:9204f74069b4 | 332 | float left_calibration_value = 1; |
jah128 | 18:9204f74069b4 | 333 | float right_calibration_value = 1; |
jah128 | 18:9204f74069b4 | 334 | for(int i=0;i<2;i++){ |
jah128 | 18:9204f74069b4 | 335 | display.clear_display(); |
jah128 | 18:9204f74069b4 | 336 | if(i==0)display.write_string("LEFT MOTOR"); |
jah128 | 18:9204f74069b4 | 337 | else display.write_string("RIGHT MOTOR"); |
jah128 | 18:9204f74069b4 | 338 | display.set_position(1,0); |
jah128 | 18:9204f74069b4 | 339 | display.write_string("STALL SPEED"); |
jah128 | 18:9204f74069b4 | 340 | wait(1); |
jah128 | 18:9204f74069b4 | 341 | wait(1); |
jah128 | 18:9204f74069b4 | 342 | while(i2c_setup.IF_get_switch_state() != 0) { |
jah128 | 18:9204f74069b4 | 343 | display.clear_display(); |
jah128 | 18:9204f74069b4 | 344 | display.set_position(1,0); |
jah128 | 18:9204f74069b4 | 345 | display.write_string("RELEASE SWITCH!"); |
jah128 | 18:9204f74069b4 | 346 | wait(0.1); |
jah128 | 18:9204f74069b4 | 347 | } |
jah128 | 18:9204f74069b4 | 348 | display.set_position(1,0); |
jah128 | 18:9204f74069b4 | 349 | display.write_string(" "); |
jah128 | 18:9204f74069b4 | 350 | char a_msg [17]; |
jah128 | 18:9204f74069b4 | 351 | char cutoff = 0; |
jah128 | 18:9204f74069b4 | 352 | for(int j=5;j<40;j++){ |
jah128 | 18:9204f74069b4 | 353 | display.set_position(1,0); |
jah128 | 18:9204f74069b4 | 354 | sprintf(a_msg,"%d ",j); |
jah128 | 18:9204f74069b4 | 355 | display.write_string(a_msg); |
jah128 | 18:9204f74069b4 | 356 | float target_speed = j * 0.01; |
jah128 | 18:9204f74069b4 | 357 | if(i==0) set_left_motor_speed(target_speed); |
jah128 | 18:9204f74069b4 | 358 | else set_right_motor_speed(target_speed); |
jah128 | 18:9204f74069b4 | 359 | char s_count = 0; |
jah128 | 18:9204f74069b4 | 360 | char switch_state = i2c_setup.IF_get_switch_state(); |
jah128 | 18:9204f74069b4 | 361 | while(switch_state == 0 && s_count < 10) { |
jah128 | 18:9204f74069b4 | 362 | s_count ++; |
jah128 | 18:9204f74069b4 | 363 | switch_state = i2c_setup.IF_get_switch_state(); |
jah128 | 18:9204f74069b4 | 364 | wait(0.1); |
jah128 | 18:9204f74069b4 | 365 | } |
jah128 | 18:9204f74069b4 | 366 | if(s_count < 10){ |
jah128 | 18:9204f74069b4 | 367 | cutoff = j; |
jah128 | 18:9204f74069b4 | 368 | break; |
jah128 | 18:9204f74069b4 | 369 | } |
jah128 | 18:9204f74069b4 | 370 | motors.brake(); |
jah128 | 18:9204f74069b4 | 371 | wait(0.05); |
jah128 | 18:9204f74069b4 | 372 | } |
jah128 | 18:9204f74069b4 | 373 | if (cutoff > 0){ |
jah128 | 18:9204f74069b4 | 374 | if(i==0) left_offset = cutoff; |
jah128 | 18:9204f74069b4 | 375 | else right_offset = cutoff; |
jah128 | 18:9204f74069b4 | 376 | pc.printf("Motor stall speed indicated as %f\n",(cutoff * 0.01f)); |
jah128 | 18:9204f74069b4 | 377 | }else pc.printf("Error: Button not pressed\n"); |
jah128 | 18:9204f74069b4 | 378 | |
jah128 | 18:9204f74069b4 | 379 | } |
jah128 | 18:9204f74069b4 | 380 | |
jah128 | 18:9204f74069b4 | 381 | |
jah128 | 18:9204f74069b4 | 382 | display.clear_display(); |
jah128 | 18:9204f74069b4 | 383 | display.set_position(0,0); |
jah128 | 18:9204f74069b4 | 384 | display.write_string("^ REJECT"); |
jah128 | 18:9204f74069b4 | 385 | display.set_position(1,2); |
jah128 | 18:9204f74069b4 | 386 | display.write_string("ACCEPT"); |
jah128 | 18:9204f74069b4 | 387 | char switch_pos = i2c_setup.IF_get_switch_state(); |
jah128 | 18:9204f74069b4 | 388 | while(switch_pos != 1 && switch_pos != 2) switch_pos = i2c_setup.IF_get_switch_state(); |
jah128 | 18:9204f74069b4 | 389 | if(switch_pos == 2) { |
jah128 | 18:9204f74069b4 | 390 | pc.printf("\nChanges accepted: Updating firmware values in EEPROM\n"); |
jah128 | 18:9204f74069b4 | 391 | display.clear_display(); |
jah128 | 18:9204f74069b4 | 392 | display.set_position(0,0); |
jah128 | 18:9204f74069b4 | 393 | display.write_string("UPDATING"); |
jah128 | 18:9204f74069b4 | 394 | display.set_position(1,0); |
jah128 | 18:9204f74069b4 | 395 | display.write_string("FIRMWARE"); |
jah128 | 18:9204f74069b4 | 396 | wait(0.5); |
jah128 | 18:9204f74069b4 | 397 | |
jah128 | 18:9204f74069b4 | 398 | eprom.IF_write_motor_calibration_values(left_calibration_value, left_offset, right_calibration_value, right_offset); |
jah128 | 18:9204f74069b4 | 399 | wait(1.5); |
jah128 | 18:9204f74069b4 | 400 | } else { |
jah128 | 18:9204f74069b4 | 401 | pc.printf("\nChanges rejected.\n"); |
jah128 | 18:9204f74069b4 | 402 | } |
jah128 | 18:9204f74069b4 | 403 | display.clear_display(); |
jah128 | 18:9204f74069b4 | 404 | wait(0.5); |
jah128 | 18:9204f74069b4 | 405 | pc.printf("Motor calibration routine complete\n\n"); |
jah128 | 18:9204f74069b4 | 406 | //Restore motor calibration parameters |
jah128 | 18:9204f74069b4 | 407 | use_motor_calibration = restore_use_motor_calibration; |
jah128 | 18:9204f74069b4 | 408 | } |
jah128 | 18:9204f74069b4 | 409 | |
jah128 | 0:d6269d17c8cf | 410 | |
jah128 | 8:6c92789d5f87 | 411 | void Motors::init_motors() |
jah128 | 0:d6269d17c8cf | 412 | { |
jah128 | 0:d6269d17c8cf | 413 | // Motor PWM outputs work optimally at 25kHz frequency |
jah128 | 0:d6269d17c8cf | 414 | motor_left_f.period_us(40); |
jah128 | 0:d6269d17c8cf | 415 | motor_right_f.period_us(40); |
jah128 | 0:d6269d17c8cf | 416 | motor_left_r.period_us(40); |
jah128 | 0:d6269d17c8cf | 417 | motor_right_r.period_us(40); |
jah128 | 0:d6269d17c8cf | 418 | motor_left_speed = 0; |
jah128 | 0:d6269d17c8cf | 419 | motor_right_speed = 0; |
jah128 | 0:d6269d17c8cf | 420 | motor_left_brake = 0; |
jah128 | 0:d6269d17c8cf | 421 | motor_right_brake = 0; |
jah128 | 0:d6269d17c8cf | 422 | IF_update_motors(); |
jah128 | 0:d6269d17c8cf | 423 | } |