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@34:1a70aa045c8f, 2017-10-22 (annotated)

Committer:
tvlogman
Date:
Sun Oct 22 08:01:59 2017 +0000
Revision:
34:1a70aa045c8f
Parent:
33:6f4858b98fe5
Child:
35:99bf23b34ee3
Now implementing the new calls to refGen and errorFetch.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tvlogman 33:6f4858b98fe5 1 #include <vector>
vsluiter 0:c8f15874531b 2 #include "mbed.h"
vsluiter 0:c8f15874531b 3 #include "MODSERIAL.h"
tvlogman 8:0067469c3389 4 #include "HIDScope.h"
tvlogman 9:5f0e796c9489 5 #include "QEI.h"
tvlogman 15:b76b8cff4d8f 6 #include "FastPWM.h"
tvlogman 29:9aa4d63a9bd1 7 #include "refGen.h"
tvlogman 30:65f0c9ecf810 8 #include "controller.h"
tvlogman 31:cc08254ab7b5 9 #include "motorConfig.h"
tvlogman 32:1bb406d2b3c3 10 #include "errorFetch.h"
tvlogman 34:1a70aa045c8f 11 #include "biquadFilter.h"
tvlogman 34:1a70aa045c8f 12 #include "biquadChain.h"
tvlogman 15:b76b8cff4d8f 13
tvlogman 33:6f4858b98fe5 14 // ADJUSTABLE PARAMETERS
tvlogman 27:a4228ea8fb8f 15
tvlogman 34:1a70aa045c8f 16 // EMG filter parameters
tvlogman 34:1a70aa045c8f 17
tvlogman 34:1a70aa045c8f 18 // high pass
tvlogman 34:1a70aa045c8f 19 biquadFilter HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770);
tvlogman 34:1a70aa045c8f 20 biquadFilter HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904);
tvlogman 34:1a70aa045c8f 21 biquadChain HPbqc(HPbq1, HPbq2);
tvlogman 34:1a70aa045c8f 22 // low pass
tvlogman 34:1a70aa045c8f 23 biquadFilter LPbq1(1.0e-6*0.1202, 1.0e-6*0.2405, 1.0e-6*0.1202, 1.0000, -1.9313, 0.9327);
tvlogman 34:1a70aa045c8f 24 biquadFilter LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.9702, 0.9716);
tvlogman 34:1a70aa045c8f 25 biquadChain LPbqc(LPbq1, LPbq2);
tvlogman 34:1a70aa045c8f 26
tvlogman 27:a4228ea8fb8f 27 // Controller parameters
tvlogman 28:8cd898ff43a2 28 const float k_p = 1;
tvlogman 27:a4228ea8fb8f 29 const float k_i = 0; // Still needs a reasonable value
tvlogman 27:a4228ea8fb8f 30 const float k_d = 0; // Again, still need to pick a reasonable value
tvlogman 27:a4228ea8fb8f 31
tvlogman 33:6f4858b98fe5 32 // Defining motor gear ratio - for BOTH motors as this is the same in the current configuration
tvlogman 33:6f4858b98fe5 33 const float gearRatio = 131;
tvlogman 33:6f4858b98fe5 34
tvlogman 33:6f4858b98fe5 35
tvlogman 33:6f4858b98fe5 36
tvlogman 33:6f4858b98fe5 37 // Declaring a controller ticker and volatile variables to store encoder counts and revs
tvlogman 33:6f4858b98fe5 38 Ticker controllerTicker;
tvlogman 33:6f4858b98fe5 39 volatile int m1counts = 0;
tvlogman 33:6f4858b98fe5 40 volatile int m2counts = 0;
tvlogman 33:6f4858b98fe5 41 volatile float m1revs = 0.00;
tvlogman 33:6f4858b98fe5 42 volatile float m2revs = 0.00;
tvlogman 33:6f4858b98fe5 43
tvlogman 33:6f4858b98fe5 44 // PWM settings
tvlogman 33:6f4858b98fe5 45 float pwmPeriod = 1.0/5000.0;
tvlogman 33:6f4858b98fe5 46 int frequency_pwm = 10000; //10kHz PWM
tvlogman 33:6f4858b98fe5 47
tvlogman 33:6f4858b98fe5 48 // Initializing encoder
tvlogman 32:1bb406d2b3c3 49 QEI Encoder1(D12,D13,NC,64, QEI::X4_ENCODING);
tvlogman 32:1bb406d2b3c3 50 QEI Encoder2(D11,D10,NC,64, QEI::X4_ENCODING);
tvlogman 10:e23cbcdde7e3 51 MODSERIAL pc(USBTX, USBRX);
tvlogman 27:a4228ea8fb8f 52 HIDScope scope(5);
tvlogman 8:0067469c3389 53
tvlogman 14:664870b5d153 54 // Defining inputs
tvlogman 14:664870b5d153 55 InterruptIn sw2(SW2);
tvlogman 15:b76b8cff4d8f 56 InterruptIn sw3(SW3);
tvlogman 16:27430afe663e 57 InterruptIn button1(D2);
tvlogman 28:8cd898ff43a2 58 InterruptIn button2(D3);
tvlogman 32:1bb406d2b3c3 59 AnalogIn pot2(A3);
tvlogman 34:1a70aa045c8f 60 //AnalogIn emg0( A0 );
tvlogman 32:1bb406d2b3c3 61 //AnalogIn emg1( A1 );
tvlogman 15:b76b8cff4d8f 62
tvlogman 27:a4228ea8fb8f 63 // Setting up HIDscope
tvlogman 16:27430afe663e 64 volatile float x;
tvlogman 27:a4228ea8fb8f 65 volatile float y;
tvlogman 27:a4228ea8fb8f 66 volatile float z;
tvlogman 27:a4228ea8fb8f 67 volatile float q;
tvlogman 27:a4228ea8fb8f 68 volatile float k;
tvlogman 27:a4228ea8fb8f 69
tvlogman 27:a4228ea8fb8f 70 void sendDataToPc(float data1, float data2, float data3, float data4, float data5){
tvlogman 27:a4228ea8fb8f 71 // Capture data
tvlogman 27:a4228ea8fb8f 72 x = data1;
tvlogman 27:a4228ea8fb8f 73 y = data2;
tvlogman 27:a4228ea8fb8f 74 z = data3;
tvlogman 27:a4228ea8fb8f 75 q = data4;
tvlogman 27:a4228ea8fb8f 76 k = data5;
tvlogman 27:a4228ea8fb8f 77 scope.set(0, x);
tvlogman 27:a4228ea8fb8f 78 scope.set(1, y);
tvlogman 27:a4228ea8fb8f 79 scope.set(2, z);
tvlogman 27:a4228ea8fb8f 80 scope.set(3, q);
tvlogman 27:a4228ea8fb8f 81 scope.set(4, z);
tvlogman 27:a4228ea8fb8f 82 scope.send(); // send what's in scope memory to PC
tvlogman 27:a4228ea8fb8f 83 }
tvlogman 14:664870b5d153 84
tvlogman 7:1bffab95fc5f 85
tvlogman 33:6f4858b98fe5 86 // REFERENCE PARAMETERS
tvlogman 27:a4228ea8fb8f 87 int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions
tvlogman 21:d266d1e503ce 88 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
tvlogman 22:2e473e9798c0 89 const float Ts = 0.1;
tvlogman 20:4ce3fb543a45 90
tvlogman 27:a4228ea8fb8f 91 // Function getReferencePosition returns reference angle based on potmeter 1
tvlogman 33:6f4858b98fe5 92 refGen ref1(A1, maxAngle);
tvlogman 34:1a70aa045c8f 93 //refGen ref2(A1, maxAngle);
tvlogman 19:f08b5cd2b7ce 94
tvlogman 21:d266d1e503ce 95 // readEncoder reads counts and revs and logs results to serial window
tvlogman 34:1a70aa045c8f 96 errorFetch e1(gearRatio, Ts);
tvlogman 34:1a70aa045c8f 97 //errorFetch e2(gearRatio, Ts);
tvlogman 21:d266d1e503ce 98
tvlogman 31:cc08254ab7b5 99 // Generate a PID controller with the specified values of k_p, k_d and k_i
tvlogman 30:65f0c9ecf810 100 controller motorController1(k_p, k_d, k_i);
tvlogman 14:664870b5d153 101
tvlogman 19:f08b5cd2b7ce 102 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
tvlogman 31:cc08254ab7b5 103 motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4);
tvlogman 19:f08b5cd2b7ce 104
tvlogman 19:f08b5cd2b7ce 105 void measureAndControl(){
tvlogman 33:6f4858b98fe5 106 m1counts = Encoder1.getPulses();
tvlogman 33:6f4858b98fe5 107 m2counts = Encoder2.getPulses();
tvlogman 34:1a70aa045c8f 108 float r1 = ref1.getFilteredReference;
tvlogman 34:1a70aa045c8f 109 e1.fetchError(m1counts, r1);
tvlogman 33:6f4858b98fe5 110 float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
tvlogman 34:1a70aa045c8f 111 float r1_unfiltered = ref1.getReference();
tvlogman 34:1a70aa045c8f 112 sendDataToPc(r1_unfiltered, r1, e1.e_pos, e1.e_der, motorValue);
tvlogman 31:cc08254ab7b5 113 motor1.setMotor(motorValue);
tvlogman 15:b76b8cff4d8f 114 }
tvlogman 15:b76b8cff4d8f 115
tvlogman 27:a4228ea8fb8f 116 void rSwitchDirection(){
tvlogman 33:6f4858b98fe5 117 ref1.r_direction = !ref1.r_direction;
tvlogman 27:a4228ea8fb8f 118 pc.printf("Switched reference direction! \r\n");
tvlogman 14:664870b5d153 119 }
vsluiter 0:c8f15874531b 120
tvlogman 21:d266d1e503ce 121
vsluiter 0:c8f15874531b 122 int main()
tvlogman 10:e23cbcdde7e3 123 {
tvlogman 19:f08b5cd2b7ce 124 pc.printf("Main function");
tvlogman 31:cc08254ab7b5 125 sw2.fall(&motor1,&motorConfig::killSwitch);
tvlogman 31:cc08254ab7b5 126 sw3.fall(&motor1, &motorConfig::turnMotorOn);
tvlogman 27:a4228ea8fb8f 127 button2.rise(&rSwitchDirection);
vsluiter 0:c8f15874531b 128 pc.baud(115200);
tvlogman 22:2e473e9798c0 129 controllerTicker.attach(measureAndControl, Ts);
tvlogman 19:f08b5cd2b7ce 130 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 131 }
tvlogman 7:1bffab95fc5f 132