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@38:f1d2d42a4bdc, 2017-10-24 (annotated)

Committer:
tvlogman
Date:
Tue Oct 24 13:17:42 2017 +0000
Revision:
38:f1d2d42a4bdc
Parent:
37:633dd1901681
Child:
39:d065ad7a978d
Working version for two motors without kinematics

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 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 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 38:f1d2d42a4bdc 71 //AnalogIn pot2(A2);
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 38:f1d2d42a4bdc 104 int posRevRange = 5; // 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 38:f1d2d42a4bdc 110 refGen ref2(A1, 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 38:f1d2d42a4bdc 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 38:f1d2d42a4bdc 118 controller motorController2(k_p, k_d, k_i);
tvlogman 38:f1d2d42a4bdc 119
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 38:f1d2d42a4bdc 129 float r2_uf = ref2.getReference();
tvlogman 37:633dd1901681 130 pc.printf("In controller ticker \r\n");
tvlogman 38:f1d2d42a4bdc 131
tvlogman 38:f1d2d42a4bdc 132 float r1 = r1_uf;
tvlogman 38:f1d2d42a4bdc 133 float r2 = r2_uf;
tvlogman 38:f1d2d42a4bdc 134
tvlogman 37:633dd1901681 135 // Finite state machine
tvlogman 37:633dd1901681 136 switch(currentState){
tvlogman 37:633dd1901681 137 case KILLED:
tvlogman 37:633dd1901681 138 {
tvlogman 37:633dd1901681 139 // Initialization of KILLED state: cut power to both motors
tvlogman 37:633dd1901681 140 if(stateChanged){
tvlogman 37:633dd1901681 141 motor1.kill();
tvlogman 38:f1d2d42a4bdc 142 motor2.kill();
tvlogman 37:633dd1901681 143 pc.printf("Killed state \r\n");
tvlogman 37:633dd1901681 144 stateChanged = false;
tvlogman 37:633dd1901681 145 }
tvlogman 37:633dd1901681 146
tvlogman 37:633dd1901681 147 // Send reference data to pc
tvlogman 37:633dd1901681 148
tvlogman 37:633dd1901681 149 // Set LED to red
tvlogman 37:633dd1901681 150 ledR = 0;
tvlogman 37:633dd1901681 151 ledG = 1;
tvlogman 37:633dd1901681 152 ledB = 1;
tvlogman 37:633dd1901681 153 // need something here to check if "killswitch" has been pressed (?)
tvlogman 37:633dd1901681 154 // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn
tvlogman 38:f1d2d42a4bdc 155 e1.fetchError(m1counts, r1);
tvlogman 38:f1d2d42a4bdc 156 e2.fetchError(m2counts, r2);
tvlogman 38:f1d2d42a4bdc 157
tvlogman 38:f1d2d42a4bdc 158 sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, 0.0); // just send the EMG signal value to HIDscope
tvlogman 38:f1d2d42a4bdc 159
tvlogman 37:633dd1901681 160 break;
tvlogman 37:633dd1901681 161 }
tvlogman 37:633dd1901681 162 case ACTIVE:
tvlogman 37:633dd1901681 163 {
tvlogman 37:633dd1901681 164 if(stateChanged){
tvlogman 37:633dd1901681 165 pc.printf("Active state \r\n");
tvlogman 37:633dd1901681 166 }
tvlogman 37:633dd1901681 167 // Filter reference
tvlogman 37:633dd1901681 168 float r1 = HPbqc.applyFilter(r1_uf);
tvlogman 37:633dd1901681 169 r1 = fabs(r1);
tvlogman 37:633dd1901681 170 r1 = LPbqc.applyFilter(r1);
tvlogman 37:633dd1901681 171
tvlogman 38:f1d2d42a4bdc 172 float r2 = HPbqc.applyFilter(r2_uf);
tvlogman 38:f1d2d42a4bdc 173 r2 = fabs(r2);
tvlogman 38:f1d2d42a4bdc 174 r2 = LPbqc.applyFilter(r2);
tvlogman 37:633dd1901681 175
tvlogman 37:633dd1901681 176
tvlogman 38:f1d2d42a4bdc 177
tvlogman 37:633dd1901681 178 // Compute error
tvlogman 37:633dd1901681 179 e1.fetchError(m1counts, r1);
tvlogman 38:f1d2d42a4bdc 180 e2.fetchError(m2counts, r2);
tvlogman 37:633dd1901681 181
tvlogman 37:633dd1901681 182 // Compute motor value using controller and set motor
tvlogman 38:f1d2d42a4bdc 183 float motorValue1 = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
tvlogman 38:f1d2d42a4bdc 184 float motorValue2 = motorController2.control(e2.e_pos, e2.e_int, e2.e_der);
tvlogman 38:f1d2d42a4bdc 185 motor1.setMotor(motorValue1);
tvlogman 38:f1d2d42a4bdc 186 motor2.setMotor(motorValue2);
tvlogman 37:633dd1901681 187
tvlogman 37:633dd1901681 188 // Send data to HIDscope
tvlogman 38:f1d2d42a4bdc 189 sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, 0.0);
tvlogman 37:633dd1901681 190
tvlogman 37:633dd1901681 191 // Set LED to blue
tvlogman 37:633dd1901681 192 ledR = 1;
tvlogman 37:633dd1901681 193 ledG = 1;
tvlogman 37:633dd1901681 194 ledB = 0;
tvlogman 37:633dd1901681 195 // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn
tvlogman 37:633dd1901681 196 break;
tvlogman 37:633dd1901681 197 }
tvlogman 37:633dd1901681 198 case CALIBRATING:
tvlogman 37:633dd1901681 199 {
tvlogman 37:633dd1901681 200 // NOTE: maybe wrap this whole calibrating thing in a library?
tvlogman 37:633dd1901681 201
tvlogman 37:633dd1901681 202 // Do I need to kill motors here?
tvlogman 37:633dd1901681 203
tvlogman 37:633dd1901681 204
tvlogman 37:633dd1901681 205 // Initialization of CALIBRATE state
tvlogman 37:633dd1901681 206 static int Ncal = 0;
tvlogman 37:633dd1901681 207 std::vector<float> EMGsamples;
tvlogman 37:633dd1901681 208
tvlogman 37:633dd1901681 209 if(stateChanged){
tvlogman 37:633dd1901681 210 // Kill motors
tvlogman 37:633dd1901681 211 pc.printf("Calibrate state \r\n");
tvlogman 37:633dd1901681 212 motor1.kill();
tvlogman 38:f1d2d42a4bdc 213 motor2.kill();
tvlogman 37:633dd1901681 214
tvlogman 37:633dd1901681 215 // Clear sample value vector and reset counter variable
tvlogman 37:633dd1901681 216 EMGsamples.clear();
tvlogman 37:633dd1901681 217 Ncal = 0;
tvlogman 37:633dd1901681 218 stateChanged = false;
tvlogman 37:633dd1901681 219 }
tvlogman 37:633dd1901681 220
tvlogman 37:633dd1901681 221 // fill the array with sample data if it is not yet filled
tvlogman 37:633dd1901681 222 if(Ncal < calSamples){
tvlogman 37:633dd1901681 223 pc.printf("%.2f \r\n", r1_uf);
tvlogman 37:633dd1901681 224 EMGsamples.push_back(r1_uf);
tvlogman 37:633dd1901681 225 pc.printf("%.2f \r\n", EMGsamples.end());
tvlogman 37:633dd1901681 226 Ncal++;
tvlogman 37:633dd1901681 227 }
tvlogman 37:633dd1901681 228 // if array is filled compute the mean value and switch to active state
tvlogman 37:633dd1901681 229 else {
tvlogman 37:633dd1901681 230 // Set new avgEMGvalue
tvlogman 37:633dd1901681 231 avgEMGvalue = std::accumulate(EMGsamples.begin(), EMGsamples.end(), 0)/calSamples; // still needs to return this value
tvlogman 37:633dd1901681 232
tvlogman 37:633dd1901681 233 pc.printf("Avg emg value is %.2f changed state to active", avgEMGvalue);
tvlogman 37:633dd1901681 234 // State transition logic
tvlogman 37:633dd1901681 235 currentState = ACTIVE;
tvlogman 37:633dd1901681 236 stateChanged = true;
tvlogman 37:633dd1901681 237 Ncal = 0;
tvlogman 37:633dd1901681 238 }
tvlogman 37:633dd1901681 239
tvlogman 37:633dd1901681 240 // Set LED to green
tvlogman 37:633dd1901681 241 ledR = 1;
tvlogman 37:633dd1901681 242 ledG = 0;
tvlogman 37:633dd1901681 243 ledB = 1;
tvlogman 38:f1d2d42a4bdc 244 sendDataToPc(r1_uf, r2_uf, 0.0, 0.0, 0.0);
tvlogman 37:633dd1901681 245 break;
tvlogman 37:633dd1901681 246 }
tvlogman 37:633dd1901681 247 }
tvlogman 37:633dd1901681 248
tvlogman 37:633dd1901681 249
tvlogman 37:633dd1901681 250
tvlogman 15:b76b8cff4d8f 251 }
tvlogman 15:b76b8cff4d8f 252
tvlogman 38:f1d2d42a4bdc 253 void r1SwitchDirection(){
tvlogman 33:6f4858b98fe5 254 ref1.r_direction = !ref1.r_direction;
tvlogman 27:a4228ea8fb8f 255 pc.printf("Switched reference direction! \r\n");
tvlogman 14:664870b5d153 256 }
tvlogman 38:f1d2d42a4bdc 257
tvlogman 38:f1d2d42a4bdc 258 void r2SwitchDirection(){
tvlogman 38:f1d2d42a4bdc 259 ref2.r_direction = !ref2.r_direction;
tvlogman 38:f1d2d42a4bdc 260 pc.printf("Switched reference direction! \r\n");
tvlogman 38:f1d2d42a4bdc 261 }
vsluiter 0:c8f15874531b 262
tvlogman 37:633dd1901681 263 void killSwitch(){
tvlogman 37:633dd1901681 264 currentState = KILLED;
tvlogman 37:633dd1901681 265 stateChanged = true;
tvlogman 37:633dd1901681 266 }
tvlogman 37:633dd1901681 267
tvlogman 37:633dd1901681 268 void activateRobot(){
tvlogman 37:633dd1901681 269 currentState = ACTIVE;
tvlogman 37:633dd1901681 270 stateChanged = true;
tvlogman 37:633dd1901681 271 }
tvlogman 37:633dd1901681 272
tvlogman 37:633dd1901681 273 void calibrateRobot(){
tvlogman 37:633dd1901681 274 currentState = CALIBRATING;
tvlogman 37:633dd1901681 275 stateChanged = true;
tvlogman 37:633dd1901681 276 }
tvlogman 21:d266d1e503ce 277
vsluiter 0:c8f15874531b 278 int main()
tvlogman 10:e23cbcdde7e3 279 {
tvlogman 37:633dd1901681 280 pc.baud(115200);
tvlogman 19:f08b5cd2b7ce 281 pc.printf("Main function");
tvlogman 37:633dd1901681 282 // Attaching state change functions to buttons;
tvlogman 37:633dd1901681 283 sw2.fall(&killSwitch);
tvlogman 37:633dd1901681 284 sw3.fall(&activateRobot);
tvlogman 38:f1d2d42a4bdc 285 button1.rise(&r1SwitchDirection);
tvlogman 38:f1d2d42a4bdc 286 button2.rise(&calibrateRobot);
tvlogman 37:633dd1901681 287
tvlogman 22:2e473e9798c0 288 controllerTicker.attach(measureAndControl, Ts);
tvlogman 19:f08b5cd2b7ce 289 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 290 }
tvlogman 7:1bffab95fc5f 291