Diff: main.cpp

Revision:

23:9eeac9d1ecbe

Parent:

21:bea7c8a08e1d

Child:

24:710d7d99b915

--- a/main.cpp	Fri Oct 18 09:05:58 2019 +0000
+++ b/main.cpp	Tue Oct 22 12:06:33 2019 +0000
@@ -20,27 +20,26 @@
 #define PI 3.14159265
 
 Serial pc(USBTX, USBRX);            //connect to pc
-HIDScope scope(1);                  //HIDScope instance
+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)
 Ticker loop_ticker;                 //used in main()
 Ticker scope_ticker;
-AnalogIn Pot1(A1);                  //pot 1 on biorobotics shield
-AnalogIn Pot2(A0);                  //pot 2 on biorobotics shield
-InterruptIn but1(D10);              //debounced button on biorobotics shield
+InterruptIn but1(SW2);              //debounced button on biorobotics shield
 InterruptIn but2(D9);               //debounced button on biorobotics shield
-AnalogIn EMG1(A2);
-AnalogIn EMG2(A3);
+AnalogIn EMG1(A0);
+AnalogIn EMG2(A1);
 
 void check_failure();
+int schmitt_trigger(float i);
 
 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 (0.000198358203463849, 0.000396716406927699, 0.000198358203463849, -1.96262073248799, 0.963423352820821);
+BiQuad bq2 (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};
@@ -72,8 +71,8 @@
 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;
+float EMG1_filtered;
+float EMG2_filtered;
 int enc1_value;
 int enc2_value;
 float error1 = 0.0;
@@ -86,14 +85,11 @@
 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 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;
+float filter_value1;
+int past_speed = 0;
 
 void do_nothing()
 
@@ -103,7 +99,7 @@
 {
     if (button1_pressed) {
         state_changed = true;
-        state = s_cali_enc;
+        state = s_cali_EMG;
         button1_pressed = false;
     }
 }
@@ -119,6 +115,10 @@
     }
 }
 
+const int EMG_cali_amount = 1000;
+float past_EMG_values[EMG_cali_amount];
+int past_EMG_count = 0;
+
 void cali_EMG()
 /*
     Calibration of the EMG. Values determined during calibration should be
@@ -126,14 +126,16 @@
 */
 {
     if (state_changed) {
-        pc.printf("Started EMG calibration\r\n");
+        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++;
     }
-    else { //calibration is has concluded
+    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];
@@ -141,7 +143,9 @@
         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);
+        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);
         EMG_has_been_calibrated = true;
         state_changed = true;
         state = s_cali_enc;
@@ -206,12 +210,14 @@
     return;
 }
 
+float EMG1_raw;
+float EMG2_raw;
 void measure_signals()
 {
     //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))));
+    EMG1_raw = EMG1.read();
+    EMG2_raw = EMG2.read();
+    filter_value1 = fabs(bq2.step(fabs(bq1.step(EMG1_raw - EMG2_raw))));
     
     if (filter_value1 > EMG_values.max) {
         EMG_values.max = filter_value1;
@@ -233,35 +239,12 @@
 
 }
 
-void sample()
-{
-    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
-    float emg0_value = EMG1.read(); 
-    float emg1_value = EMG2.read();
-    
-    //double filter_value = bqc.step(emg1_value);
-    float filter_value = fabs(bq2.step(fabs(bq1.step(emg0_value - emg1_value))));
-    if (filter_value > EMG_values.max) {
-        EMG_values.max = filter_value;
-        }
-    if (EMG_values.min > filter_value) {
-        EMG_values.min = filter_value;
-        }
+void motor_controller() {
+    int speed = schmitt_trigger(EMG1_filtered);
+    if (speed == -1) {speed = past_speed;}
+    past_speed = speed;
     
-    filter_value = filter_value-EMG_values.min;
-    filter_value = filter_value/(EMG_values.max-EMG_values.min);
     
-    scope.set(0, EMG1.read() );
-    scope.set(1, EMG2.read() );
-    scope.set(2, filter_value);
-    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
-    *  Ensure that enough channels are available (HIDScope scope( 2 ))
-    *  Finally, send all channels to the PC at once */
-    scope.send();
-    /* To indicate that the function is working, the LED is toggled */
-}
-
-void motor_controller() {
     float error1, error2;
     //P part of the controller
     float P_action1 = PID1.P * error1;
@@ -294,6 +277,7 @@
     motor2_pwm.write(actuator.duty_cycle2);
     
     scope.set(0, EMG1_filtered);
+    scope.set(1, past_speed);
 }
 
 void state_machine()
@@ -346,7 +330,7 @@
     if(but2.read()) {//both buttons are pressed
         failure_occurred = true;
     }
-    but1_pressed = true;
+    button1_pressed = true;
     pc.printf("Button 1 pressed \n\r");
 }
 
@@ -355,7 +339,7 @@
     if(but1.read()) {//both buttons are pressed
         failure_occurred = true;
     }
-    but2_pressed = true;
+    button2_pressed = true;
     pc.printf("Button 2 pressed \n\r");
 }
 
@@ -363,11 +347,9 @@
 {
     int speed;
     speed = -1; //default value, this means the state should not change
-    if (i >  0/14 && i <  2/14) {speed = 0;}
-    if (i >  3/14 && i <  5/14) {speed = 1;}
-    if (i >  6/14 && i <  8/14) {speed = 2;}
-    if (i >  9/14 && i < 11/14) {speed = 3;}
-    if (i > 12/14 && i < 14/14) {speed = 4;}    
+    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;}   
     return speed;
 }
 
@@ -384,6 +366,7 @@
     actuator.dir2 = 0;
 
     actuator.magnet = false;
+    EMG_values.max = 0.01;
 
     but1.fall(&but1_interrupt);
     but2.fall(&but2_interrupt);