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@40:7418f46a1ac0, 2017-10-26 (annotated)

Committer:
tvlogman
Date:
Thu Oct 26 14:02:49 2017 +0000
Revision:
40:7418f46a1ac0
Parent:
39:d065ad7a978d
Child:
41:9678fd827d25
Now pseudo-integrating the reference to translate a relation from EMG to velocity to EMG to position. Doesn't work so well.

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