Psi Swarm Robot / PsiSwarmV8_CPP

C++ Library for the PsiSwarm Robot - Version 0.8

Dependents:   PsiSwarm_V8_Blank_CPP Autonomia_RndmWlk

Fork of PsiSwarmV7_CPP by Psi Swarm Robot

motors.cpp@4:1c621cb8cf0d, 2016-08-03 (annotated)

Committer:
jah128
Date:
Wed Aug 03 12:25:23 2016 +0000
Revision:
4:1c621cb8cf0d
Parent:
2:c6986ee3c7c5
Child:
5:3cdd1a37cdd7
test;

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 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 4:1c621cb8cf0d 8 * PsiSwarm Library Version: 0.5
jah128 0:d6269d17c8cf 9 *
jah128 4:1c621cb8cf0d 10 * April 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 }