Diff: main.cpp

Revision:

19:a37cae6964ca

Parent:

18:dddc8d9f7638

Child:

20:4424d447f3cd

--- a/main.cpp	Tue Oct 15 09:14:37 2019 +0000
+++ b/main.cpp	Thu Oct 17 11:01:37 2019 +0000
@@ -9,10 +9,9 @@
     Gravity compensation
     PID values
     General program layout
+    better names for EMG input
 */
 
-
-
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "FastPWM.h"
@@ -64,27 +63,37 @@
     float I;
     float D;
     float I_counter;
-} PID1 PID2;
+};
+PID PID1;
+PID PID2;
 
 float dt = 0.001;
-int enc1_zero;  //the zero position of the encoders, to be determined from the
-int enc2_zero;  //encoder calibration
+float theta;
+float L;
+int enc1_zero = 0;  //the zero position of the encoders, to be determined from the
+int enc2_zero = 0;  //encoder calibration
 int EMG1_filtered;
 int EMG2_filtered;
 int enc1_value;
 int enc2_value;
 float error1 = 0.0;
 float error2 = 0.0;
+float last_error1 = 0.0;
+float last_error2 = 0.0;
+float action1;
+float action2;
 bool state_changed = false; //used to see if the state is "starting"
 volatile bool but1_pressed = false;
 volatile bool but2_pressed = false;
 volatile bool failure_occurred = false;
 float pot_1;                //used to keep track of the potentiometer values
 float pot_2;
-bool enc_has_been_calibrated;
 bool EMG_has_been_calibrated;
 bool button1_pressed;
 bool button2_pressed;
+const int EMG_cali_amount = 1000;
+float past_EMG_values[EMG_cali_amount];
+int past_EMG_count = 0;
 
 void do_nothing()
 
@@ -120,12 +129,29 @@
         pc.printf("Started EMG calibration\r\n");
         state_changed = false;
     }
+    if (past_EMG_count < EMG_cali_amount) {
+        past_EMG_values[past_EMG_count] = EMG1_filtered;
+        past_EMG_count++;
+    }
+    else { //calibration is has concluded
+        float sum = 0.0;
+        for(int i = 0; i<EMG_cali_amount; i++) {
+            sum += past_EMG_values[i];
+        }
+        float mean = sum/(float)EMG_cali_amount;
+        EMG_values.min = mean;
+        //calibration done, moving to cali_enc
+        pc.printf("Calibration of the EMG is done, lower bound = %f", mean);
+        EMG_has_been_calibrated = true;
+        state_changed = true;
+        state = s_cali_enc;
+    }
 }
 
 void cali_enc()
 /*
     Calibration of the encoder. The encoder should be moved to the lowest
-    position for the linear stage and the most upright postition for the
+    position for the linear stage and the horizontal postition for the
     rotating stage.
 */
 {
@@ -134,9 +160,9 @@
         state_changed = false;
     }
     if (button1_pressed) {
+        pc.printf("Encoder has been calibrated");
         enc1_zero = enc1_value;
         enc2_zero = enc2_value;
-        enc_has_been_calibrated = true;
         button1_pressed = false;
         state = s_moving_magnet_off;
         state_changed = true;
@@ -182,16 +208,29 @@
 
 void measure_signals()
 {
-    float EMG1_raw;
-    float EMG2_raw;
+    //only one emg input, reference and plus
+    float EMG1_raw = EMG1.read();
+    float EMG2_raw = EMG2.read();
+    float filter_value1 = fabs(bq2.step(fabs(bq1.step(EMG1_raw - EMG2_raw))));
+    
+    if (filter_value1 > EMG_values.max) {
+        EMG_values.max = filter_value1;
+    }
+    if (EMG_has_been_calibrated) {
+        EMG1_filtered = (filter_value1-EMG_values.min)/(EMG_values.max-EMG_values.min);
+    }
+    else {
+        EMG1_filtered = filter_value1;
+    }
+        
     enc1_value = enc1.getPulses();
     enc2_value = enc2.getPulses();
-    if (enc_has_been_calibrated) {
-        enc1_value -= enc1_zero;
-        enc2_value -= enc2_zero;
-    }
-    EMG1_raw = EMG1.read();
-    EMG2_raw = EMG2.read();
+    enc1_value -= enc1_zero;
+    enc2_value -= enc2_zero;
+    theta = (float)(enc1_value)/(float)(8400*2*PI);
+    L     = (float)(enc2_value)/(float)(8400*2*PI);
+    
+
 }
 
 void sample()