Diff: main.cpp

Revision:

24:7cad312a1e38

Parent:

23:9eeac9d1ecbe

--- a/main.cpp	Tue Oct 22 12:06:33 2019 +0000
+++ b/main.cpp	Wed Oct 23 10:48:46 2019 +0000
@@ -20,7 +20,7 @@
 #define PI 3.14159265
 
 Serial pc(USBTX, USBRX);            //connect to pc
-HIDScope scope(2);                  //HIDScope instance
+HIDScope scope(4);                  //HIDScope instance
 DigitalOut motor1_direction(D4);    //rotation motor 1 on shield (always D6)
 FastPWM motor1_pwm(D5);             //pwm 1 on shield (always D7)
 DigitalOut motor2_direction(D7);    //rotation motor 2 on shield (always D4)
@@ -31,6 +31,11 @@
 InterruptIn but2(D9);               //debounced button on biorobotics shield
 AnalogIn EMG1(A0);
 AnalogIn EMG2(A1);
+AnalogIn EMG3(A4);
+AnalogIn EMG4(A5);
+DigitalOut led_r(LED1);
+DigitalOut led_g(LED2);
+DigitalOut led_b(LED3);
 
 void check_failure();
 int schmitt_trigger(float i);
@@ -55,7 +60,7 @@
 struct EMG_params {
     float max;  //params of the emg, tbd during calibration
     float min;
-} EMG_values;
+} EMG_values1, EMG_values2;
 
 struct PID {
     float P;
@@ -71,8 +76,10 @@
 float L;
 int enc1_zero = 0;//the zero position of the encoders, to be determined from the
 int enc2_zero = 0;//encoder calibration
-float EMG1_filtered;
-float EMG2_filtered;
+float EMG1_filtered; // x
+float EMG2_filtered; // x
+float EMG3_filtered; // y
+float EMG4_filtered; // y
 int enc1_value;
 int enc2_value;
 float error1 = 0.0;
@@ -89,7 +96,9 @@
 bool button1_pressed;
 bool button2_pressed;
 float filter_value1;
-int past_speed = 0;
+float filter_value2;
+int past_speed1 = 0;
+int past_speed2 = 0;
 
 void do_nothing()
 
@@ -115,9 +124,13 @@
     }
 }
 
-const int EMG_cali_amount = 1000;
-float past_EMG_values[EMG_cali_amount];
-int past_EMG_count = 0;
+const int EMG_cali_amount1 = 1000;
+float past_EMG_values1[EMG_cali_amount1];
+int past_EMG_count1 = 0;
+
+const int EMG_cali_amount2 = 1000;
+float past_EMG_values2[EMG_cali_amount2];
+int past_EMG_count2 = 0;
 
 void cali_EMG()
 /*
@@ -129,23 +142,35 @@
         pc.printf("Started EMG calibration\r\nTime is %i\r\n", us_ticker_read());
         state_changed = false;
     }
-    if (past_EMG_count < EMG_cali_amount) {
-        past_EMG_values[past_EMG_count] = EMG1_filtered;
-        past_EMG_count++;
+    if (past_EMG_count1 < EMG_cali_amount1) {
+        past_EMG_values1[past_EMG_count1] = EMG1_filtered;
+        past_EMG_count1++;
     }
+    if (past_EMG_count2 < EMG_cali_amount2) {
+        past_EMG_values2[past_EMG_count2] = EMG3_filtered;
+        past_EMG_count2++;
+    }
+    
     else { //calibration has concluded
         pc.printf("Calibration concluded.\r\nTime is %i\r\n", us_ticker_read());
-        float highest = 0.0;
-        float sum = 0.0;
-        for(int i = 0; i<EMG_cali_amount; i++) {
-            sum += past_EMG_values[i];
+        float sum1 = 0.0;
+        for(int i = 0; i<EMG_cali_amount1; i++) {
+            sum1 += past_EMG_values1[i];
         }
-        float mean = sum/(float)EMG_cali_amount;
-        EMG_values.min = mean;
+        float mean1 = sum1/(float)EMG_cali_amount1;
+        EMG_values1.min = mean1;
+        /*
+        float sum2 = 0.0;
+        for(int i = 0; i<EMG_cali_amount1; i++) {
+            sum1 += past_EMG_values1[i];
+        }
+        float mean2 = sum2/(float)EMG_cali_amount2;
+        EMG_values2.min = mean2;
+        */
         //calibration done, moving to cali_enc
-        pc.printf("Min value: %f\r\n", EMG_values.min);
-        pc.printf("Length: %f\r\n", (float)EMG_cali_amount);
-        pc.printf("Calibration of the EMG is done, lower bound = %f\r\n", mean);
+        pc.printf("1st Min value: %f\r\n 2nd Min value: %f\r\n", EMG_values1.min, EMG_values2.min);
+        pc.printf("1st Length: %f\r\n 2nd Length: %f\r\n", (float)EMG_cali_amount1, (float)EMG_cali_amount2);
+        pc.printf("Calibration of the EMG is done, 1st lower bound = %f\r\n 2nd lower bound = %f\r\n", mean1, mean2);
         EMG_has_been_calibrated = true;
         state_changed = true;
         state = s_cali_enc;
@@ -212,22 +237,38 @@
 
 float EMG1_raw;
 float EMG2_raw;
+float EMG3_raw;
+float EMG4_raw;
+
 void measure_signals()
 {
     //only one emg input, reference and plus
     EMG1_raw = EMG1.read();
     EMG2_raw = EMG2.read();
+    EMG3_raw = EMG3.read();
+    EMG4_raw = EMG4.read();
     filter_value1 = fabs(bq2.step(fabs(bq1.step(EMG1_raw - EMG2_raw))));
+    filter_value2 = fabs(bq2.step(fabs(bq1.step(EMG3_raw - EMG4_raw))));
     
-    if (filter_value1 > EMG_values.max) {
-        EMG_values.max = filter_value1;
+    if (filter_value1 > EMG_values1.max) {
+        EMG_values1.max = filter_value1;
     }
     if (EMG_has_been_calibrated) {
-        EMG1_filtered = (filter_value1-EMG_values.min)/(EMG_values.max-EMG_values.min);
+        EMG1_filtered = (filter_value1-EMG_values1.min)/(EMG_values1.max-EMG_values1.min);
     }
     else {
         EMG1_filtered = filter_value1;
     }
+    
+    if (filter_value2 > EMG_values2.max) {
+        EMG_values2.max = filter_value2;
+    }
+    if (EMG_has_been_calibrated) {
+        EMG3_filtered = (filter_value2-EMG_values2.min)/(EMG_values2.max-EMG_values2.min);
+    }
+    else {
+        EMG3_filtered = filter_value2;
+    }
         
     enc1_value = enc1.getPulses();
     enc2_value = enc2.getPulses();
@@ -240,9 +281,13 @@
 }
 
 void motor_controller() {
-    int speed = schmitt_trigger(EMG1_filtered);
-    if (speed == -1) {speed = past_speed;}
-    past_speed = speed;
+    int speed1 = schmitt_trigger(EMG1_filtered);
+    if (speed1 == -1) {speed1 = past_speed1;}
+    past_speed1 = speed1;
+    
+    int speed2 = schmitt_trigger(EMG3_filtered);
+    if (speed2 == -1) {speed2 = past_speed1;}
+    past_speed2 = speed2;
     
     
     float error1, error2;
@@ -277,7 +322,9 @@
     motor2_pwm.write(actuator.duty_cycle2);
     
     scope.set(0, EMG1_filtered);
-    scope.set(1, past_speed);
+    scope.set(1, past_speed1);
+    scope.set(2, EMG3_filtered);
+    scope.set(3, past_speed2);
 }
 
 void state_machine()
@@ -347,9 +394,9 @@
 {
     int speed;
     speed = -1; //default value, this means the state should not change
-    if (i >  0.000 && i < 0.125) {speed = 0;}
-    if (i >  0.250 && i < 0.375) {speed = 1;}
-    if (i >  0.500 && i < 1.000) {speed = 2;}   
+    if (i >  0.000 && i < 0.125) {speed = 0; led_r = 0; led_g = 1; led_b = 1;}
+    if (i >  0.250 && i < 0.375) {speed = 1; led_g = 0; led_r = 1; led_b = 1;}
+    if (i >  0.500 && i < 1.000) {speed = 2; led_b = 0; led_r = 1; led_g = 1;}   
     return speed;
 }
 
@@ -366,7 +413,8 @@
     actuator.dir2 = 0;
 
     actuator.magnet = false;
-    EMG_values.max = 0.01;
+    EMG_values1.max = 0.01;
+    EMG_values2.max = 0.01;
 
     but1.fall(&but1_interrupt);
     but2.fall(&but2_interrupt);