Diff: main.cpp

Revision:

16:696e9cbcc823

Parent:

15:9a1f34bc9958

Child:

17:615c5d8b3710

--- a/main.cpp	Mon Oct 07 13:38:54 2019 +0000
+++ b/main.cpp	Thu Oct 10 11:33:38 2019 +0000
@@ -14,6 +14,8 @@
 AnalogIn Pot2(A0);                  //pot 2 on biorobotics shield
 InterruptIn but1(D10);              //debounced button on biorobotics shield
 InterruptIn but2(D9);               //debounced button on biorobotics shield
+AnalogIn EMG1(A2);
+AnalogIn EMG2(A3);
 
 QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
 QEI encoder2 (D1,  D2,  NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
@@ -31,25 +33,38 @@
 } actuators;
 
 struct EMG_params {
-    float idk;  //params of the emg, tbd during calibration
-} emg_values;
+    float max;  //params of the emg, tbd during calibration
+} EMG_values;
 
-int enc_zero;   //the zero position of the encoders, to be determined from the
-//encoder calibration
+int enc1_zero;  //the zero position of the encoders, to be determined from the
+int enc2_zero;  //encoder calibration
+int EMG1_filtered;
+int EMG2_filtered;
+int enc1_value;
+int enc2_value;
 
 //variables used throughout the program
 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;
 
 void do_nothing()
 
 /*
     Idle state. Used in the beginning, before the calibration states.
 */
-{}
+{
+    if (button1_pressed) {
+        state_changed = true;
+        state = cali_enc;
+        button1_pressed = false;
+    }
+}
 
 void failure()
 /*
@@ -64,7 +79,7 @@
 
 void cali_EMG()
 /*
-    Calibratioin of the EMG. Values determined during calibration should be
+    Calibration of the EMG. Values determined during calibration should be
     added to the EMG_params instance.
 */
 {
@@ -73,6 +88,7 @@
         state_changed = false;
     }
 }
+
 void cali_enc()
 /*
     Calibration of the encoder. The encoder should be moved to the lowest
@@ -84,7 +100,17 @@
         pc.printf("Started encoder calibration\r\n");
         state_changed = false;
     }
+    if (button1_pressed) {
+        enc1_zero = enc1_value;
+        enc2_zero = enc2_value;
+        enc_has_been_calibrated = true;
+        button1_pressed = false;
+        state = moving_magnet_off;
+        state_changed = true;
+
+    }
 }
+
 void moving_magnet_off()
 /*
     Moving with the magnet disabled. This is the part from the home position
@@ -95,8 +121,8 @@
         pc.printf("Moving without magnet\r\n");
         state_changed = false;
     }
-    return;
 }
+
 void moving_magnet_on()
 /*
     Moving with the magnet enabled. This is the part of the movement from the
@@ -121,36 +147,29 @@
     return;
 }
 
-// the funtions needed to run the program
 void measure_signals()
 {
-    return;
+    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();
 }
 
-void do_nothing()
-{
-    actuator.duty_cycle1 = 0;
-    actuator.duty_cycle2 = 0;
-
-
-    //state guard
-    if (but1_pressed) { //this moves the program from the idle to cw state
-        current_state = cw;
-        state_changed = true; //to show next state it can initialize
-        pc.printf("Changed state from idle to cw\r\n");
-        but1_pressed = false; //reset button 1
-    }
-
-}
-
-
 void output()
 {
-    return;
+    motor1_direction = actuator.dir1;
+    motor2_direction = acuator.dir2;
+    motor1_pwm.write(actuator.pwm1);
+    motor2_pwm.write(actuator.pwm2);
 }
 
 void state_machine()
 {
+    check_failure(); //check for an error in the last loop before state machine
     //run current state
     switch (current_state) {
         case idle:
@@ -187,17 +206,40 @@
 
 //Helper functions, not directly called by the main_loop functions or
 //state machines
-void but1_interrupt ()
+void check_failure()
 {
+    state = failure;
+    state_changed = true;
+}
+
+void but1_interrupt()
+{
+    if(button2.read()) {//both buttons are pressed
+        failure_occurred = true;
+    }
     but1_pressed = true;
     pc.printf("Button 1 pressed \n\r");
 }
 
-void but2_interrupt ()
+void but2_interrupt()
 {
+    if(button1.read()) {//both buttons are pressed
+        failure_occurred = true;
+    }
     but2_pressed = true;
     pc.printf("Button 2 pressed \n\r");
 }
+int schmitt_trigger(float i) 
+{
+    speed = -1; //default value, this means the state should not change
+    float levels = 5.0;
+    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;}    
+    return speed;
+}
 
 int main()
 {
@@ -217,5 +259,7 @@
     but2.fall(&but2_interrupt);
     loop_ticker.attach(&main_loop, 0.001f); //main loop at 1kHz
     pc.printf("Main_loop is running\n\r");
-    while (true) {}
-}
\ No newline at end of file
+    while (true) {
+        wait(0.1f);
+    }
+}]
\ No newline at end of file