Jonathan Wheadon / Mbed OS SOFT564Z_Group3

For coursework of group 3 in SOFT564Z

Dependencies:   Motordriver ros_lib_kinetic

Motors.cpp@1:6a10e58b3d43, 2019-11-28 (annotated)

Committer:
firnenenrif
Date:
Thu Nov 28 11:56:51 2019 +0000
Revision:
1:6a10e58b3d43
Parent:
0:3dfee562823a
Child:
3:7da9888ac8dc
Made changes to 'main.cpp', 'Motors.cpp' and 'ROS_Handler.cpp', adding the basic velocity control and moving a function out of the while loop in main. Changes marked visually using asterisks.

Who changed what in which revision?

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