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@41:9678fd827d25, 2017-10-26 (annotated)

Committer:
tvlogman
Date:
Thu Oct 26 19:39:46 2017 +0000
Revision:
41:9678fd827d25
Parent:
40:7418f46a1ac0
Child:
42:ae78ff03d9d6
Variant of position control - not tested yet but worth a try

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