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@37:633dd1901681, 2017-10-23 (annotated)

Committer:
tvlogman
Date:
Mon Oct 23 13:50:49 2017 +0000
Revision:
37:633dd1901681
Parent:
36:f3f3327d1d5a
Child:
38:f1d2d42a4bdc
Used EMG signals to control up to two motors and implements filtering;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tvlogman 33:6f4858b98fe5 1 #include <vector>
tvlogman 37:633dd1901681 2 #include <numeric>
vsluiter 0:c8f15874531b 3 #include "mbed.h"
vsluiter 0:c8f15874531b 4 #include "MODSERIAL.h"
tvlogman 8:0067469c3389 5 #include "HIDScope.h"
tvlogman 9:5f0e796c9489 6 #include "QEI.h"
tvlogman 15:b76b8cff4d8f 7 #include "FastPWM.h"
tvlogman 29:9aa4d63a9bd1 8 #include "refGen.h"
tvlogman 30:65f0c9ecf810 9 #include "controller.h"
tvlogman 31:cc08254ab7b5 10 #include "motorConfig.h"
tvlogman 32:1bb406d2b3c3 11 #include "errorFetch.h"
tvlogman 34:1a70aa045c8f 12 #include "biquadFilter.h"
tvlogman 34:1a70aa045c8f 13 #include "biquadChain.h"
tvlogman 15:b76b8cff4d8f 14
tvlogman 33:6f4858b98fe5 15 // ADJUSTABLE PARAMETERS
tvlogman 37:633dd1901681 16 // controller ticker time interval
tvlogman 37:633dd1901681 17 const float Ts = 0.1;
tvlogman 27:a4228ea8fb8f 18
tvlogman 34:1a70aa045c8f 19 // EMG filter parameters
tvlogman 37:633dd1901681 20 // calibration time
tvlogman 37:633dd1901681 21 const int calSamples = 100;
tvlogman 37:633dd1901681 22
tvlogman 37:633dd1901681 23 volatile float avgEMGvalue = 0;
tvlogman 37:633dd1901681 24
tvlogman 34:1a70aa045c8f 25
tvlogman 34:1a70aa045c8f 26 // high pass
tvlogman 34:1a70aa045c8f 27 biquadFilter HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770);
tvlogman 34:1a70aa045c8f 28 biquadFilter HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904);
tvlogman 34:1a70aa045c8f 29 biquadChain HPbqc(HPbq1, HPbq2);
tvlogman 34:1a70aa045c8f 30 // low pass
tvlogman 36:f3f3327d1d5a 31 biquadFilter LPbq1(1.0e-5*1.3294, 1.0e-5*2.6587, 1.0e-5*1.3294, 1.0000, -1.7783, 0.7924);
tvlogman 36:f3f3327d1d5a 32 biquadFilter LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.8934, 0.9085);
tvlogman 34:1a70aa045c8f 33 biquadChain LPbqc(LPbq1, LPbq2);
tvlogman 34:1a70aa045c8f 34
tvlogman 27:a4228ea8fb8f 35 // Controller parameters
tvlogman 28:8cd898ff43a2 36 const float k_p = 1;
tvlogman 27:a4228ea8fb8f 37 const float k_i = 0; // Still needs a reasonable value
tvlogman 27:a4228ea8fb8f 38 const float k_d = 0; // Again, still need to pick a reasonable value
tvlogman 27:a4228ea8fb8f 39
tvlogman 33:6f4858b98fe5 40 // Defining motor gear ratio - for BOTH motors as this is the same in the current configuration
tvlogman 33:6f4858b98fe5 41 const float gearRatio = 131;
tvlogman 33:6f4858b98fe5 42
tvlogman 37:633dd1901681 43 // LOGISTICS
tvlogman 37:633dd1901681 44 // Declaring finite-state-machine states
tvlogman 37:633dd1901681 45 enum robotStates {KILLED, ACTIVE, CALIBRATING};
tvlogman 37:633dd1901681 46 volatile robotStates currentState = KILLED;
tvlogman 37:633dd1901681 47 volatile bool stateChanged = true;
tvlogman 33:6f4858b98fe5 48
tvlogman 33:6f4858b98fe5 49 // Declaring a controller ticker and volatile variables to store encoder counts and revs
tvlogman 33:6f4858b98fe5 50 Ticker controllerTicker;
tvlogman 33:6f4858b98fe5 51 volatile int m1counts = 0;
tvlogman 33:6f4858b98fe5 52 volatile int m2counts = 0;
tvlogman 33:6f4858b98fe5 53 volatile float m1revs = 0.00;
tvlogman 33:6f4858b98fe5 54 volatile float m2revs = 0.00;
tvlogman 33:6f4858b98fe5 55
tvlogman 33:6f4858b98fe5 56 // PWM settings
tvlogman 33:6f4858b98fe5 57 float pwmPeriod = 1.0/5000.0;
tvlogman 33:6f4858b98fe5 58 int frequency_pwm = 10000; //10kHz PWM
tvlogman 33:6f4858b98fe5 59
tvlogman 33:6f4858b98fe5 60 // Initializing encoder
tvlogman 32:1bb406d2b3c3 61 QEI Encoder1(D12,D13,NC,64, QEI::X4_ENCODING);
tvlogman 32:1bb406d2b3c3 62 QEI Encoder2(D11,D10,NC,64, QEI::X4_ENCODING);
tvlogman 10:e23cbcdde7e3 63 MODSERIAL pc(USBTX, USBRX);
tvlogman 27:a4228ea8fb8f 64 HIDScope scope(5);
tvlogman 8:0067469c3389 65
tvlogman 14:664870b5d153 66 // Defining inputs
tvlogman 14:664870b5d153 67 InterruptIn sw2(SW2);
tvlogman 15:b76b8cff4d8f 68 InterruptIn sw3(SW3);
tvlogman 16:27430afe663e 69 InterruptIn button1(D2);
tvlogman 28:8cd898ff43a2 70 InterruptIn button2(D3);
tvlogman 37:633dd1901681 71 //AnalogIn pot2(A3);
tvlogman 34:1a70aa045c8f 72 //AnalogIn emg0( A0 );
tvlogman 32:1bb406d2b3c3 73 //AnalogIn emg1( A1 );
tvlogman 15:b76b8cff4d8f 74
tvlogman 37:633dd1901681 75 // Defining LED outputs to indicate robot state-us
tvlogman 37:633dd1901681 76 DigitalOut ledG(LED_GREEN);
tvlogman 37:633dd1901681 77 DigitalOut ledR(LED_RED);
tvlogman 37:633dd1901681 78 DigitalOut ledB(LED_BLUE);
tvlogman 37:633dd1901681 79
tvlogman 27:a4228ea8fb8f 80 // Setting up HIDscope
tvlogman 16:27430afe663e 81 volatile float x;
tvlogman 27:a4228ea8fb8f 82 volatile float y;
tvlogman 27:a4228ea8fb8f 83 volatile float z;
tvlogman 27:a4228ea8fb8f 84 volatile float q;
tvlogman 27:a4228ea8fb8f 85 volatile float k;
tvlogman 27:a4228ea8fb8f 86
tvlogman 27:a4228ea8fb8f 87 void sendDataToPc(float data1, float data2, float data3, float data4, float data5){
tvlogman 27:a4228ea8fb8f 88 // Capture data
tvlogman 27:a4228ea8fb8f 89 x = data1;
tvlogman 27:a4228ea8fb8f 90 y = data2;
tvlogman 27:a4228ea8fb8f 91 z = data3;
tvlogman 27:a4228ea8fb8f 92 q = data4;
tvlogman 27:a4228ea8fb8f 93 k = data5;
tvlogman 27:a4228ea8fb8f 94 scope.set(0, x);
tvlogman 27:a4228ea8fb8f 95 scope.set(1, y);
tvlogman 27:a4228ea8fb8f 96 scope.set(2, z);
tvlogman 27:a4228ea8fb8f 97 scope.set(3, q);
tvlogman 27:a4228ea8fb8f 98 scope.set(4, z);
tvlogman 27:a4228ea8fb8f 99 scope.send(); // send what's in scope memory to PC
tvlogman 27:a4228ea8fb8f 100 }
tvlogman 14:664870b5d153 101
tvlogman 7:1bffab95fc5f 102
tvlogman 33:6f4858b98fe5 103 // REFERENCE PARAMETERS
tvlogman 27:a4228ea8fb8f 104 int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions
tvlogman 21:d266d1e503ce 105 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
tvlogman 37:633dd1901681 106
tvlogman 20:4ce3fb543a45 107
tvlogman 27:a4228ea8fb8f 108 // Function getReferencePosition returns reference angle based on potmeter 1
tvlogman 33:6f4858b98fe5 109 refGen ref1(A1, maxAngle);
tvlogman 37:633dd1901681 110 //refGen ref2(A2, maxAngle);
tvlogman 19:f08b5cd2b7ce 111
tvlogman 21:d266d1e503ce 112 // readEncoder reads counts and revs and logs results to serial window
tvlogman 34:1a70aa045c8f 113 errorFetch e1(gearRatio, Ts);
tvlogman 34:1a70aa045c8f 114 //errorFetch e2(gearRatio, Ts);
tvlogman 21:d266d1e503ce 115
tvlogman 31:cc08254ab7b5 116 // Generate a PID controller with the specified values of k_p, k_d and k_i
tvlogman 30:65f0c9ecf810 117 controller motorController1(k_p, k_d, k_i);
tvlogman 14:664870b5d153 118
tvlogman 19:f08b5cd2b7ce 119 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
tvlogman 37:633dd1901681 120 motorConfig motor1(D4,D5);
tvlogman 37:633dd1901681 121 motorConfig motor2(D7,D6);
tvlogman 37:633dd1901681 122
tvlogman 37:633dd1901681 123 // 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 124 void measureAndControl(){
tvlogman 37:633dd1901681 125 // Read encoders and EMG signal (unnfiltered reference)
tvlogman 33:6f4858b98fe5 126 m1counts = Encoder1.getPulses();
tvlogman 33:6f4858b98fe5 127 m2counts = Encoder2.getPulses();
tvlogman 37:633dd1901681 128 float r1_uf = ref1.getReference();
tvlogman 37:633dd1901681 129 //float r2_uf = ref2.getReference();
tvlogman 37:633dd1901681 130 pc.printf("In controller ticker \r\n");
tvlogman 37:633dd1901681 131 // Finite state machine
tvlogman 37:633dd1901681 132 switch(currentState){
tvlogman 37:633dd1901681 133 case KILLED:
tvlogman 37:633dd1901681 134 {
tvlogman 37:633dd1901681 135 // Initialization of KILLED state: cut power to both motors
tvlogman 37:633dd1901681 136 if(stateChanged){
tvlogman 37:633dd1901681 137 motor1.kill();
tvlogman 37:633dd1901681 138 //motor2.kill();
tvlogman 37:633dd1901681 139 pc.printf("Killed state \r\n");
tvlogman 37:633dd1901681 140 stateChanged = false;
tvlogman 37:633dd1901681 141 }
tvlogman 37:633dd1901681 142
tvlogman 37:633dd1901681 143 // Send reference data to pc
tvlogman 37:633dd1901681 144 sendDataToPc(r1_uf, r1_uf, r1_uf, r1_uf, r1_uf); // just send the EMG signal value to HIDscope
tvlogman 37:633dd1901681 145
tvlogman 37:633dd1901681 146 // Set LED to red
tvlogman 37:633dd1901681 147 ledR = 0;
tvlogman 37:633dd1901681 148 ledG = 1;
tvlogman 37:633dd1901681 149 ledB = 1;
tvlogman 37:633dd1901681 150 // need something here to check if "killswitch" has been pressed (?)
tvlogman 37:633dd1901681 151 // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn
tvlogman 37:633dd1901681 152 break;
tvlogman 37:633dd1901681 153 }
tvlogman 37:633dd1901681 154 case ACTIVE:
tvlogman 37:633dd1901681 155 {
tvlogman 37:633dd1901681 156 if(stateChanged){
tvlogman 37:633dd1901681 157 pc.printf("Active state \r\n");
tvlogman 37:633dd1901681 158 }
tvlogman 37:633dd1901681 159 // Filter reference
tvlogman 37:633dd1901681 160 float r1 = HPbqc.applyFilter(r1_uf);
tvlogman 37:633dd1901681 161 r1 = fabs(r1);
tvlogman 37:633dd1901681 162 r1 = LPbqc.applyFilter(r1);
tvlogman 37:633dd1901681 163
tvlogman 37:633dd1901681 164 //float r2 = HPbqc.applyFilter(r2_uf);
tvlogman 37:633dd1901681 165 //r2 = fabs(r2);
tvlogman 37:633dd1901681 166 //r2 = LPbqc.applyFilter(r2);
tvlogman 37:633dd1901681 167
tvlogman 37:633dd1901681 168
tvlogman 37:633dd1901681 169 // Compute error
tvlogman 37:633dd1901681 170 e1.fetchError(m1counts, r1);
tvlogman 37:633dd1901681 171 //e2.fetchError(m2counts, r2);
tvlogman 37:633dd1901681 172
tvlogman 37:633dd1901681 173 // Compute motor value using controller and set motor
tvlogman 37:633dd1901681 174 float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
tvlogman 37:633dd1901681 175 motor1.setMotor(motorValue);
tvlogman 37:633dd1901681 176
tvlogman 37:633dd1901681 177 // Send data to HIDscope
tvlogman 37:633dd1901681 178 sendDataToPc(r1_uf, r1, e1.e_pos, e1.e_der, motorValue);
tvlogman 37:633dd1901681 179
tvlogman 37:633dd1901681 180 // Set LED to blue
tvlogman 37:633dd1901681 181 ledR = 1;
tvlogman 37:633dd1901681 182 ledG = 1;
tvlogman 37:633dd1901681 183 ledB = 0;
tvlogman 37:633dd1901681 184 // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn
tvlogman 37:633dd1901681 185 break;
tvlogman 37:633dd1901681 186 }
tvlogman 37:633dd1901681 187 case CALIBRATING:
tvlogman 37:633dd1901681 188 {
tvlogman 37:633dd1901681 189 // NOTE: maybe wrap this whole calibrating thing in a library?
tvlogman 37:633dd1901681 190
tvlogman 37:633dd1901681 191 // Do I need to kill motors here?
tvlogman 37:633dd1901681 192
tvlogman 37:633dd1901681 193
tvlogman 37:633dd1901681 194 // Initialization of CALIBRATE state
tvlogman 37:633dd1901681 195 static int Ncal = 0;
tvlogman 37:633dd1901681 196 std::vector<float> EMGsamples;
tvlogman 37:633dd1901681 197
tvlogman 37:633dd1901681 198 if(stateChanged){
tvlogman 37:633dd1901681 199 // Kill motors
tvlogman 37:633dd1901681 200 pc.printf("Calibrate state \r\n");
tvlogman 37:633dd1901681 201 motor1.kill();
tvlogman 37:633dd1901681 202 //motor2.kill();
tvlogman 37:633dd1901681 203
tvlogman 37:633dd1901681 204 // Clear sample value vector and reset counter variable
tvlogman 37:633dd1901681 205 EMGsamples.clear();
tvlogman 37:633dd1901681 206 Ncal = 0;
tvlogman 37:633dd1901681 207 stateChanged = false;
tvlogman 37:633dd1901681 208 }
tvlogman 37:633dd1901681 209
tvlogman 37:633dd1901681 210 // fill the array with sample data if it is not yet filled
tvlogman 37:633dd1901681 211 if(Ncal < calSamples){
tvlogman 37:633dd1901681 212 pc.printf("%.2f \r\n", r1_uf);
tvlogman 37:633dd1901681 213 EMGsamples.push_back(r1_uf);
tvlogman 37:633dd1901681 214 pc.printf("%.2f \r\n", EMGsamples.end());
tvlogman 37:633dd1901681 215 Ncal++;
tvlogman 37:633dd1901681 216 }
tvlogman 37:633dd1901681 217 // if array is filled compute the mean value and switch to active state
tvlogman 37:633dd1901681 218 else {
tvlogman 37:633dd1901681 219 // Set new avgEMGvalue
tvlogman 37:633dd1901681 220 avgEMGvalue = std::accumulate(EMGsamples.begin(), EMGsamples.end(), 0)/calSamples; // still needs to return this value
tvlogman 37:633dd1901681 221
tvlogman 37:633dd1901681 222 pc.printf("Avg emg value is %.2f changed state to active", avgEMGvalue);
tvlogman 37:633dd1901681 223 // State transition logic
tvlogman 37:633dd1901681 224 currentState = ACTIVE;
tvlogman 37:633dd1901681 225 stateChanged = true;
tvlogman 37:633dd1901681 226 Ncal = 0;
tvlogman 37:633dd1901681 227 }
tvlogman 37:633dd1901681 228
tvlogman 37:633dd1901681 229 // Set LED to green
tvlogman 37:633dd1901681 230 ledR = 1;
tvlogman 37:633dd1901681 231 ledG = 0;
tvlogman 37:633dd1901681 232 ledB = 1;
tvlogman 37:633dd1901681 233 sendDataToPc(r1_uf, r1_uf, r1_uf, r1_uf, r1_uf);
tvlogman 37:633dd1901681 234 break;
tvlogman 37:633dd1901681 235 }
tvlogman 37:633dd1901681 236 }
tvlogman 37:633dd1901681 237
tvlogman 37:633dd1901681 238
tvlogman 37:633dd1901681 239
tvlogman 15:b76b8cff4d8f 240 }
tvlogman 15:b76b8cff4d8f 241
tvlogman 27:a4228ea8fb8f 242 void rSwitchDirection(){
tvlogman 33:6f4858b98fe5 243 ref1.r_direction = !ref1.r_direction;
tvlogman 27:a4228ea8fb8f 244 pc.printf("Switched reference direction! \r\n");
tvlogman 14:664870b5d153 245 }
vsluiter 0:c8f15874531b 246
tvlogman 37:633dd1901681 247 void killSwitch(){
tvlogman 37:633dd1901681 248 currentState = KILLED;
tvlogman 37:633dd1901681 249 stateChanged = true;
tvlogman 37:633dd1901681 250 }
tvlogman 37:633dd1901681 251
tvlogman 37:633dd1901681 252 void activateRobot(){
tvlogman 37:633dd1901681 253 currentState = ACTIVE;
tvlogman 37:633dd1901681 254 stateChanged = true;
tvlogman 37:633dd1901681 255 }
tvlogman 37:633dd1901681 256
tvlogman 37:633dd1901681 257 void calibrateRobot(){
tvlogman 37:633dd1901681 258 currentState = CALIBRATING;
tvlogman 37:633dd1901681 259 stateChanged = true;
tvlogman 37:633dd1901681 260 }
tvlogman 21:d266d1e503ce 261
vsluiter 0:c8f15874531b 262 int main()
tvlogman 10:e23cbcdde7e3 263 {
tvlogman 37:633dd1901681 264 pc.baud(115200);
tvlogman 19:f08b5cd2b7ce 265 pc.printf("Main function");
tvlogman 37:633dd1901681 266 // Attaching state change functions to buttons;
tvlogman 37:633dd1901681 267 sw2.fall(&killSwitch);
tvlogman 37:633dd1901681 268 sw3.fall(&activateRobot);
tvlogman 37:633dd1901681 269 button1.rise(&calibrateRobot);
tvlogman 27:a4228ea8fb8f 270 button2.rise(&rSwitchDirection);
tvlogman 37:633dd1901681 271
tvlogman 22:2e473e9798c0 272 controllerTicker.attach(measureAndControl, Ts);
tvlogman 19:f08b5cd2b7ce 273 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 274 }
tvlogman 7:1bffab95fc5f 275