Jonathan Wheadon / Mbed OS SOFT564Z_Group3

For coursework of group 3 in SOFT564Z

Dependencies:   Motordriver ros_lib_kinetic

Motors.cpp@12:82b8fe254222, 2020-01-05 (annotated)

Committer:
Jonathan738
Date:
Sun Jan 05 15:42:22 2020 +0000
Revision:
12:82b8fe254222
Parent:
6:3872858b7844 
Added working version of TOF code

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
Jonathan738 12:82b8fe254222 54 void initialize()
Jonathan738 12:82b8fe254222 55 {
Jonathan738 0:3dfee562823a 56 //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 57 //this corresponds to the counting program for the encoders for each wheel, given that quadrature encoding is used.
Jonathan738 0:3dfee562823a 58 encoderAchannel1.rise(&callbackA);
Jonathan738 0:3dfee562823a 59 encoderAchannel1.fall(&callbackA);
Jonathan738 0:3dfee562823a 60 encoderAchannel2.rise(&callbackA);
Jonathan738 0:3dfee562823a 61 encoderAchannel2.fall(&callbackA);
Jonathan738 0:3dfee562823a 62 encoderBchannel1.rise(&callbackB);
Jonathan738 0:3dfee562823a 63 encoderBchannel1.fall(&callbackB);
Jonathan738 0:3dfee562823a 64 encoderBchannel2.rise(&callbackB);
Jonathan738 0:3dfee562823a 65 encoderBchannel2.fall(&callbackB);
Jonathan738 0:3dfee562823a 66 //make sure the initial values for the encoders are all set correctly, and reflect their current real state.
Jonathan738 0:3dfee562823a 67 channelAstate1 = encoderAchannel1.read();
Jonathan738 0:3dfee562823a 68 channelAstate2 = encoderAchannel2.read();
Jonathan738 0:3dfee562823a 69 currentStateA = (channelAstate1 << 1) | (channelAstate2);
Jonathan738 0:3dfee562823a 70 previousStateA = currentStateA;
Jonathan738 0:3dfee562823a 71 channelBstate1 = encoderBchannel1.read();
Jonathan738 0:3dfee562823a 72 channelBstate2 = encoderBchannel2.read();
Jonathan738 0:3dfee562823a 73 currentStateB = (channelBstate1 << 1) | (channelBstate2);
Jonathan738 0:3dfee562823a 74 previousStateB = currentStateB;
Jonathan738 0:3dfee562823a 75 }
Jonathan738 0:3dfee562823a 76
Jonathan738 12:82b8fe254222 77 void callbackA()
Jonathan738 12:82b8fe254222 78 {
Jonathan738 0:3dfee562823a 79 ////////
Jonathan738 0:3dfee562823a 80 //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 81 ////////
Jonathan738 12:82b8fe254222 82
Jonathan738 0:3dfee562823a 83 volatile int changeA = 0;
Jonathan738 0:3dfee562823a 84 channelAstate1 = encoderAchannel1.read();
Jonathan738 0:3dfee562823a 85 channelAstate2 = encoderAchannel2.read();
Jonathan738 0:3dfee562823a 86 currentStateA = (channelAstate1 << 1) | (channelAstate2);
Jonathan738 12:82b8fe254222 87
Jonathan738 0:3dfee562823a 88 if (((currentStateA ^ previousStateA) != INVALID) && (currentStateA != previousStateA)) {
Jonathan738 0:3dfee562823a 89 //2 bit state. Right hand bit of prev XOR left hand bit of current
Jonathan738 0:3dfee562823a 90 //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
Jonathan738 0:3dfee562823a 91 changeA = (previousStateA & PREV_MASK) ^ ((currentStateA & CURR_MASK) >> 1);
Jonathan738 0:3dfee562823a 92 if (changeA == 0) {
Jonathan738 0:3dfee562823a 93 changeA = -1;
Jonathan738 0:3dfee562823a 94 }
Jonathan738 0:3dfee562823a 95 acurrent -= changeA;
Jonathan738 0:3dfee562823a 96 }
Jonathan738 0:3dfee562823a 97 previousStateA = currentStateA;
Jonathan738 0:3dfee562823a 98 //if the encoder signal is incremented or decremented to the target value, immediately hit the brakes for this wheel
Jonathan738 0:3dfee562823a 99 if(acurrent <= atarget + brakedist && acurrent >= atarget - brakedist) {
Jonathan738 0:3dfee562823a 100 A.stop(1);
Jonathan738 0:3dfee562823a 101 nextmoveflagA = 1;
Jonathan738 0:3dfee562823a 102 //amend the target of the wheels, to prevent buildup of error from stopping at target+-brakedist instead of exactly target
Jonathan738 0:3dfee562823a 103 atarget = acurrent;
Jonathan738 0:3dfee562823a 104 }
Jonathan738 0:3dfee562823a 105 }
Jonathan738 0:3dfee562823a 106
Jonathan738 12:82b8fe254222 107 void callbackB()
Jonathan738 12:82b8fe254222 108 {
Jonathan738 0:3dfee562823a 109 ////////
Jonathan738 0:3dfee562823a 110 //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 111 ////////
Jonathan738 12:82b8fe254222 112
Jonathan738 0:3dfee562823a 113 //every time this function is called, increment or decrement the encoder count depending on which direction the relevant wheel is moving
Jonathan738 12:82b8fe254222 114
Jonathan738 0:3dfee562823a 115 volatile int changeB = 0;
Jonathan738 0:3dfee562823a 116 channelBstate1 = encoderBchannel1.read();
Jonathan738 0:3dfee562823a 117 channelBstate2 = encoderBchannel2.read();
Jonathan738 0:3dfee562823a 118 currentStateB = (channelBstate1 << 1) | (channelBstate2);
Jonathan738 12:82b8fe254222 119
Jonathan738 0:3dfee562823a 120 if (((currentStateB ^ previousStateB) != INVALID) && (currentStateB != previousStateB)) {
Jonathan738 0:3dfee562823a 121 //2 bit state. Right hand bit of prev XOR left hand bit of current
Jonathan738 0:3dfee562823a 122 //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
Jonathan738 0:3dfee562823a 123 changeB = (previousStateB & PREV_MASK) ^ ((currentStateB & CURR_MASK) >> 1);
Jonathan738 0:3dfee562823a 124 if (changeB == 0) {
Jonathan738 0:3dfee562823a 125 changeB = -1;
Jonathan738 0:3dfee562823a 126 }
Jonathan738 0:3dfee562823a 127 bcurrent -= changeB;
Jonathan738 0:3dfee562823a 128 }
Jonathan738 0:3dfee562823a 129 previousStateB = currentStateB;
Jonathan738 0:3dfee562823a 130 //if the encoder signal is incremented or decremented to the target value, immediately hit the brakes for this wheel
Jonathan738 0:3dfee562823a 131 if(bcurrent <= btarget + brakedist && bcurrent >= btarget - brakedist) {
Jonathan738 0:3dfee562823a 132 B.stop(1);
Jonathan738 0:3dfee562823a 133 nextmoveflagB = 1;
Jonathan738 0:3dfee562823a 134 //amend the target of the wheels, to prevent buildup of error from stopping at target+-brakedist instead of exactly target
Jonathan738 0:3dfee562823a 135 btarget = bcurrent;
Jonathan738 0:3dfee562823a 136 }
Jonathan738 0:3dfee562823a 137 }
Jonathan738 0:3dfee562823a 138
Jonathan738 12:82b8fe254222 139 void move(const std_msgs::Int32& dinput)
Jonathan738 12:82b8fe254222 140 {
Jonathan738 0:3dfee562823a 141 if(nextmoveflagA == 1 && nextmoveflagB == 1) {
Jonathan738 0:3dfee562823a 142 //import the relevant ROS message and convert it to a usable encoder target
Jonathan738 0:3dfee562823a 143 float distance = dinput.data;
Jonathan738 0:3dfee562823a 144 float mtarget = distance/(2*3.142*radius)*benchmark;
Jonathan738 0:3dfee562823a 145 //set the encoder target as the interval for the wheels to move - must be an integer to match encoder readings
Jonathan738 0:3dfee562823a 146 atarget = ceil(mtarget + aprevtarget);
Jonathan738 0:3dfee562823a 147 btarget = ceil(mtarget + bprevtarget);
Jonathan738 0:3dfee562823a 148 //move the motors to their respective targets
Jonathan738 0:3dfee562823a 149 nextmoveflagA = 0;
Jonathan738 0:3dfee562823a 150 nextmoveflagB = 0;
Jonathan738 0:3dfee562823a 151 driveMotors();
Jonathan738 0:3dfee562823a 152 }
Jonathan738 0:3dfee562823a 153 }
Jonathan738 0:3dfee562823a 154
Jonathan738 12:82b8fe254222 155 void tempMove(float distance)
Jonathan738 12:82b8fe254222 156 {
Jonathan738 0:3dfee562823a 157 if(nextmoveflagA == 1 && nextmoveflagB == 1) {
Jonathan738 0:3dfee562823a 158 float mtarget = distance/(2*3.142*radius)*benchmark;
Jonathan738 0:3dfee562823a 159 atarget = ceil(mtarget + aprevtarget);
Jonathan738 0:3dfee562823a 160 btarget = ceil(mtarget + bprevtarget);
Jonathan738 0:3dfee562823a 161 pc.printf("Moving: A target is: %d B target is: %d M target is %f\n\r", atarget, btarget, mtarget);
Jonathan738 0:3dfee562823a 162 nextmoveflagA = 0;
Jonathan738 0:3dfee562823a 163 nextmoveflagB = 0;
Jonathan738 0:3dfee562823a 164 driveMotors();
Jonathan738 0:3dfee562823a 165 }
Jonathan738 0:3dfee562823a 166 }
Jonathan738 0:3dfee562823a 167
Jonathan738 12:82b8fe254222 168 void rotate(const std_msgs::Int32& rinput)
Jonathan738 12:82b8fe254222 169 {
Jonathan738 0:3dfee562823a 170 if(nextmoveflagA == 1 && nextmoveflagB == 1) {
Jonathan738 0:3dfee562823a 171 //import the relevant ROS message and convert it to usable encoder targets
Jonathan738 0:3dfee562823a 172 float degrees = rinput.data;
Jonathan738 0:3dfee562823a 173 float rtarget = (degrees/(360*3))*9.15*benchmark;
Jonathan738 0:3dfee562823a 174 //set the encoder targets as the intervals for the wheels to move - must be integers to match encoder readings
Jonathan738 0:3dfee562823a 175 atarget = ceil(rtarget + aprevtarget);
Jonathan738 0:3dfee562823a 176 btarget = ceil(-rtarget + bprevtarget);
Jonathan738 0:3dfee562823a 177 //move the motors to their respective targets
Jonathan738 0:3dfee562823a 178 nextmoveflagA = 0;
Jonathan738 0:3dfee562823a 179 nextmoveflagB = 0;
Jonathan738 0:3dfee562823a 180 driveMotors();
Jonathan738 0:3dfee562823a 181 }
Jonathan738 0:3dfee562823a 182 }
Jonathan738 0:3dfee562823a 183
Jonathan738 12:82b8fe254222 184 void tempRotate(float degrees)
Jonathan738 12:82b8fe254222 185 {
Jonathan738 0:3dfee562823a 186 if(nextmoveflagA == 1 && nextmoveflagB == 1) {
Jonathan738 0:3dfee562823a 187 float rtarget = (degrees/(360*3))*9.15*benchmark;
Jonathan738 0:3dfee562823a 188 atarget = ceil(rtarget + aprevtarget);
Jonathan738 0:3dfee562823a 189 btarget = ceil(-rtarget + bprevtarget);
Jonathan738 0:3dfee562823a 190 pc.printf("Rotating: A target is: %d B target is: %d R target is: %f\n\r", atarget, btarget, rtarget);
Jonathan738 0:3dfee562823a 191 nextmoveflagA = 0;
Jonathan738 0:3dfee562823a 192 nextmoveflagB = 0;
Jonathan738 0:3dfee562823a 193 driveMotors();
Jonathan738 0:3dfee562823a 194 }
Jonathan738 0:3dfee562823a 195 }
Jonathan738 0:3dfee562823a 196
Jonathan738 12:82b8fe254222 197 void driveMotors()
Jonathan738 12:82b8fe254222 198 {
Jonathan738 0:3dfee562823a 199 //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 200 if(atarget > aprevtarget) {
Jonathan738 0:3dfee562823a 201 apwm = pwm;
Jonathan738 0:3dfee562823a 202 } else if(atarget < aprevtarget) {
Jonathan738 0:3dfee562823a 203 apwm = -pwm;
Jonathan738 0:3dfee562823a 204 } else if(atarget == aprevtarget) {
Jonathan738 0:3dfee562823a 205 apwm = 0;
Jonathan738 0:3dfee562823a 206 }
Jonathan738 0:3dfee562823a 207 if(btarget > bprevtarget) {
Jonathan738 0:3dfee562823a 208 bpwm = pwm;
Jonathan738 0:3dfee562823a 209 } else if(btarget < bprevtarget) {
Jonathan738 0:3dfee562823a 210 bpwm = -pwm;
Jonathan738 0:3dfee562823a 211 } else if(btarget == bprevtarget) {
Jonathan738 0:3dfee562823a 212 bpwm = 0;
Jonathan738 0:3dfee562823a 213 }
Jonathan738 0:3dfee562823a 214 //apply the pwm output to the motors
Jonathan738 0:3dfee562823a 215 pc.printf("Driving: A pwm is: %f B pwm is: %f\n\r", apwm, bpwm);
Jonathan738 0:3dfee562823a 216 A.speed(apwm);
Jonathan738 0:3dfee562823a 217 B.speed(bpwm);
Jonathan738 0:3dfee562823a 218 pc.printf("Driving: A target is: %d B target is: %d\n\r", atarget, btarget);
Jonathan738 0:3dfee562823a 219 pc.printf("Driving: A target was: %d B target was: %d\n\r", aprevtarget, bprevtarget);
firnenenrif 1:6a10e58b3d43 220 /**************************************************************************/
firnenenrif 1:6a10e58b3d43 221 //Note that this code prevents consecutive movement commands from being executed until the
firnenenrif 1:6a10e58b3d43 222 //previous one is done, but has the downside of keeping the thread busy during movement.
firnenenrif 1:6a10e58b3d43 223 //Feel free to replace it with a different method of waiting, if you want.
Jonathan738 0:3dfee562823a 224 while(nextmoveflagA == 0 || nextmoveflagB == 0) {
Jonathan738 0:3dfee562823a 225 wait_ms(500);
Jonathan738 0:3dfee562823a 226 }
firnenenrif 1:6a10e58b3d43 227 /**************************************************************************/
Jonathan738 0:3dfee562823a 228 //update the recordings of the previous command's movement targets
Jonathan738 0:3dfee562823a 229 aprevtarget = atarget;
Jonathan738 0:3dfee562823a 230 bprevtarget = btarget;
Jonathan738 3:7da9888ac8dc 231 }
Jonathan738 12:82b8fe254222 232