Diff: main.cpp

Revision:

42:ae78ff03d9d6

Parent:

41:9678fd827d25

Child:

43:dd0888f86357

diff -r 9678fd827d25 -r ae78ff03d9d6 main.cpp
--- a/main.cpp	Thu Oct 26 19:39:46 2017 +0000
+++ b/main.cpp	Fri Oct 27 10:37:43 2017 +0000
@@ -1,5 +1,6 @@
 #include <vector>
 #include <numeric>
+#include <algorithm>
 #include "mbed.h"
 #include "Matrix.h"
 #include "MODSERIAL.h"
@@ -18,7 +19,7 @@
 
 // EMG filter parameters
 // calibration time
-const int calSamples = 100;
+const int calSamples = 1000;
 
 // Initialize average and max EMG value for calibration to 0 and 1 respectively
 volatile float avgEMGvalue = 0;
@@ -32,11 +33,11 @@
 
 // low pass
 BiQuadChain LPbqc;
-BiQuad LPbq1(1.0e-5*1.3294, 1.0e-5*2.6587, 1.0e-5*1.3294, 1.0000, -1.7783, 0.7924);
-BiQuad LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.8934, 0.9085);
+BiQuad LPbq1(1.0e-7*1.2023, 1.0e-7*2.4046, 1.0e-7*1.2023, 1.0000, -1.9313, 0.9327);
+BiQuad LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.9702, 0.9716);
 
 // Controller parameters
-const float k_p = 10;
+const float k_p = 1;
 const float k_i = 0; // Still needs a reasonable value
 const float k_d = 0; // Again, still need to pick a reasonable value
 
@@ -108,13 +109,13 @@
 
 
 // REFERENCE PARAMETERS
-int posRevRange = 5; // describes the ends of the position range in complete motor output shaft revolutions in both directions
-const float maxAngle = 2*3.14*posRevRange; // max angle in radians
+int posRevRange = 1; // describes the ends of the position range in complete motor output shaft revolutions in both directions
+const float maxAngle = 1*3.14*posRevRange; // max angle in radians
 
 
 // Function getReferencePosition returns reference angle based on potmeter 1
-refGen ref1(A1, maxAngle);
-refGen ref2(A1, maxAngle);
+refGen ref1(A2, maxAngle);
+refGen ref2(A3, maxAngle);
 
 // readEncoder reads counts and revs and logs results to serial window
 errorFetch e1(gearRatio, Ts);
@@ -138,24 +139,27 @@
     
     // measuring and normalizing EMG signals to use as basis for reference
 
-    float emg1 = ref1.getReference()/maxEMGvalue;
-    float emg2 = ref2.getReference()/maxEMGvalue;
+    double emg1 = ref1.getReference();
+    double emg2 = ref2.getReference();
     
     
     // Filtering the EMG signals   
-    
-    double thet1 = HPbqc.step(emg1);
-    thet1 = fabs(thet1);
-    thet1 = LPbqc.step(thet1);
+
+    double emg1HP = HPbqc.step(emg1);
+    double emg1HP_abs = fabs(emg1HP);
+    double emg1HPLP_abs = LPbqc.step(emg1HP_abs);
+//    thet1 = fabs(thet1);
+//    thet1 = LPbqc.step(thet1);
     
 
-    double thet2 = HPbqc.step(emg2);
-    thet2 = fabs(thet2);
-    thet2 = LPbqc.step(thet2);
+    double emg2HP = HPbqc.step(emg2);
+    double emg2HP_abs = fabs(emg2);
+    double emg2HPLP_abs = LPbqc.step(emg2HP_abs);
+//    thet2 = LPbqc.step(thet2);
 
     // Something worth trying: set a position setpoint that constantly changes but will never be reached until EMG value is 0 as it is computed from the robot's current position    
-    double thet1 = m1position + thet1;
-    double thet2 = m1position + thet2;
+//    thet1 = m1position + thet1;
+//    thet2 = m1position + thet2;
     
     // Other possibility: use thet1 and thet2 directly as reference angles. That'll require the user to hold muscle tension to stay in a certain position though
     
@@ -180,10 +184,10 @@
                 ledB = 1;
                 // need something here to check if "killswitch" has been pressed (?)
                 // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn
-                e1.fetchError(m1position, thet1);
-                e2.fetchError(m2position, thet1);
+//                e1.fetchError(m1position, thet1);
+//                e2.fetchError(m2position, thet1);
 
-                sendDataToPc(thet1, thet2, e1.e_pos, e2.e_pos, 0.0,0.0); // just send the EMG signal value to HIDscope
+                sendDataToPc(emg1, emg1HP_abs, emg1HPLP_abs, emg2, emg2HP, emg2HPLP_abs); // just send the EMG signal value to HIDscope
                 
                 break;
                 }
@@ -194,8 +198,8 @@
                     }
                 
                 // Compute error
-                e1.fetchError(m1position, thet1);
-                e2.fetchError(m2position, thet2);
+                e1.fetchError(m1position, emg1);
+                e2.fetchError(m2position, emg2);
                 
                 // Compute motor value using controller and set motor
                 float motorValue1 = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
@@ -204,7 +208,7 @@
                 motor2.setMotor(motorValue2);
                 
                 // Send data to HIDscope
-                sendDataToPc(thet1, thet2, e1.e_pos, e2.e_pos, motorValue1, motorValue2);
+                sendDataToPc(emg1, emg2, e1.e_pos, e2.e_pos, motorValue1, motorValue2);
                 
                 // Set LED to blue
                 ledR = 1;
@@ -245,9 +249,8 @@
                 else {
                     // Set new avgEMGvalue
                     avgEMGvalue = std::accumulate(EMGsamples.begin(), EMGsamples.end(), 0)/calSamples; // still needs to return this value 
-                    double maxEMGvalue = std::*max_element(EMGsamples.begin(), EMGsamples.end());
+                    double maxEMGvalue = *std::max_element(EMGsamples.begin(), EMGsamples.end());
                     
-                    double max = *max_element(vector.begin(), vector.end());
                     
                     pc.printf("Avg emg value is %.2f changed state to active", avgEMGvalue);
                     // State transition logic
@@ -260,7 +263,7 @@
                 ledR = 1;
                 ledG = 0;
                 ledB = 1;
-                sendDataToPc(thet1, thet2, 0.0, 0.0, 0.0, 0.0);
+//                sendDataToPc(thet1, thet2, 0.0, 0.0, 0.0, 0.0);
                 break;
                 }
         }
@@ -305,7 +308,7 @@
     sw2.fall(&killSwitch);
     sw3.fall(&activateRobot);
     button1.rise(&r1SwitchDirection);
-    button2.rise(&calibrateRobot);
+    button2.rise(&r2SwitchDirection);
     
     controllerTicker.attach(measureAndControl, Ts);
     pc.printf("Encoder ticker attached and baudrate set");