Diff: main.cpp

Revision:

25:710d7d99b915

Parent:

23:9eeac9d1ecbe

--- a/main.cpp	Tue Oct 22 12:06:33 2019 +0000
+++ b/main.cpp	Thu Oct 24 11:30:32 2019 +0000
@@ -10,7 +10,6 @@
     General program layout
     better names for EMG input
 */
-
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "FastPWM.h"
@@ -20,17 +19,19 @@
 #define PI 3.14159265
 
 Serial pc(USBTX, USBRX);            //connect to pc
-HIDScope scope(2);                  //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)
-FastPWM motor2_pwm(D6);             //pwm 2 on shield (always D5)
+HIDScope scope(4);                  //HIDScope instance
+DigitalOut motor0_direction(D4);    //rotation motor 1 on shield (always D6)
+FastPWM motor0_pwm(D5);             //pwm 1 on shield (always D7)
+DigitalOut motor1_direction(D7);    //rotation motor 2 on shield (always D4)
+FastPWM motor1_pwm(D6);             //pwm 2 on shield (always D5)
 Ticker loop_ticker;                 //used in main()
 Ticker scope_ticker;
 InterruptIn but1(SW2);              //debounced button on biorobotics shield
 InterruptIn but2(D9);               //debounced button on biorobotics shield
-AnalogIn EMG1(A0);
-AnalogIn EMG2(A1);
+AnalogIn EMG1_sig(A0);
+AnalogIn EMG1_ref(A1);
+AnalogIn EMG2_sig(A2);
+AnalogIn EMG2_ref(A3);
 
 void check_failure();
 int schmitt_trigger(float i);
@@ -38,58 +39,47 @@
 QEI enc1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
 QEI enc2 (D1,  D2,  NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
 
-BiQuad bq1 (0.881889334678067,    -1.76377866935613,    0.8818893346780671,   -1.77069673005903, 0.797707797506027);
-BiQuad bq2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
+BiQuad bq1_1 (0.881889334678067,    -1.76377866935613,    0.8818893346780671,   -1.77069673005903, 0.797707797506027);
+BiQuad bq1_2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
+BiQuad bq2_1 (0.881889334678067,    -1.76377866935613,    0.8818893346780671,   -1.77069673005903, 0.797707797506027);
+BiQuad bq2_2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
 
 //variables
 enum States {s_idle, s_cali_EMG, s_cali_enc, s_moving_magnet_off, s_moving_magnet_on, s_homing, s_failure};
 States state;                       //using the States enum
 struct actuator_state {
-    float duty_cycle1;              //pwm of 1st motor
-    float duty_cycle2;              //pwm of 2nd motor
-    int dir1;                       //direction of 1st motor
-    int dir2;                       //direction of 2nd motor
+    float duty_cycle[2];            //pwm of 1st motor
+    int direction[2];               //direction of 1st motor
     bool magnet;                    //state of the magnet
 } actuator;
 
 struct EMG_params {
-    float max;  //params of the emg, tbd during calibration
-    float min;
-} EMG_values;
+    float max[2];  //params of the emg, tbd during calibration
+    float min[2];
+} EMG;
 
-struct PID {
-    float P;
-    float I;
-    float D;
-    float I_counter;
-};
-PID PID1;
-PID PID2;
+struct PID_struct {
+    float P[2];
+    float I[2];
+    float D[2];
+    float I_counter[2];
+} PID;
 
 float dt = 0.001;
-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
-float EMG1_filtered;
-float 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;
+float theta[2];
+int enc_zero[2] = {0, 0};//the zero position of the encoders, to be determined from the encoder calibration
+float EMG_filtered[2];
+int enc_value[2];
+float current_error[2] = {0.0, 0.0};
+float last_error[2] = {0.0, 0.0};
+float action[2];
 bool state_changed = false; //used to see if the state is "starting"
-volatile bool but1_pressed = false;
-volatile bool but2_pressed = false;
+volatile bool button1_pressed = false;
+volatile bool button2_pressed = false;
 volatile bool failure_occurred = false;
 bool EMG_has_been_calibrated;
-bool button1_pressed;
-bool button2_pressed;
-float filter_value1;
-int past_speed = 0;
+float filter_value[2];
+int past_speed[2] = {0, 0};
 
 void do_nothing()
 
@@ -116,7 +106,7 @@
 }
 
 const int EMG_cali_amount = 1000;
-float past_EMG_values[EMG_cali_amount];
+float past_EMG_values[2][EMG_cali_amount];
 int past_EMG_count = 0;
 
 void cali_EMG()
@@ -130,22 +120,25 @@
         state_changed = false;
     }
     if (past_EMG_count < EMG_cali_amount) {
-        past_EMG_values[past_EMG_count] = EMG1_filtered;
+        past_EMG_values[0][past_EMG_count] = EMG_filtered[0];
+        past_EMG_values[1][past_EMG_count] = EMG_filtered[1];
         past_EMG_count++;
     }
     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 sum[2] = {0.0, 0.0};
+        float mean[2] = {0.0, 0.0};
+        for(int c = 0; c<2; c++){
+            for(int i = 0; i<EMG_cali_amount; i++) {
+                sum[c] += past_EMG_values[c][i];
+            }
+            mean[c] = sum[c]/(float)EMG_cali_amount;
+            EMG.min[c] = mean[c];
         }
-        float mean = sum/(float)EMG_cali_amount;
-        EMG_values.min = mean;
+        
         //calibration done, moving to cali_enc
-        pc.printf("Min value: %f\r\n", EMG_values.min);
+        pc.printf("Min values: %f %f\r\n", EMG.min[0], EMG.min[1]);
         pc.printf("Length: %f\r\n", (float)EMG_cali_amount);
-        pc.printf("Calibration of the EMG is done, lower bound = %f\r\n", mean);
         EMG_has_been_calibrated = true;
         state_changed = true;
         state = s_cali_enc;
@@ -165,8 +158,8 @@
     }
     if (button1_pressed) {
         pc.printf("Encoder has been calibrated");
-        enc1_zero = enc1_value;
-        enc2_zero = enc2_value;
+        enc_zero[0] = enc_value[0];
+        enc_zero[1] = enc_value[1];
         button1_pressed = false;
         state = s_moving_magnet_off;
         state_changed = true;
@@ -210,74 +203,78 @@
     return;
 }
 
-float EMG1_raw;
-float EMG2_raw;
+float EMG_raw[2][2];
 void measure_signals()
 {
     //only one emg input, reference and plus
-    EMG1_raw = EMG1.read();
-    EMG2_raw = EMG2.read();
-    filter_value1 = fabs(bq2.step(fabs(bq1.step(EMG1_raw - EMG2_raw))));
+    EMG_raw[0][0] = EMG1_sig.read();
+    EMG_raw[0][1] = EMG1_ref.read();
+    EMG_raw[1][0] = EMG2_sig.read();
+    EMG_raw[1][1] = EMG2_ref.read();
+    filter_value[0] = fabs(bq1_2.step(fabs(bq1_1.step(EMG_raw[0][0] - EMG_raw[0][1]))));
+    filter_value[1] = fabs(bq2_2.step(fabs(bq2_1.step(EMG_raw[1][0] - EMG_raw[1][1]))));
+    
+    enc_value[0] = enc1.getPulses();
+    enc_value[1] = enc2.getPulses();
     
-    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);
+    for(int c = 0; c < 2; c++) {
+        if (filter_value[c] > EMG.max[c]) {
+            EMG.max[c] = filter_value[c];
+        }
+        if (EMG_has_been_calibrated) {
+            EMG_filtered[c] = (filter_value[c]-EMG.min[c])/(EMG.max[c]-EMG.min[c]);
+        }
+        else {
+            EMG_filtered[c] = filter_value[c];
+        }
+        enc_value[c] -= enc_zero[c];
+        theta[c] = (float)(enc_value[c])/(float)(8400*2*PI);
+        
     }
-    else {
-        EMG1_filtered = filter_value1;
-    }
-        
-    enc1_value = enc1.getPulses();
-    enc2_value = enc2.getPulses();
-    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 motor_controller() {
-    int speed = schmitt_trigger(EMG1_filtered);
-    if (speed == -1) {speed = past_speed;}
-    past_speed = speed;
-    
-    
-    float error1, error2;
-    //P part of the controller
-    float P_action1 = PID1.P * error1;
-    float P_action2 = PID2.P * error2;
+    int speed[2];
+    for(int c = 0; c<2; c++) {
+        speed[c] = schmitt_trigger(EMG_filtered[c]);
+        if (speed[c] == -1) {speed[c] = past_speed[c];}
+        past_speed[c] = speed[c];
+    }
     
-    //I part
-    PID1.I_counter += error1;
-    PID2.I_counter += error2;
-    float I_action1 = PID1.I_counter * PID1.I;
-    float I_action2 = PID2.I_counter * PID2.I;
+    float errors[2];
+    float P_action[2];
+    float I_action[2];
+    float D_action[2];
+    float velocity_estimate[2];
+    for (int c = 0; c<2; c++) {
+        
+        //P action
+        P_action[c] = PID.P[c] * errors[c];
+           
+        //I part        
+        PID.I_counter[c] += errors[c];
+        I_action[c] = PID.I_counter[c] * PID.I[c];
     
-    //D part
-    float velocity_estimate_1 = (error1-last_error1)/dt; //estimate of the time derivative of the error
-    float velocity_estimate_2 = (error2-last_error2)/dt;
-    float D_action1 = velocity_estimate_1 * PID1.D;
-    float D_action2 = velocity_estimate_2 * PID2.D;
+        //D part
+        velocity_estimate[c] = (errors[c]-last_error[c])/dt; //estimate of the time derivative of the error
+        D_action[c] = velocity_estimate[c] * PID.D[c];
     
-    action1 = P_action1 + I_action1 + D_action1;
-    action2 = P_action2 + I_action2 + D_action2;
-        
-    last_error1 = error1;
-    last_error2 = error2;
+        action[c] = P_action[c] + I_action[c] + D_action[c];        
+        last_error[c] = errors[c];
+    }
 }
 
 void output()
 {
-    motor1_direction = actuator.dir1;
-    motor2_direction = actuator.dir2;
-    motor1_pwm.write(actuator.duty_cycle1);
-    motor2_pwm.write(actuator.duty_cycle2);
+    motor0_direction = actuator.direction[0];
+    motor1_direction = actuator.direction[1];
+    motor0_pwm.write(actuator.duty_cycle[0]);
+    motor1_pwm.write(actuator.duty_cycle[1]);
     
-    scope.set(0, EMG1_filtered);
-    scope.set(1, past_speed);
+    scope.set(0, EMG_filtered[0]);
+    scope.set(1, past_speed[0]);    
+    scope.set(2, EMG_filtered[1]);
+    scope.set(3, past_speed[1]);
 }
 
 void state_machine()
@@ -321,8 +318,10 @@
 //state machines
 void check_failure()
 {
-    //state = s_failure;
-    //state_changed = true;
+    //if (failure_occurred) {
+    //    state = s_failure;
+    //    state_changed = true;
+    //}
 }
 
 void but1_interrupt()
@@ -359,15 +358,20 @@
     pc.printf("Executing main()... \r\n");
     state = s_idle;
 
-    motor2_pwm.period(1/160000);   // 1/frequency van waarop hij draait
+    motor0_pwm.period(1/160000);   // 1/frequency van waarop hij draait
     motor1_pwm.period(1/160000);   // 1/frequency van waarop hij draait
 
-    actuator.dir1 = 0;
-    actuator.dir2 = 0;
-
+    actuator.direction[0] = 0;
+    actuator.direction[1] = 0;
+    
+    
+    PID.I_counter[0] = 0.0;
+    PID.I_counter[1] = 0.0;
+    
     actuator.magnet = false;
-    EMG_values.max = 0.01;
-
+    EMG.max[0] = 0.01;
+    EMG.max[1] = 0.01;
+    
     but1.fall(&but1_interrupt);
     but2.fall(&but2_interrupt);
     scope_ticker.attach(&scope, &HIDScope::send, 0.02);