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@39:d065ad7a978d, 2017-10-26 (annotated)

Committer:
tvlogman
Date:
Thu Oct 26 13:29:56 2017 +0000
Revision:
39:d065ad7a978d
Parent:
38:f1d2d42a4bdc
Child:
40:7418f46a1ac0
Working version implementing Tom Lankhorst's BiQuad library and using position control.

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