Jonathan Wheadon / Mbed OS SOFT564Z_Group3

For coursework of group 3 in SOFT564Z

Dependencies:   Motordriver ros_lib_kinetic

Motors.cpp@6:3872858b7844, 2019-12-05 (annotated)

Committer:
firnenenrif
Date:
Thu Dec 05 12:56:34 2019 +0000
Revision:
6:3872858b7844
Parent:
4:8afc50a3e4ac
Child:
12:82b8fe254222
Added velocity control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jonathan738 4:8afc50a3e4ac 1 #include "mbed.h"
Jonathan738 0:3dfee562823a 2 #include "General.hpp"
Jonathan738 4:8afc50a3e4ac 3 #include "ros.h"
Jonathan738 4:8afc50a3e4ac 4 #include "rtos.h"
Jonathan738 4:8afc50a3e4ac 5 #include "ROS_Handler.hpp"
Jonathan738 4:8afc50a3e4ac 6 #include <motordriver.h>
Jonathan738 4:8afc50a3e4ac 7 #include "math.h"
Jonathan738 4:8afc50a3e4ac 8 #include "Motors.hpp"
Jonathan738 4:8afc50a3e4ac 9 #include "Battery_Monitor.hpp"
Jonathan738 4:8afc50a3e4ac 10 #include "Pins.h"
Jonathan738 0:3dfee562823a 11
Jonathan738 0:3dfee562823a 12 //Defines used for the encoder counting
Jonathan738 0:3dfee562823a 13 #define PREV_MASK 0x1 //Mask for the previous state in determining direction of rotation.
Jonathan738 0:3dfee562823a 14 #define CURR_MASK 0x2 //Mask for the current state in determining direction of rotation.
Jonathan738 0:3dfee562823a 15 #define INVALID 0x3 //XORing two states where both bits have changed.
Jonathan738 0:3dfee562823a 16
Jonathan738 0:3dfee562823a 17 Motor A(PB_13, PB_15, PC_6, 1); // pwm, fwd, rev, can brake
Jonathan738 0:3dfee562823a 18 Motor B(PB_4, PC_7, PA_15, 1); // pwm, fwd, rev, can brake
Jonathan738 0:3dfee562823a 19
Jonathan738 0:3dfee562823a 20 Serial pc(USBTX, USBRX);
Jonathan738 0:3dfee562823a 21
Jonathan738 0:3dfee562823a 22 InterruptIn encoderAchannel1(PB_12);
Jonathan738 0:3dfee562823a 23 InterruptIn encoderAchannel2(PA_4);
Jonathan738 0:3dfee562823a 24 InterruptIn encoderBchannel1(PB_5);
Jonathan738 0:3dfee562823a 25 InterruptIn encoderBchannel2(PB_3);
Jonathan738 0:3dfee562823a 26
Jonathan738 0:3dfee562823a 27 float radius = 3; //wheel radius in cm
Jonathan738 0:3dfee562823a 28 float bodyradius = 9.15; //radius of wheel centre from centre of bot, measured in vertical axis
Jonathan738 0:3dfee562823a 29 float benchmark = 3576; //number of encoder pulses per rotation of a wheel
Jonathan738 0:3dfee562823a 30 float pwm = -0.5; //the pwm constant that the wheels will use when moving
Jonathan738 0:3dfee562823a 31 float brakedist = 105; //the encoder target offset where the brakes will engage to prevent overshoot
Jonathan738 0:3dfee562823a 32 //^^ found using trial and error for a pwm value of 0.5 - will almost certainly vary if that is changed
Jonathan738 0:3dfee562823a 33
Jonathan738 0:3dfee562823a 34 volatile long acurrent = 0; //wheel A's current position in encoder counts
Jonathan738 0:3dfee562823a 35 int atarget = 0; //wheel A's current target position in encoder counts
Jonathan738 0:3dfee562823a 36 int aprevtarget = 0; //wheel A's previous target position in encoder counts
Jonathan738 0:3dfee562823a 37 float apwm = 0; //the pwm for wheel a, updated using the 'pwm' variable
Jonathan738 0:3dfee562823a 38 volatile int channelAstate1 = encoderAchannel1.read(); //read the encoder channels for motor A and record their states
Jonathan738 0:3dfee562823a 39 volatile int channelAstate2 = encoderAchannel2.read();
Jonathan738 0:3dfee562823a 40 int currentStateA = (channelAstate1 << 1) | (channelAstate2); //store a 2-bit state to track the current encoder readings
Jonathan738 0:3dfee562823a 41 int previousStateA = currentStateA;
Jonathan738 0:3dfee562823a 42 int nextmoveflagA = 1;
Jonathan738 0:3dfee562823a 43
Jonathan738 0:3dfee562823a 44 volatile long bcurrent = 0;
Jonathan738 0:3dfee562823a 45 int btarget = 0;
Jonathan738 0:3dfee562823a 46 int bprevtarget = 0;
Jonathan738 0:3dfee562823a 47 float bpwm = 0;
Jonathan738 0:3dfee562823a 48 volatile int channelBstate1 = encoderBchannel1.read();
Jonathan738 0:3dfee562823a 49 volatile int channelBstate2 = encoderBchannel2.read();
Jonathan738 0:3dfee562823a 50 int currentStateB = (channelBstate1 << 1) | (channelBstate2);
Jonathan738 0:3dfee562823a 51 int previousStateB = currentStateB;
Jonathan738 0:3dfee562823a 52 int nextmoveflagB = 1;
Jonathan738 0:3dfee562823a 53
firnenenrif 6:3872858b7844 54 //X is forward, Z is upwards, Y is sideways in this structure.
firnenenrif 6:3872858b7844 55 //Order is X, Y, Z linear, then X, Y, Z rotational, from 1:6.
firnenenrif 6:3872858b7844 56 void cmdvelresponse(const std_msgs::Int32MultiArray& vinput) {
firnenenrif 6:3872858b7844 57 //Extracting the only commands that will be used in the multiarray, and assuming that the int value attached to each point is 100 times
firnenenrif 6:3872858b7844 58 //the intended PWM constant for that movement command (e.g. a value of 50 in msg.data[6] would give a PWM duty cycle of 0.5 for rotation):
firnenenrif 6:3872858b7844 59 float xlin = 0.01 * vinput.data[1];
firnenenrif 6:3872858b7844 60 float zrot = 0.01 * vinput.data[6];
firnenenrif 6:3872858b7844 61 //assume rotation needs to be taken care of first, generally, then linear movement
firnenenrif 6:3872858b7844 62 if(zrot != 0) { //assume positive z is clockwise, negative is anticlockwise, A is left, B is right
firnenenrif 6:3872858b7844 63 A.speed(-zrot);
firnenenrif 6:3872858b7844 64 B.speed(zrot);
firnenenrif 6:3872858b7844 65 } else if(xlin != 0) {
firnenenrif 6:3872858b7844 66 A.speed(xlin);
firnenenrif 6:3872858b7844 67 B.speed(xlin);
firnenenrif 6:3872858b7844 68 } else {
firnenenrif 6:3872858b7844 69 A.stop(1);
firnenenrif 6:3872858b7844 70 B.stop(1);
firnenenrif 6:3872858b7844 71 }
firnenenrif 6:3872858b7844 72 }
firnenenrif 6:3872858b7844 73
Jonathan738 0:3dfee562823a 74 void initialize() {
Jonathan738 0:3dfee562823a 75 //every time a rising or falling edge is detected for the A or B channels, call the encoder counting interrupt for the respective wheel.
Jonathan738 0:3dfee562823a 76 //this corresponds to the counting program for the encoders for each wheel, given that quadrature encoding is used.
Jonathan738 0:3dfee562823a 77 encoderAchannel1.rise(&callbackA);
Jonathan738 0:3dfee562823a 78 encoderAchannel1.fall(&callbackA);
Jonathan738 0:3dfee562823a 79 encoderAchannel2.rise(&callbackA);
Jonathan738 0:3dfee562823a 80 encoderAchannel2.fall(&callbackA);
Jonathan738 0:3dfee562823a 81 encoderBchannel1.rise(&callbackB);
Jonathan738 0:3dfee562823a 82 encoderBchannel1.fall(&callbackB);
Jonathan738 0:3dfee562823a 83 encoderBchannel2.rise(&callbackB);
Jonathan738 0:3dfee562823a 84 encoderBchannel2.fall(&callbackB);
Jonathan738 0:3dfee562823a 85 //make sure the initial values for the encoders are all set correctly, and reflect their current real state.
Jonathan738 0:3dfee562823a 86 channelAstate1 = encoderAchannel1.read();
Jonathan738 0:3dfee562823a 87 channelAstate2 = encoderAchannel2.read();
Jonathan738 0:3dfee562823a 88 currentStateA = (channelAstate1 << 1) | (channelAstate2);
Jonathan738 0:3dfee562823a 89 previousStateA = currentStateA;
Jonathan738 0:3dfee562823a 90 channelBstate1 = encoderBchannel1.read();
Jonathan738 0:3dfee562823a 91 channelBstate2 = encoderBchannel2.read();
Jonathan738 0:3dfee562823a 92 currentStateB = (channelBstate1 << 1) | (channelBstate2);
Jonathan738 0:3dfee562823a 93 previousStateB = currentStateB;
Jonathan738 0:3dfee562823a 94 }
Jonathan738 0:3dfee562823a 95
Jonathan738 0:3dfee562823a 96 void callbackA() {
Jonathan738 0:3dfee562823a 97 ////////
Jonathan738 0:3dfee562823a 98 //insert the 'if X, carry out the rest of this code' debouncing check here, ensure the delay is minimal enough to avoid hindering 3576 measurements per rotation at max speed
Jonathan738 0:3dfee562823a 99 ////////
Jonathan738 0:3dfee562823a 100
Jonathan738 0:3dfee562823a 101 volatile int changeA = 0;
Jonathan738 0:3dfee562823a 102 channelAstate1 = encoderAchannel1.read();
Jonathan738 0:3dfee562823a 103 channelAstate2 = encoderAchannel2.read();
Jonathan738 0:3dfee562823a 104 currentStateA = (channelAstate1 << 1) | (channelAstate2);
Jonathan738 0:3dfee562823a 105
Jonathan738 0:3dfee562823a 106 if (((currentStateA ^ previousStateA) != INVALID) && (currentStateA != previousStateA)) {
Jonathan738 0:3dfee562823a 107 //2 bit state. Right hand bit of prev XOR left hand bit of current
Jonathan738 0:3dfee562823a 108 //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
Jonathan738 0:3dfee562823a 109 changeA = (previousStateA & PREV_MASK) ^ ((currentStateA & CURR_MASK) >> 1);
Jonathan738 0:3dfee562823a 110 if (changeA == 0) {
Jonathan738 0:3dfee562823a 111 changeA = -1;
Jonathan738 0:3dfee562823a 112 }
Jonathan738 0:3dfee562823a 113 acurrent -= changeA;
Jonathan738 0:3dfee562823a 114 }
Jonathan738 0:3dfee562823a 115 previousStateA = currentStateA;
Jonathan738 0:3dfee562823a 116 //if the encoder signal is incremented or decremented to the target value, immediately hit the brakes for this wheel
Jonathan738 0:3dfee562823a 117 if(acurrent <= atarget + brakedist && acurrent >= atarget - brakedist) {
Jonathan738 0:3dfee562823a 118 A.stop(1);
Jonathan738 0:3dfee562823a 119 nextmoveflagA = 1;
Jonathan738 0:3dfee562823a 120 //amend the target of the wheels, to prevent buildup of error from stopping at target+-brakedist instead of exactly target
Jonathan738 0:3dfee562823a 121 atarget = acurrent;
Jonathan738 0:3dfee562823a 122 }
Jonathan738 0:3dfee562823a 123 }
Jonathan738 0:3dfee562823a 124
Jonathan738 0:3dfee562823a 125 void callbackB() {
Jonathan738 0:3dfee562823a 126 ////////
Jonathan738 0:3dfee562823a 127 //insert the 'if X, carry out the rest of this code' debouncing check here, ensure the delay is minimal enough to avoid hindering 3576 measurements per rotation at max speed
Jonathan738 0:3dfee562823a 128 ////////
Jonathan738 0:3dfee562823a 129
Jonathan738 0:3dfee562823a 130 //every time this function is called, increment or decrement the encoder count depending on which direction the relevant wheel is moving
Jonathan738 0:3dfee562823a 131
Jonathan738 0:3dfee562823a 132 volatile int changeB = 0;
Jonathan738 0:3dfee562823a 133 channelBstate1 = encoderBchannel1.read();
Jonathan738 0:3dfee562823a 134 channelBstate2 = encoderBchannel2.read();
Jonathan738 0:3dfee562823a 135 currentStateB = (channelBstate1 << 1) | (channelBstate2);
Jonathan738 0:3dfee562823a 136
Jonathan738 0:3dfee562823a 137 if (((currentStateB ^ previousStateB) != INVALID) && (currentStateB != previousStateB)) {
Jonathan738 0:3dfee562823a 138 //2 bit state. Right hand bit of prev XOR left hand bit of current
Jonathan738 0:3dfee562823a 139 //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
Jonathan738 0:3dfee562823a 140 changeB = (previousStateB & PREV_MASK) ^ ((currentStateB & CURR_MASK) >> 1);
Jonathan738 0:3dfee562823a 141 if (changeB == 0) {
Jonathan738 0:3dfee562823a 142 changeB = -1;
Jonathan738 0:3dfee562823a 143 }
Jonathan738 0:3dfee562823a 144 bcurrent -= changeB;
Jonathan738 0:3dfee562823a 145 }
Jonathan738 0:3dfee562823a 146 previousStateB = currentStateB;
Jonathan738 0:3dfee562823a 147 //if the encoder signal is incremented or decremented to the target value, immediately hit the brakes for this wheel
Jonathan738 0:3dfee562823a 148 if(bcurrent <= btarget + brakedist && bcurrent >= btarget - brakedist) {
Jonathan738 0:3dfee562823a 149 B.stop(1);
Jonathan738 0:3dfee562823a 150 nextmoveflagB = 1;
Jonathan738 0:3dfee562823a 151 //amend the target of the wheels, to prevent buildup of error from stopping at target+-brakedist instead of exactly target
Jonathan738 0:3dfee562823a 152 btarget = bcurrent;
Jonathan738 0:3dfee562823a 153 }
Jonathan738 0:3dfee562823a 154 }
Jonathan738 0:3dfee562823a 155
Jonathan738 0:3dfee562823a 156 void move(const std_msgs::Int32& dinput) {
Jonathan738 0:3dfee562823a 157 if(nextmoveflagA == 1 && nextmoveflagB == 1) {
Jonathan738 0:3dfee562823a 158 //import the relevant ROS message and convert it to a usable encoder target
Jonathan738 0:3dfee562823a 159 float distance = dinput.data;
Jonathan738 0:3dfee562823a 160 float mtarget = distance/(2*3.142*radius)*benchmark;
Jonathan738 0:3dfee562823a 161 //set the encoder target as the interval for the wheels to move - must be an integer to match encoder readings
Jonathan738 0:3dfee562823a 162 atarget = ceil(mtarget + aprevtarget);
Jonathan738 0:3dfee562823a 163 btarget = ceil(mtarget + bprevtarget);
Jonathan738 0:3dfee562823a 164 //move the motors to their respective targets
Jonathan738 0:3dfee562823a 165 nextmoveflagA = 0;
Jonathan738 0:3dfee562823a 166 nextmoveflagB = 0;
Jonathan738 0:3dfee562823a 167 driveMotors();
Jonathan738 0:3dfee562823a 168 }
Jonathan738 0:3dfee562823a 169 }
Jonathan738 0:3dfee562823a 170
Jonathan738 0:3dfee562823a 171 void tempMove(float distance) {
Jonathan738 0:3dfee562823a 172 if(nextmoveflagA == 1 && nextmoveflagB == 1) {
Jonathan738 0:3dfee562823a 173 float mtarget = distance/(2*3.142*radius)*benchmark;
Jonathan738 0:3dfee562823a 174 atarget = ceil(mtarget + aprevtarget);
Jonathan738 0:3dfee562823a 175 btarget = ceil(mtarget + bprevtarget);
Jonathan738 0:3dfee562823a 176 pc.printf("Moving: A target is: %d B target is: %d M target is %f\n\r", atarget, btarget, mtarget);
Jonathan738 0:3dfee562823a 177 nextmoveflagA = 0;
Jonathan738 0:3dfee562823a 178 nextmoveflagB = 0;
Jonathan738 0:3dfee562823a 179 driveMotors();
Jonathan738 0:3dfee562823a 180 }
Jonathan738 0:3dfee562823a 181 }
Jonathan738 0:3dfee562823a 182
Jonathan738 0:3dfee562823a 183 void rotate(const std_msgs::Int32& rinput) {
Jonathan738 0:3dfee562823a 184 if(nextmoveflagA == 1 && nextmoveflagB == 1) {
Jonathan738 0:3dfee562823a 185 //import the relevant ROS message and convert it to usable encoder targets
Jonathan738 0:3dfee562823a 186 float degrees = rinput.data;
Jonathan738 0:3dfee562823a 187 float rtarget = (degrees/(360*3))*9.15*benchmark;
Jonathan738 0:3dfee562823a 188 //set the encoder targets as the intervals for the wheels to move - must be integers to match encoder readings
Jonathan738 0:3dfee562823a 189 atarget = ceil(rtarget + aprevtarget);
Jonathan738 0:3dfee562823a 190 btarget = ceil(-rtarget + bprevtarget);
Jonathan738 0:3dfee562823a 191 //move the motors to their respective targets
Jonathan738 0:3dfee562823a 192 nextmoveflagA = 0;
Jonathan738 0:3dfee562823a 193 nextmoveflagB = 0;
Jonathan738 0:3dfee562823a 194 driveMotors();
Jonathan738 0:3dfee562823a 195 }
Jonathan738 0:3dfee562823a 196 }
Jonathan738 0:3dfee562823a 197
Jonathan738 0:3dfee562823a 198 void tempRotate(float degrees) {
Jonathan738 0:3dfee562823a 199 if(nextmoveflagA == 1 && nextmoveflagB == 1) {
Jonathan738 0:3dfee562823a 200 float rtarget = (degrees/(360*3))*9.15*benchmark;
Jonathan738 0:3dfee562823a 201 atarget = ceil(rtarget + aprevtarget);
Jonathan738 0:3dfee562823a 202 btarget = ceil(-rtarget + bprevtarget);
Jonathan738 0:3dfee562823a 203 pc.printf("Rotating: A target is: %d B target is: %d R target is: %f\n\r", atarget, btarget, rtarget);
Jonathan738 0:3dfee562823a 204 nextmoveflagA = 0;
Jonathan738 0:3dfee562823a 205 nextmoveflagB = 0;
Jonathan738 0:3dfee562823a 206 driveMotors();
Jonathan738 0:3dfee562823a 207 }
Jonathan738 0:3dfee562823a 208 }
Jonathan738 0:3dfee562823a 209
Jonathan738 0:3dfee562823a 210 void driveMotors() {
Jonathan738 0:3dfee562823a 211 //depending on the direction the wheels have to move, set their pwms to either positive or negative so each motor moves correctly
Jonathan738 0:3dfee562823a 212 if(atarget > aprevtarget) {
Jonathan738 0:3dfee562823a 213 apwm = pwm;
Jonathan738 0:3dfee562823a 214 } else if(atarget < aprevtarget) {
Jonathan738 0:3dfee562823a 215 apwm = -pwm;
Jonathan738 0:3dfee562823a 216 } else if(atarget == aprevtarget) {
Jonathan738 0:3dfee562823a 217 apwm = 0;
Jonathan738 0:3dfee562823a 218 }
Jonathan738 0:3dfee562823a 219 if(btarget > bprevtarget) {
Jonathan738 0:3dfee562823a 220 bpwm = pwm;
Jonathan738 0:3dfee562823a 221 } else if(btarget < bprevtarget) {
Jonathan738 0:3dfee562823a 222 bpwm = -pwm;
Jonathan738 0:3dfee562823a 223 } else if(btarget == bprevtarget) {
Jonathan738 0:3dfee562823a 224 bpwm = 0;
Jonathan738 0:3dfee562823a 225 }
Jonathan738 0:3dfee562823a 226 //apply the pwm output to the motors
Jonathan738 0:3dfee562823a 227 pc.printf("Driving: A pwm is: %f B pwm is: %f\n\r", apwm, bpwm);
Jonathan738 0:3dfee562823a 228 A.speed(apwm);
Jonathan738 0:3dfee562823a 229 B.speed(bpwm);
Jonathan738 0:3dfee562823a 230 pc.printf("Driving: A target is: %d B target is: %d\n\r", atarget, btarget);
Jonathan738 0:3dfee562823a 231 pc.printf("Driving: A target was: %d B target was: %d\n\r", aprevtarget, bprevtarget);
firnenenrif 1:6a10e58b3d43 232 /**************************************************************************/
firnenenrif 1:6a10e58b3d43 233 //Note that this code prevents consecutive movement commands from being executed until the
firnenenrif 1:6a10e58b3d43 234 //previous one is done, but has the downside of keeping the thread busy during movement.
firnenenrif 1:6a10e58b3d43 235 //Feel free to replace it with a different method of waiting, if you want.
Jonathan738 0:3dfee562823a 236 while(nextmoveflagA == 0 || nextmoveflagB == 0) {
Jonathan738 0:3dfee562823a 237 wait_ms(500);
Jonathan738 0:3dfee562823a 238 }
firnenenrif 1:6a10e58b3d43 239 /**************************************************************************/
Jonathan738 0:3dfee562823a 240 //update the recordings of the previous command's movement targets
Jonathan738 0:3dfee562823a 241 aprevtarget = atarget;
Jonathan738 0:3dfee562823a 242 bprevtarget = btarget;
Jonathan738 3:7da9888ac8dc 243 }