Tristan Vlogman / Mbed 2 deprecated locomotion_pid_action_refactor_EMG

Control up to two motors using filtered EMG signals and a PID controller

Dependencies:   FastPWM HIDScope MODSERIAL QEI Matrix biquadFilter controller errorFetch mbed motorConfig refGen MatrixMath inverseKinematics

Fork of Minor_test_serial by First Last

main.cpp@44:d157094b48d5, 2017-11-01 (annotated)

Committer:
tvlogman
Date:
Wed Nov 01 16:17:21 2017 +0000
Revision:
44:d157094b48d5
Parent:
43:dd0888f86357 
potmeter controlled robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tvlogman 33:6f4858b98fe5 1 #include <vector>
tvlogman 37:633dd1901681 2 #include <numeric>
tvlogman 42:ae78ff03d9d6 3 #include <algorithm>
vsluiter 0:c8f15874531b 4 #include "mbed.h"
tvlogman 39:d065ad7a978d 5 #include "Matrix.h"
vsluiter 0:c8f15874531b 6 #include "MODSERIAL.h"
tvlogman 8:0067469c3389 7 #include "HIDScope.h"
tvlogman 9:5f0e796c9489 8 #include "QEI.h"
tvlogman 15:b76b8cff4d8f 9 #include "FastPWM.h"
tvlogman 29:9aa4d63a9bd1 10 #include "refGen.h"
tvlogman 30:65f0c9ecf810 11 #include "controller.h"
tvlogman 31:cc08254ab7b5 12 #include "motorConfig.h"
tvlogman 32:1bb406d2b3c3 13 #include "errorFetch.h"
tvlogman 39:d065ad7a978d 14 #include "BiQuad.h"
tvlogman 43:dd0888f86357 15 #include "inverseKinematics.h"
tvlogman 15:b76b8cff4d8f 16
tvlogman 33:6f4858b98fe5 17 // ADJUSTABLE PARAMETERS
tvlogman 43:dd0888f86357 18 // robot dimensions
tvlogman 43:dd0888f86357 19 const float L1 = 0.391;
tvlogman 43:dd0888f86357 20 const float L2 = 0.391;
tvlogman 43:dd0888f86357 21
tvlogman 37:633dd1901681 22 // controller ticker time interval
tvlogman 43:dd0888f86357 23 const float Ts = 0.008;
tvlogman 43:dd0888f86357 24
tvlogman 43:dd0888f86357 25 // Defining an inverse-kinematics calculator
tvlogman 43:dd0888f86357 26 inverseKinematics robotKinematics(L1,L2,Ts);
tvlogman 27:a4228ea8fb8f 27
tvlogman 34:1a70aa045c8f 28 // EMG filter parameters
tvlogman 37:633dd1901681 29 // calibration time
tvlogman 42:ae78ff03d9d6 30 const int calSamples = 1000;
tvlogman 37:633dd1901681 31
tvlogman 43:dd0888f86357 32 // KINEMATICS reference motor position
tvlogman 43:dd0888f86357 33 volatile double Mp1C = 0;
tvlogman 43:dd0888f86357 34 volatile double Mp2C = 0;
tvlogman 43:dd0888f86357 35
tvlogman 41:9678fd827d25 36 // Initialize average and max EMG value for calibration to 0 and 1 respectively
tvlogman 37:633dd1901681 37 volatile float avgEMGvalue = 0;
tvlogman 41:9678fd827d25 38 volatile double maxEMGvalue = 1;
tvlogman 34:1a70aa045c8f 39
tvlogman 27:a4228ea8fb8f 40 // Controller parameters
tvlogman 43:dd0888f86357 41 const float k_p = 0.01;
tvlogman 27:a4228ea8fb8f 42 const float k_i = 0; // Still needs a reasonable value
tvlogman 27:a4228ea8fb8f 43 const float k_d = 0; // Again, still need to pick a reasonable value
tvlogman 27:a4228ea8fb8f 44
tvlogman 33:6f4858b98fe5 45 // Defining motor gear ratio - for BOTH motors as this is the same in the current configuration
tvlogman 33:6f4858b98fe5 46 const float gearRatio = 131;
tvlogman 33:6f4858b98fe5 47
tvlogman 37:633dd1901681 48 // LOGISTICS
tvlogman 37:633dd1901681 49 // Declaring finite-state-machine states
tvlogman 43:dd0888f86357 50 enum robotStates {KILLED, ACTIVE};
tvlogman 37:633dd1901681 51 volatile robotStates currentState = KILLED;
tvlogman 37:633dd1901681 52 volatile bool stateChanged = true;
tvlogman 33:6f4858b98fe5 53
tvlogman 33:6f4858b98fe5 54 // Declaring a controller ticker and volatile variables to store encoder counts and revs
tvlogman 33:6f4858b98fe5 55 Ticker controllerTicker;
tvlogman 33:6f4858b98fe5 56 volatile int m1counts = 0;
tvlogman 33:6f4858b98fe5 57 volatile int m2counts = 0;
tvlogman 33:6f4858b98fe5 58 volatile float m1revs = 0.00;
tvlogman 33:6f4858b98fe5 59 volatile float m2revs = 0.00;
tvlogman 33:6f4858b98fe5 60
tvlogman 33:6f4858b98fe5 61 // PWM settings
tvlogman 33:6f4858b98fe5 62 float pwmPeriod = 1.0/5000.0;
tvlogman 33:6f4858b98fe5 63 int frequency_pwm = 10000; //10kHz PWM
tvlogman 33:6f4858b98fe5 64
tvlogman 33:6f4858b98fe5 65 // Initializing encoder
tvlogman 32:1bb406d2b3c3 66 QEI Encoder1(D12,D13,NC,64, QEI::X4_ENCODING);
tvlogman 32:1bb406d2b3c3 67 QEI Encoder2(D11,D10,NC,64, QEI::X4_ENCODING);
tvlogman 10:e23cbcdde7e3 68 MODSERIAL pc(USBTX, USBRX);
tvlogman 27:a4228ea8fb8f 69 HIDScope scope(5);
tvlogman 8:0067469c3389 70
tvlogman 14:664870b5d153 71 // Defining inputs
tvlogman 14:664870b5d153 72 InterruptIn sw2(SW2);
tvlogman 15:b76b8cff4d8f 73 InterruptIn sw3(SW3);
tvlogman 16:27430afe663e 74 InterruptIn button1(D2);
tvlogman 28:8cd898ff43a2 75 InterruptIn button2(D3);
tvlogman 15:b76b8cff4d8f 76
tvlogman 37:633dd1901681 77 // Defining LED outputs to indicate robot state-us
tvlogman 37:633dd1901681 78 DigitalOut ledG(LED_GREEN);
tvlogman 37:633dd1901681 79 DigitalOut ledR(LED_RED);
tvlogman 37:633dd1901681 80 DigitalOut ledB(LED_BLUE);
tvlogman 37:633dd1901681 81
tvlogman 27:a4228ea8fb8f 82 // Setting up HIDscope
tvlogman 16:27430afe663e 83 volatile float x;
tvlogman 27:a4228ea8fb8f 84 volatile float y;
tvlogman 27:a4228ea8fb8f 85 volatile float z;
tvlogman 27:a4228ea8fb8f 86 volatile float q;
tvlogman 27:a4228ea8fb8f 87 volatile float k;
tvlogman 39:d065ad7a978d 88 volatile float w;
tvlogman 27:a4228ea8fb8f 89
tvlogman 39:d065ad7a978d 90
tvlogman 43:dd0888f86357 91 void sendDataToPc(float data1, float data2, float data3, float data4){
tvlogman 27:a4228ea8fb8f 92 // Capture data
tvlogman 27:a4228ea8fb8f 93 x = data1;
tvlogman 27:a4228ea8fb8f 94 y = data2;
tvlogman 27:a4228ea8fb8f 95 z = data3;
tvlogman 27:a4228ea8fb8f 96 q = data4;
tvlogman 27:a4228ea8fb8f 97 scope.set(0, x);
tvlogman 27:a4228ea8fb8f 98 scope.set(1, y);
tvlogman 27:a4228ea8fb8f 99 scope.set(2, z);
tvlogman 27:a4228ea8fb8f 100 scope.set(3, q);
tvlogman 27:a4228ea8fb8f 101 scope.set(4, z);
tvlogman 39:d065ad7a978d 102 scope.set(5, w);
tvlogman 27:a4228ea8fb8f 103 scope.send(); // send what's in scope memory to PC
tvlogman 27:a4228ea8fb8f 104 }
tvlogman 14:664870b5d153 105
tvlogman 7:1bffab95fc5f 106
tvlogman 33:6f4858b98fe5 107 // REFERENCE PARAMETERS
tvlogman 42:ae78ff03d9d6 108 int posRevRange = 1; // describes the ends of the position range in complete motor output shaft revolutions in both directions
tvlogman 42:ae78ff03d9d6 109 const float maxAngle = 1*3.14*posRevRange; // max angle in radians
tvlogman 37:633dd1901681 110
tvlogman 20:4ce3fb543a45 111
tvlogman 43:dd0888f86357 112 // References based on potmeter 1 and 2
tvlogman 43:dd0888f86357 113 // Set Vx using pot1 = A5
tvlogman 43:dd0888f86357 114 // Set Vy using pot2 = A4
tvlogman 43:dd0888f86357 115 refGen ref1(A4, 0.1);
tvlogman 43:dd0888f86357 116 refGen ref2(A3, 0.1);
tvlogman 19:f08b5cd2b7ce 117
tvlogman 21:d266d1e503ce 118 // readEncoder reads counts and revs and logs results to serial window
tvlogman 34:1a70aa045c8f 119 errorFetch e1(gearRatio, Ts);
tvlogman 38:f1d2d42a4bdc 120 errorFetch e2(gearRatio, Ts);
tvlogman 21:d266d1e503ce 121
tvlogman 31:cc08254ab7b5 122 // Generate a PID controller with the specified values of k_p, k_d and k_i
tvlogman 30:65f0c9ecf810 123 controller motorController1(k_p, k_d, k_i);
tvlogman 38:f1d2d42a4bdc 124 controller motorController2(k_p, k_d, k_i);
tvlogman 38:f1d2d42a4bdc 125
tvlogman 37:633dd1901681 126 motorConfig motor1(D4,D5);
tvlogman 37:633dd1901681 127 motorConfig motor2(D7,D6);
tvlogman 37:633dd1901681 128
tvlogman 37:633dd1901681 129 // PROBLEM: if I'm processing the state machine in the endless while loop, how can I adjust robot behavior in the ticker (as it'll keep running)? Do I need to also implement it there? If so, why bother with the while(1) in the main function in the first place?
tvlogman 19:f08b5cd2b7ce 130 void measureAndControl(){
tvlogman 43:dd0888f86357 131 // Read encoders and potmeter signal (unnfiltered reference)
tvlogman 33:6f4858b98fe5 132 m1counts = Encoder1.getPulses();
tvlogman 33:6f4858b98fe5 133 m2counts = Encoder2.getPulses();
tvlogman 40:7418f46a1ac0 134
tvlogman 40:7418f46a1ac0 135 double m1position = e1.fetchMotorPosition(m1counts);
tvlogman 44:d157094b48d5 136 double m2position = e2.fetchMotorPosition(m2counts) - m1position;
tvlogman 40:7418f46a1ac0 137
tvlogman 43:dd0888f86357 138 double pot1 = ref1.getReference();
tvlogman 43:dd0888f86357 139 double pot2 = ref2.getReference();
tvlogman 38:f1d2d42a4bdc 140
tvlogman 37:633dd1901681 141 // Finite state machine
tvlogman 37:633dd1901681 142 switch(currentState){
tvlogman 37:633dd1901681 143 case KILLED:
tvlogman 37:633dd1901681 144 {
tvlogman 37:633dd1901681 145 // Initialization of KILLED state: cut power to both motors
tvlogman 37:633dd1901681 146 if(stateChanged){
tvlogman 37:633dd1901681 147 motor1.kill();
tvlogman 38:f1d2d42a4bdc 148 motor2.kill();
tvlogman 37:633dd1901681 149 pc.printf("Killed state \r\n");
tvlogman 37:633dd1901681 150 stateChanged = false;
tvlogman 37:633dd1901681 151 }
tvlogman 37:633dd1901681 152
tvlogman 37:633dd1901681 153 // Set LED to red
tvlogman 37:633dd1901681 154 ledR = 0;
tvlogman 37:633dd1901681 155 ledG = 1;
tvlogman 37:633dd1901681 156 ledB = 1;
tvlogman 38:f1d2d42a4bdc 157
tvlogman 43:dd0888f86357 158 sendDataToPc(pot1, pot2, m1counts, m2counts); // just send the EMG signal value to HIDscope
tvlogman 44:d157094b48d5 159
tvlogman 44:d157094b48d5 160 double Mp1Xa = -L1*sin(m1position) - L2*sin(m1position+m2position);
tvlogman 44:d157094b48d5 161 double Mp2Xa = L1*cos(m1position) + L2*cos(m1position+m2position);
tvlogman 44:d157094b48d5 162
tvlogman 44:d157094b48d5 163 // pc.printf("Mp1Xa is %.2f \r\n", Mp1Xa);
tvlogman 44:d157094b48d5 164 // pc.printf("Mp2Xa is %.2f \r\n", Mp2Xa);
tvlogman 44:d157094b48d5 165 //
tvlogman 44:d157094b48d5 166 // pc.printf("theta1 is %.2f \r\n", m1position);
tvlogman 44:d157094b48d5 167 // pc.printf("theta2 is %.2f \r\n", m2position);
tvlogman 44:d157094b48d5 168
tvlogman 37:633dd1901681 169 break;
tvlogman 37:633dd1901681 170 }
tvlogman 43:dd0888f86357 171
tvlogman 43:dd0888f86357 172
tvlogman 37:633dd1901681 173 case ACTIVE:
tvlogman 37:633dd1901681 174 {
tvlogman 37:633dd1901681 175 if(stateChanged){
tvlogman 37:633dd1901681 176 pc.printf("Active state \r\n");
tvlogman 43:dd0888f86357 177 Mp1C = m1position;
tvlogman 43:dd0888f86357 178 Mp2C = m2position;
tvlogman 43:dd0888f86357 179 stateChanged = false;
tvlogman 37:633dd1901681 180 }
tvlogman 43:dd0888f86357 181
tvlogman 43:dd0888f86357 182 // Using potmeter signals to define a desired end-effector velocity;
tvlogman 43:dd0888f86357 183
tvlogman 43:dd0888f86357 184 double vx = pot1;
tvlogman 43:dd0888f86357 185 double vy = pot2;
tvlogman 38:f1d2d42a4bdc 186
tvlogman 43:dd0888f86357 187 // Translating vx and vy to angular velocities
tvlogman 43:dd0888f86357 188 Matrix q_dot = robotKinematics.computeAngularVelocities(vx,vy,Mp1C,Mp2C);
tvlogman 43:dd0888f86357 189 double q_dot1 = q_dot(1,1);
tvlogman 43:dd0888f86357 190 double q_dot2 = q_dot(2,1);
tvlogman 43:dd0888f86357 191
tvlogman 43:dd0888f86357 192 // Computing position setpoint for next ticker tick using desired end-effector velocity
tvlogman 43:dd0888f86357 193 double Mp1N = Mp1C + Ts*q_dot1;
tvlogman 43:dd0888f86357 194 double Mp2N = Mp2C + Ts*q_dot2;
tvlogman 43:dd0888f86357 195
tvlogman 44:d157094b48d5 196 double Mp1X = -L1*sin(Mp1N) - L2*sin(Mp1N+Mp2N);
tvlogman 44:d157094b48d5 197 double Mp2X = L1*cos(Mp1N) + L2*cos(Mp1N+Mp2N);
tvlogman 44:d157094b48d5 198
tvlogman 44:d157094b48d5 199 double Mp1Xa = -L1*sin(m1position) - L2*sin(m1position+m2position);
tvlogman 44:d157094b48d5 200 double Mp2Xa = L1*cos(m1position) + L2*cos(m1position+m2position);
tvlogman 44:d157094b48d5 201
tvlogman 43:dd0888f86357 202 // Compute error between actual CURRENT motor position and NEXT position setpoint
tvlogman 43:dd0888f86357 203 e1.fetchError(m1position, Mp1N);
tvlogman 43:dd0888f86357 204 e2.fetchError(m2position, Mp2N);
tvlogman 37:633dd1901681 205
tvlogman 37:633dd1901681 206 // Compute motor value using controller and set motor
tvlogman 38:f1d2d42a4bdc 207 float motorValue1 = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
tvlogman 38:f1d2d42a4bdc 208 float motorValue2 = motorController2.control(e2.e_pos, e2.e_int, e2.e_der);
tvlogman 38:f1d2d42a4bdc 209 motor1.setMotor(motorValue1);
tvlogman 38:f1d2d42a4bdc 210 motor2.setMotor(motorValue2);
tvlogman 37:633dd1901681 211
tvlogman 37:633dd1901681 212 // Send data to HIDscope
tvlogman 44:d157094b48d5 213 //sendDataToPc(vx, vy, Mp1N, Mp2N);
tvlogman 44:d157094b48d5 214
tvlogman 44:d157094b48d5 215 // pc.printf("Mp1X is %.2f \r\n", Mp1X);
tvlogman 44:d157094b48d5 216 // pc.printf("Mp2X is %.2f \r\n", Mp2X);
tvlogman 44:d157094b48d5 217 // pc.printf("Mp1Xa is %.2f \r\n", Mp1Xa);
tvlogman 44:d157094b48d5 218 // pc.printf("Mp2Xa is %.2f \r\n", Mp2Xa);
tvlogman 44:d157094b48d5 219 // pc.printf("vx is %.2f \r\n", vx);
tvlogman 44:d157094b48d5 220 // pc.printf("vy is %.2f \r\n", vy);
tvlogman 43:dd0888f86357 221
tvlogman 43:dd0888f86357 222 // Prepare for next round
tvlogman 43:dd0888f86357 223 Mp1C = Mp1N;
tvlogman 43:dd0888f86357 224 Mp2C = Mp2N;
tvlogman 37:633dd1901681 225
tvlogman 37:633dd1901681 226 // Set LED to blue
tvlogman 37:633dd1901681 227 ledR = 1;
tvlogman 37:633dd1901681 228 ledG = 1;
tvlogman 37:633dd1901681 229 ledB = 0;
tvlogman 37:633dd1901681 230 // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn
tvlogman 37:633dd1901681 231 break;
tvlogman 37:633dd1901681 232 }
tvlogman 37:633dd1901681 233 }
tvlogman 15:b76b8cff4d8f 234 }
tvlogman 15:b76b8cff4d8f 235
tvlogman 38:f1d2d42a4bdc 236 void r1SwitchDirection(){
tvlogman 33:6f4858b98fe5 237 ref1.r_direction = !ref1.r_direction;
tvlogman 27:a4228ea8fb8f 238 pc.printf("Switched reference direction! \r\n");
tvlogman 14:664870b5d153 239 }
tvlogman 38:f1d2d42a4bdc 240
tvlogman 38:f1d2d42a4bdc 241 void r2SwitchDirection(){
tvlogman 38:f1d2d42a4bdc 242 ref2.r_direction = !ref2.r_direction;
tvlogman 38:f1d2d42a4bdc 243 pc.printf("Switched reference direction! \r\n");
tvlogman 38:f1d2d42a4bdc 244 }
vsluiter 0:c8f15874531b 245
tvlogman 37:633dd1901681 246 void killSwitch(){
tvlogman 37:633dd1901681 247 currentState = KILLED;
tvlogman 37:633dd1901681 248 stateChanged = true;
tvlogman 37:633dd1901681 249 }
tvlogman 37:633dd1901681 250
tvlogman 37:633dd1901681 251 void activateRobot(){
tvlogman 37:633dd1901681 252 currentState = ACTIVE;
tvlogman 37:633dd1901681 253 stateChanged = true;
tvlogman 37:633dd1901681 254 }
tvlogman 37:633dd1901681 255
tvlogman 21:d266d1e503ce 256
vsluiter 0:c8f15874531b 257 int main()
tvlogman 10:e23cbcdde7e3 258 {
tvlogman 37:633dd1901681 259 pc.baud(115200);
tvlogman 19:f08b5cd2b7ce 260 pc.printf("Main function");
tvlogman 39:d065ad7a978d 261
tvlogman 37:633dd1901681 262 // Attaching state change functions to buttons;
tvlogman 37:633dd1901681 263 sw2.fall(&killSwitch);
tvlogman 37:633dd1901681 264 sw3.fall(&activateRobot);
tvlogman 38:f1d2d42a4bdc 265 button1.rise(&r1SwitchDirection);
tvlogman 42:ae78ff03d9d6 266 button2.rise(&r2SwitchDirection);
tvlogman 37:633dd1901681 267
tvlogman 22:2e473e9798c0 268 controllerTicker.attach(measureAndControl, Ts);
tvlogman 19:f08b5cd2b7ce 269 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 270 }
tvlogman 7:1bffab95fc5f 271