James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

PsiSwarm/motors.cpp@8:00558287a4ef, 2015-10-22 (annotated)

Committer:
jah128
Date:
Thu Oct 22 15:36:16 2015 +0000
Revision:
8:00558287a4ef
Parent:
6:ff3c66f7372b
Child:
9:085e090e1ec1
Added time_based_turn_degrees

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:8a5497a2e366 1 /* University of York Robotics Laboratory PsiSwarm Library: Motor Functions Source File
jah128 8:00558287a4ef 2 *
jah128 0:8a5497a2e366 3 * File: motors.cpp
jah128 0:8a5497a2e366 4 *
jah128 6:ff3c66f7372b 5 * (C) Dept. Electronics & Computer Science, University of York
jah128 6:ff3c66f7372b 6 * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
jah128 8:00558287a4ef 7 *
jah128 6:ff3c66f7372b 8 * PsiSwarm Library Version: 0.3
jah128 0:8a5497a2e366 9 *
jah128 6:ff3c66f7372b 10 * October 2015
jah128 0:8a5497a2e366 11 *
jah128 8:00558287a4ef 12 */
jah128 0:8a5497a2e366 13
jah128 0:8a5497a2e366 14 #include "psiswarm.h"
jah128 0:8a5497a2e366 15
jah128 8:00558287a4ef 16 Timeout time_based_action_timeout;
jah128 8:00558287a4ef 17
jah128 0:8a5497a2e366 18 void set_left_motor_speed(float speed)
jah128 0:8a5497a2e366 19 {
jah128 0:8a5497a2e366 20 motor_left_speed = speed;
jah128 0:8a5497a2e366 21 motor_left_brake = 0;
jah128 0:8a5497a2e366 22 IF_update_motors();
jah128 0:8a5497a2e366 23 }
jah128 0:8a5497a2e366 24
jah128 0:8a5497a2e366 25 void set_right_motor_speed(float speed)
jah128 0:8a5497a2e366 26 {
jah128 0:8a5497a2e366 27 motor_right_speed = speed;
jah128 0:8a5497a2e366 28 motor_right_brake = 0;
jah128 0:8a5497a2e366 29 IF_update_motors();
jah128 0:8a5497a2e366 30 }
jah128 0:8a5497a2e366 31
jah128 0:8a5497a2e366 32 void brake_left_motor()
jah128 0:8a5497a2e366 33 {
jah128 0:8a5497a2e366 34 motor_left_speed = 0;
jah128 0:8a5497a2e366 35 motor_left_brake = 1;
jah128 0:8a5497a2e366 36 IF_update_motors();
jah128 0:8a5497a2e366 37 }
jah128 0:8a5497a2e366 38
jah128 0:8a5497a2e366 39 void brake_right_motor()
jah128 0:8a5497a2e366 40 {
jah128 0:8a5497a2e366 41 motor_right_speed = 0;
jah128 0:8a5497a2e366 42 motor_right_brake = 1;
jah128 0:8a5497a2e366 43 IF_update_motors();
jah128 0:8a5497a2e366 44 }
jah128 0:8a5497a2e366 45
jah128 0:8a5497a2e366 46 void brake()
jah128 0:8a5497a2e366 47 {
jah128 0:8a5497a2e366 48 motor_left_speed = 0;
jah128 0:8a5497a2e366 49 motor_right_speed = 0;
jah128 0:8a5497a2e366 50 motor_left_brake = 1;
jah128 0:8a5497a2e366 51 motor_right_brake = 1;
jah128 0:8a5497a2e366 52 IF_update_motors();
jah128 0:8a5497a2e366 53 }
jah128 0:8a5497a2e366 54
jah128 0:8a5497a2e366 55 void stop()
jah128 0:8a5497a2e366 56 {
jah128 0:8a5497a2e366 57 motor_left_speed = 0;
jah128 0:8a5497a2e366 58 motor_right_speed = 0;
jah128 0:8a5497a2e366 59 motor_left_brake = 0;
jah128 0:8a5497a2e366 60 motor_right_brake = 0;
jah128 0:8a5497a2e366 61 IF_update_motors();
jah128 0:8a5497a2e366 62 }
jah128 0:8a5497a2e366 63
jah128 0:8a5497a2e366 64 void forward(float speed)
jah128 0:8a5497a2e366 65 {
jah128 0:8a5497a2e366 66 motor_left_speed = speed;
jah128 0:8a5497a2e366 67 motor_right_speed = speed;
jah128 0:8a5497a2e366 68 motor_left_brake = 0;
jah128 0:8a5497a2e366 69 motor_right_brake = 0;
jah128 0:8a5497a2e366 70 IF_update_motors();
jah128 0:8a5497a2e366 71 }
jah128 0:8a5497a2e366 72
jah128 0:8a5497a2e366 73 void backward(float speed)
jah128 0:8a5497a2e366 74 {
jah128 0:8a5497a2e366 75 motor_left_speed = -speed;
jah128 0:8a5497a2e366 76 motor_right_speed = -speed;
jah128 0:8a5497a2e366 77 motor_left_brake = 0;
jah128 0:8a5497a2e366 78 motor_right_brake = 0;
jah128 0:8a5497a2e366 79 IF_update_motors();
jah128 0:8a5497a2e366 80 }
jah128 0:8a5497a2e366 81
jah128 0:8a5497a2e366 82 void turn(float speed)
jah128 0:8a5497a2e366 83 {
jah128 0:8a5497a2e366 84 //A positive turn is clockwise
jah128 0:8a5497a2e366 85 motor_left_speed = speed;
jah128 0:8a5497a2e366 86 motor_right_speed = -speed;
jah128 0:8a5497a2e366 87 motor_left_brake = 0;
jah128 0:8a5497a2e366 88 motor_right_brake = 0;
jah128 8:00558287a4ef 89 IF_update_motors();
jah128 8:00558287a4ef 90 }
jah128 8:00558287a4ef 91
jah128 8:00558287a4ef 92 void time_based_turn(float speed, int microseconds)
jah128 8:00558287a4ef 93 {
jah128 0:8a5497a2e366 94 }
jah128 0:8a5497a2e366 95
jah128 8:00558287a4ef 96 void time_based_turn_degrees(float speed, float degrees)
jah128 8:00558287a4ef 97 {
jah128 8:00558287a4ef 98 if(speed < 0 || speed > 1 || degrees == 0) {
jah128 8:00558287a4ef 99 debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees);
jah128 8:00558287a4ef 100 } else {
jah128 8:00558287a4ef 101 char invert = 0;
jah128 8:00558287a4ef 102 if(degrees < 0) {degrees=-degrees; invert = 1;}
jah128 8:00558287a4ef 103
jah128 8:00558287a4ef 104 //Main calculation for turn time
jah128 8:00558287a4ef 105 float turn_time = degrees / ((290 * speed));
jah128 8:00558287a4ef 106
jah128 8:00558287a4ef 107 //Add a hard offset of 4ms to account for start\stop time
jah128 8:00558287a4ef 108 if(degrees > 4) {
jah128 8:00558287a4ef 109 turn_time += 0.004;
jah128 8:00558287a4ef 110 } else turn_time += 0.002;
jah128 8:00558287a4ef 111
jah128 8:00558287a4ef 112 // Add offset for slow speed
jah128 8:00558287a4ef 113 if(speed<0.31) {
jah128 8:00558287a4ef 114 float mul_fact = 0.31 - speed;
jah128 8:00558287a4ef 115 if(mul_fact < 0) mul_fact = 0;
jah128 8:00558287a4ef 116 mul_fact /= 2;
jah128 8:00558287a4ef 117 mul_fact += 1;
jah128 8:00558287a4ef 118 turn_time *= mul_fact;
jah128 8:00558287a4ef 119 }
jah128 8:00558287a4ef 120
jah128 8:00558287a4ef 121 // Add offset for short turns
jah128 8:00558287a4ef 122 if(degrees < 360) {
jah128 8:00558287a4ef 123 float short_offset_multiplier = 1.0 + (0.9 / degrees);
jah128 8:00558287a4ef 124 turn_time *= short_offset_multiplier;
jah128 8:00558287a4ef 125 }
jah128 8:00558287a4ef 126
jah128 8:00558287a4ef 127 //pc.printf("Speed: %f Turn time: %f\n",speed,turn_time);
jah128 8:00558287a4ef 128 if(invert) speed=-speed;
jah128 8:00558287a4ef 129 turn(speed);
jah128 8:00558287a4ef 130 time_based_action_timeout.attach(&IF_end_time_based_action,turn_time);
jah128 8:00558287a4ef 131 }
jah128 8:00558287a4ef 132 }
jah128 8:00558287a4ef 133
jah128 8:00558287a4ef 134
jah128 8:00558287a4ef 135 void IF_end_time_based_action()
jah128 8:00558287a4ef 136 {
jah128 8:00558287a4ef 137 brake();
jah128 8:00558287a4ef 138 }
jah128 8:00558287a4ef 139
jah128 8:00558287a4ef 140 void IF_update_motors()
jah128 8:00558287a4ef 141 {
jah128 8:00558287a4ef 142 if(motor_left_speed > 1.0) {
jah128 0:8a5497a2e366 143 motor_left_speed = 1.0;
jah128 8:00558287a4ef 144 //Throw exception...
jah128 0:8a5497a2e366 145 }
jah128 8:00558287a4ef 146 if(motor_right_speed > 1.0) {
jah128 8:00558287a4ef 147 motor_right_speed = 1.0;
jah128 8:00558287a4ef 148 //Throw exception...
jah128 8:00558287a4ef 149 }
jah128 8:00558287a4ef 150 if(motor_left_speed < -1.0) {
jah128 8:00558287a4ef 151 motor_left_speed = -1.0;
jah128 8:00558287a4ef 152 //Throw exception...
jah128 0:8a5497a2e366 153 }
jah128 8:00558287a4ef 154 if(motor_right_speed < -1.0) {
jah128 8:00558287a4ef 155 motor_right_speed = -1.0;
jah128 0:8a5497a2e366 156 //Throw exception...
jah128 8:00558287a4ef 157 }
jah128 8:00558287a4ef 158 if(motor_left_brake) {
jah128 8:00558287a4ef 159 motor_left_f.write(1);
jah128 8:00558287a4ef 160 motor_left_r.write(1);
jah128 8:00558287a4ef 161 if(motor_left_speed!=0) {
jah128 8:00558287a4ef 162 motor_left_speed = 0;
jah128 8:00558287a4ef 163 //Throw exception...
jah128 8:00558287a4ef 164 }
jah128 8:00558287a4ef 165 } else {
jah128 8:00558287a4ef 166 if(motor_left_speed >= 0) {
jah128 0:8a5497a2e366 167 motor_left_f.write(0);
jah128 0:8a5497a2e366 168 motor_left_r.write(IF_calibrated_speed(motor_left_speed));
jah128 8:00558287a4ef 169
jah128 8:00558287a4ef 170 } else {
jah128 0:8a5497a2e366 171 motor_left_r.write(0);
jah128 0:8a5497a2e366 172 motor_left_f.write(IF_calibrated_speed(-motor_left_speed));
jah128 0:8a5497a2e366 173 }
jah128 0:8a5497a2e366 174 }
jah128 8:00558287a4ef 175 if(motor_right_brake) {
jah128 8:00558287a4ef 176 motor_right_f.write(1);
jah128 8:00558287a4ef 177 motor_right_r.write(1);
jah128 8:00558287a4ef 178 if(motor_right_speed!=0) {
jah128 8:00558287a4ef 179 motor_right_speed = 0;
jah128 8:00558287a4ef 180 //Throw exception...
jah128 8:00558287a4ef 181 }
jah128 8:00558287a4ef 182 } else {
jah128 8:00558287a4ef 183 if(motor_right_speed >= 0) {
jah128 0:8a5497a2e366 184 motor_right_f.write(0);
jah128 0:8a5497a2e366 185 motor_right_r.write(IF_calibrated_speed(motor_right_speed));
jah128 8:00558287a4ef 186 } else {
jah128 0:8a5497a2e366 187 motor_right_r.write(0);
jah128 0:8a5497a2e366 188 motor_right_f.write(IF_calibrated_speed(-motor_right_speed));
jah128 0:8a5497a2e366 189 }
jah128 0:8a5497a2e366 190 }
jah128 8:00558287a4ef 191
jah128 0:8a5497a2e366 192 }
jah128 0:8a5497a2e366 193
jah128 0:8a5497a2e366 194
jah128 8:00558287a4ef 195 float IF_calibrated_speed(float speed)
jah128 8:00558287a4ef 196 {
jah128 0:8a5497a2e366 197 if(speed == 0) return 0;
jah128 0:8a5497a2e366 198 //Converts an input value to take account of the stall speed of the motor; aims to produce a more linear speed
jah128 0:8a5497a2e366 199 float adjusted = speed;
jah128 8:00558287a4ef 200 if(OFFSET_MOTORS) {
jah128 8:00558287a4ef 201 adjusted*=0.8f;
jah128 8:00558287a4ef 202 adjusted+=0.2;
jah128 8:00558287a4ef 203 }
jah128 8:00558287a4ef 204 return adjusted;
jah128 0:8a5497a2e366 205 }
jah128 0:8a5497a2e366 206
jah128 8:00558287a4ef 207 void IF_init_motors()
jah128 8:00558287a4ef 208 {
jah128 0:8a5497a2e366 209 // Motor PWM outputs work optimally at 25kHz frequency
jah128 0:8a5497a2e366 210 motor_left_f.period_us(40);
jah128 8:00558287a4ef 211 motor_right_f.period_us(40);
jah128 0:8a5497a2e366 212 motor_left_r.period_us(40);
jah128 0:8a5497a2e366 213 motor_right_r.period_us(40);
jah128 0:8a5497a2e366 214 motor_left_speed = 0;
jah128 0:8a5497a2e366 215 motor_right_speed = 0;
jah128 0:8a5497a2e366 216 motor_left_brake = 0;
jah128 8:00558287a4ef 217 motor_right_brake = 0;
jah128 8:00558287a4ef 218 IF_update_motors();
jah128 0:8a5497a2e366 219 }