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

Diff: main.cpp

Revision:
34:1a70aa045c8f
Parent:
33:6f4858b98fe5
Child:
35:99bf23b34ee3
diff -r 6f4858b98fe5 -r 1a70aa045c8f main.cpp
--- a/main.cpp	Fri Oct 20 16:30:26 2017 +0000
+++ b/main.cpp	Sun Oct 22 08:01:59 2017 +0000
@@ -8,9 +8,22 @@
 #include "controller.h"
 #include "motorConfig.h"
 #include "errorFetch.h"
+#include "biquadFilter.h"
+#include "biquadChain.h"
 
 // ADJUSTABLE PARAMETERS
 
+// EMG filter parameters
+
+// high pass
+biquadFilter HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770);
+biquadFilter HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904);
+biquadChain HPbqc(HPbq1, HPbq2);
+// low pass
+biquadFilter LPbq1(1.0e-6*0.1202, 1.0e-6*0.2405, 1.0e-6*0.1202, 1.0000, -1.9313, 0.9327);
+biquadFilter LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.9702, 0.9716);
+biquadChain LPbqc(LPbq1, LPbq2);
+
 // Controller parameters
 const float k_p = 1;
 const float k_i = 0; // Still needs a reasonable value
@@ -44,7 +57,7 @@
 InterruptIn button1(D2);
 InterruptIn button2(D3);
 AnalogIn pot2(A3);
-AnalogIn emg0( A0 );
+//AnalogIn emg0( A0 );
 //AnalogIn emg1( A1 );
 
 // Setting up HIDscope
@@ -77,11 +90,11 @@
 
 // Function getReferencePosition returns reference angle based on potmeter 1
 refGen ref1(A1, maxAngle);
-refGen ref2(A1, maxAngle);
+//refGen ref2(A1, maxAngle);
 
 // readEncoder reads counts and revs and logs results to serial window
-errorFetch e1(ref1, gearRatio, Ts);
-errorFetch e2(ref2, gearRatio, Ts);
+errorFetch e1(gearRatio, Ts);
+//errorFetch e2(gearRatio, Ts);
 
 // Generate a PID controller with the specified values of k_p, k_d and k_i
 controller motorController1(k_p, k_d, k_i);
@@ -92,10 +105,11 @@
 void measureAndControl(){
     m1counts = Encoder1.getPulses();
     m2counts = Encoder2.getPulses();
-    e1.fetchError(m1counts);
+    float r1 = ref1.getFilteredReference;
+    e1.fetchError(m1counts, r1);
     float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
-    float r = ref1.getReferencePosition();
-    sendDataToPc(r, e1.e_pos, emg0.read(), e1.e_der, motorValue);
+    float r1_unfiltered = ref1.getReference();
+    sendDataToPc(r1_unfiltered, r1, e1.e_pos, e1.e_der, motorValue);
     motor1.setMotor(motorValue);
     }