James Hilder / PsiSwarmV9-Fork

Psi Swarm robot library version 0.9

Fork of PsiSwarmV9 by Psi Swarm Robot

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?

UserRevisionLine numberNew 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 }