Diff: main.cpp

Revision:

17:615c5d8b3710

Parent:

16:696e9cbcc823

Child:

18:dddc8d9f7638

--- a/main.cpp	Thu Oct 10 11:33:38 2019 +0000
+++ b/main.cpp	Fri Oct 11 09:51:35 2019 +0000
@@ -2,9 +2,12 @@
 #include "MODSERIAL.h"
 #include "FastPWM.h"
 #include "QEI.h"
+#include "HIDScope.h"
+#include "BiQuad.h"
 #define PI 3.14159265
 
 Serial pc(USBTX, USBRX);            //connect to pc
+HIDScope scope(3);                  //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)
@@ -17,12 +20,16 @@
 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
+void check_failure();
 
+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);
 
 //variables
-enum States {idle, cali_EMG, cali_enc, moving_magnet_off, moving_magnet_on, homing, failure};
+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
@@ -30,10 +37,11 @@
     int dir1;                       //direction of 1st motor
     int dir2;                       //direction of 2nd motor
     bool magnet;                    //state of the magnet
-} actuators;
+} actuator;
 
 struct EMG_params {
     float max;  //params of the emg, tbd during calibration
+    float min;
 } EMG_values;
 
 int enc1_zero;  //the zero position of the encoders, to be determined from the
@@ -52,6 +60,8 @@
 float pot_2;
 bool enc_has_been_calibrated;
 bool EMG_has_been_calibrated;
+bool button1_pressed;
+bool button2_pressed;
 
 void do_nothing()
 
@@ -61,7 +71,7 @@
 {
     if (button1_pressed) {
         state_changed = true;
-        state = cali_enc;
+        state = s_cali_enc;
         button1_pressed = false;
     }
 }
@@ -105,7 +115,7 @@
         enc2_zero = enc2_value;
         enc_has_been_calibrated = true;
         button1_pressed = false;
-        state = moving_magnet_off;
+        state = s_moving_magnet_off;
         state_changed = true;
 
     }
@@ -149,6 +159,8 @@
 
 void measure_signals()
 {
+    float EMG1_raw;
+    float EMG2_raw;
     enc1_value = enc1.getPulses();
     enc2_value = enc2.getPulses();
     if (enc_has_been_calibrated) {
@@ -159,38 +171,70 @@
     EMG2_raw = EMG2.read();
 }
 
+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;
+        }
+    
+    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() {
+    
+}
+
 void output()
 {
     motor1_direction = actuator.dir1;
-    motor2_direction = acuator.dir2;
-    motor1_pwm.write(actuator.pwm1);
-    motor2_pwm.write(actuator.pwm2);
+    motor2_direction = actuator.dir2;
+    motor1_pwm.write(actuator.duty_cycle1);
+    motor2_pwm.write(actuator.duty_cycle2);
 }
 
 void state_machine()
 {
     check_failure(); //check for an error in the last loop before state machine
     //run current state
-    switch (current_state) {
-        case idle:
+    switch (state) {
+        case s_idle:
             do_nothing();
             break;
-        case failure:
+        case s_failure:
             failure();
             break;
-        case cali_EMG:
+        case s_cali_EMG:
             cali_EMG();
             break;
-        case cali_ENC:
-            cali_encoder();
+        case s_cali_enc:
+            cali_enc();
             break;
-        case moving_magnet_on:
+        case s_moving_magnet_on:
             moving_magnet_on();
             break;
-        case moving_magnet_off:
+        case s_moving_magnet_off:
             moving_magnet_off();
             break;
-        case homing:
+        case s_homing:
             homing();
             break;
     }
@@ -208,13 +252,13 @@
 //state machines
 void check_failure()
 {
-    state = failure;
+    state = s_failure;
     state_changed = true;
 }
 
 void but1_interrupt()
 {
-    if(button2.read()) {//both buttons are pressed
+    if(but2.read()) {//both buttons are pressed
         failure_occurred = true;
     }
     but1_pressed = true;
@@ -223,19 +267,20 @@
 
 void but2_interrupt()
 {
-    if(button1.read()) {//both buttons are pressed
+    if(but1.read()) {//both buttons are pressed
         failure_occurred = true;
     }
     but2_pressed = true;
     pc.printf("Button 2 pressed \n\r");
 }
+
 int schmitt_trigger(float i) 
 {
+    int speed;
     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 >  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;
@@ -245,10 +290,10 @@
 {
     pc.baud(115200);
     pc.printf("Executing main()... \r\n");
-    current_state = idle;
+    state = s_idle;
 
-    motor2_pwm.period(1/160000f);   // 1/frequency van waarop hij draait
-    motor1_pwm.period(1/160000f);   // 1/frequency van waarop hij draait
+    motor2_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;
@@ -262,4 +307,4 @@
     while (true) {
         wait(0.1f);
     }
-}]
\ No newline at end of file
+}
\ No newline at end of file