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@42:ae78ff03d9d6, 2017-10-27 (annotated)

Committer:
tvlogman
Date:
Fri Oct 27 10:37:43 2017 +0000
Revision:
42:ae78ff03d9d6
Parent:
41:9678fd827d25
Child:
43:dd0888f86357
Working setup to test the motors using the potmeter

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