Diff: main.cpp

Revision:

26:088e397ec26f

Parent:

24:a9ec9b836fd9

Child:

27:d37b3a0e0f2b

--- a/main.cpp	Mon Oct 21 15:11:14 2019 +0000
+++ b/main.cpp	Tue Oct 29 11:13:15 2019 +0000
@@ -7,20 +7,23 @@
 
 // Pins
 MODSERIAL pc(USBTX, USBRX);
-InterruptIn stepresponse(D0);
+
+QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
+QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
 
 FastPWM     PWM_motor_1(D6);
 FastPWM     PWM_motor_2(D5);
 
 DigitalOut  direction_motor_1(D7);
 DigitalOut  direction_motor_2(D4);
+
 DigitalOut  led_red(LED1);
 DigitalOut  led_green(LED2);
 DigitalOut  led_blue(LED3);
 
-AnalogIn    emg_bl( A0 );
-AnalogIn    emg_br( A1 );
-AnalogIn    emg_leg( A2 );
+AnalogIn    emg_bl(A0);
+AnalogIn    emg_br(A1);
+AnalogIn    emg_leg(A2);
 
 InterruptIn button_1(SW2);
 InterruptIn button_2(SW3);
@@ -33,45 +36,43 @@
 Timer           sinus_time;
 Timeout         rest_timeout;
 Timeout         mvc_timeout;
+Timeout         led_timeout;
 enum    states  {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement};
 states  CurrentState = start;
 bool    StateChanged = true;
 enum    substates {rest_biceps_left, mvc_biceps_left, rest_biceps_right, mvc_biceps_right, rest_biceps_leg, mvc_biceps_leg};
 substates CurrentSubstate = rest_biceps_left;
 bool    SubstateChanged = true;
-bool    pressed_1    = false;
-bool    pressed_2    = false;
+volatile bool    pressed_1    = false;
+volatile bool    pressed_2    = false;
 HIDScope scope(3);
-QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
-QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
-volatile double theta_1;
+
+volatile float theta_1;
 //volatile float  theta_error_1;
 volatile float  theta_reference_1;
-volatile double theta_2;
+volatile float theta_2;
 //volatile float  theta_error_2;
 volatile float  theta_reference_2;
-float Ts    = 0.001;
+float Ts    = 0.01;
 float Kp;
 float Ki;
 float Kd;
 
-BiQuadChain highnotch;
-BiQuadChain low;
-BiQuad Lowpass1 (3.73938e-07, 7.47876e-07, 3.73938e-07, -1.90886e+00, 9.11279e-01);
-BiQuad Lowpass2 (1.00000e+00, 2.00000e+00, 1.00000e+00, -1.95979e+00, 9.62270e-01);
-BiQuad Highpass (9.69531e-01, -9.69531e-01, 0.00000e+00, -9.39063e-01, 0.00000e+00 );
-BiQuad Notch (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96863e-01);
+
+BiQuad Lowpass ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 );
+BiQuad Highpass ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00);
+BiQuad notch (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01);
+
 int n = 0;
 
-double emgFiltered_bl;
-double emgFiltered_br;
-double emgFiltered_leg;
-double emg;
-double xmvc_value = 1e-11;
+float emgFiltered_bl;
+float emgFiltered_br;
+float emgFiltered_leg;
+float emg;
+float xmvc_value = 1e-11;
 
-int   muscle;
 float sum = 0;
-float rest_value;
+float xrest_value;
 float rest_value_bl;
 float rest_value_br;
 float rest_value_leg;
@@ -81,6 +82,56 @@
 float mvc_value_leg;
 
 // functies
+void ledred()
+{
+    led_red = 0;
+    led_green = 1;
+    led_blue = 1;
+}
+void ledgreen()
+{
+    led_green=0;
+    led_blue=1;
+    led_red=1;
+}
+void ledblue()
+{
+    led_green=1;
+    led_blue=0;
+    led_red=1;
+}
+void ledyellow()
+{
+    led_green=0;
+    led_blue=1;
+    led_red=0;
+}
+void ledmagenta()
+{
+    led_green=1;
+    led_blue=0;
+    led_red=0;
+}
+void ledcyan()
+{
+    led_green=0;
+    led_blue=0;
+    led_red=1;
+}
+void ledwhite()
+{
+    led_green=0;
+    led_blue=0;
+    led_red=0;
+}
+void ledoff()
+{
+    led_green=1;
+    led_blue=1;
+    led_red=1;
+}
+
+
 float CalculateError(float theta_reference,float theta)
 {
     float theta_error = theta_reference-theta;
@@ -141,7 +192,7 @@
 void MotorControl()
 {
     ReadEncoder();
-    theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f); // voor test, moet weg in eindscript
+    theta_reference_1 = 360.0f*sin(0.1f*sinus_time.read()*2.0f*3.14f); // voor test, moet weg in eindscript
     CalculateDirectionMotor();
     PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1),0)/360.0f));
     PWM_motor_2.write(fabs(Controller(CalculateError(theta_reference_2,theta_2),1)/360.0f));
@@ -157,7 +208,7 @@
     pressed_2 = !pressed_2;
 }
 
-float EmgCalibration(double emgFiltered, float mvc_value, float rest_value)
+float EmgCalibration(float emgFiltered, float mvc_value, float rest_value)
 {
     float emgCalibrated;
     if (emgFiltered <= rest_value) {
@@ -173,139 +224,198 @@
 
 void emgsample()
 {
-    emgFiltered_bl = highnotch.step(emg_bl.read());
+    emgFiltered_bl = Highpass.step(emg_bl.read());
+    emgFiltered_bl = notch.step(emgFiltered_bl);
     emgFiltered_bl = fabs(emgFiltered_bl);
-    emgFiltered_bl = low.step(emgFiltered_bl);
+    emgFiltered_bl = Lowpass.step(emgFiltered_bl);
 
-    emgFiltered_br = highnotch.step(emg_br.read());
+    emgFiltered_br = Highpass.step(emg_br.read());
+    emgFiltered_br = notch.step(emgFiltered_br);
     emgFiltered_br = fabs(emgFiltered_br);
-    emgFiltered_br = low.step(emgFiltered_br);
+    emgFiltered_br = Lowpass.step(emgFiltered_br);
 
-    emgFiltered_leg = highnotch.step(emg_leg.read());
+    emgFiltered_leg = Highpass.step(emg_leg.read());
+    emgFiltered_leg = notch.step(emgFiltered_leg);
     emgFiltered_leg = fabs(emgFiltered_leg);
-    emgFiltered_leg = low.step(emgFiltered_leg);
+    emgFiltered_leg = Lowpass.step(emgFiltered_leg);
 }
 
 void rest()
 {
-    if (muscle == 0)
-    {
+    if (CurrentSubstate == rest_biceps_left) {
         emg = emgFiltered_bl;
+        //pc.printf("emg: %f \n\r",emgFiltered_bl);
     }
-    if (muscle == 2)
-    {
+    if (CurrentSubstate == rest_biceps_right) {
         emg = emgFiltered_br;
     }
-    if (muscle == 4)
-    {
+    if (CurrentSubstate == rest_biceps_leg) {
         emg = emgFiltered_leg;
     }
-    if (n < 50) 
-    {
+    if (n < 500) {
+        ledred();
         sum = sum + emg;
+        //pc.printf("sum: %f \n\r",sum);
         n++;
-        rest_value = float (sum/n);
-        rest_timeout.attach(rest,0.01f);
+        rest_timeout.attach(rest,0.001f);
     }
-    if (n == 50) 
-    {
+    if (n == 500) {
         sum = sum + emg;
+        //pc.printf("sum: %f \n\r",sum);
         n++;
-        rest_value = float (sum/n);
-        n = 0;
-        sum = 0;
-        if (muscle == 0) 
-        {
-            rest_value_bl = rest_value;
+        xrest_value = float (sum/n);
+        if (CurrentSubstate == rest_biceps_left) {
+            rest_value_bl = xrest_value;
+            pc.printf("rest_value_bl %f \n\r", rest_value_bl);
             CurrentSubstate = mvc_biceps_left;
             SubstateChanged = true;
-            led_red = 1;
+            ledblue();
+            
         }
-        if (muscle == 2) 
-        {
-            rest_value_br = rest_value;
+        if (CurrentSubstate == rest_biceps_right) {
+            rest_value_br = xrest_value;
+            pc.printf("rest_value_br %f \n\r", rest_value_br);
             CurrentSubstate = mvc_biceps_right;
             SubstateChanged = true;
-            led_red = 1;
+            ledmagenta();
         }
-        if (muscle == 4) 
-        {
-            rest_value_leg = rest_value;
+        if (CurrentSubstate == rest_biceps_leg) {
+            rest_value_leg = xrest_value;
+            pc.printf("rest_value_leg %f \n\r", rest_value_leg);
+            pc.printf("rest_value_bl %f \n\r", rest_value_bl);
             CurrentSubstate = mvc_biceps_leg;
             SubstateChanged = true;
-            led_red = 1;
+            ledwhite();
         }
     }
 }
 
 void mvc()
 {
-    if (muscle == 1)
-    {
+    if (CurrentSubstate == mvc_biceps_left) {
         emg = emgFiltered_bl;
     }
-    if (muscle == 3)
-    {
+    if (CurrentSubstate == mvc_biceps_right) {
         emg = emgFiltered_br;
     }
-    if (muscle == 5)
-    {
+    if (CurrentSubstate == mvc_biceps_leg) {
         emg = emgFiltered_leg;
     }
-    if (emg >= xmvc_value)
-    {
+    if (emg >= xmvc_value) {
         xmvc_value = emg;
-    }  
+    }
     n++;
-    if (n < 100) 
-    {
-        mvc_timeout.attach(mvc,0.01f);
+    if (n < 1000) {
+        mvc_timeout.attach(mvc,0.001f);
+        ledred();
     }
-    if (n == 100)
-    {
-        n = 0;
-        if (muscle == 1)
-        {
+    if (n == 1000) {
+        if (CurrentSubstate == mvc_biceps_left) {
             mvc_value_bl = xmvc_value;
+            pc.printf("mvc_value_bl %f \n\r", mvc_value_bl);
             CurrentSubstate = rest_biceps_right;
             SubstateChanged = true;
-            led_red = 1;
+            ledyellow();
         }
-        if (muscle == 3)
-        {
+        if (CurrentSubstate == mvc_biceps_right) {
             mvc_value_br = xmvc_value;
+            pc.printf("mvc_value_br %f \n\r", mvc_value_br);
             CurrentSubstate = rest_biceps_leg;
             SubstateChanged = true;
-            led_red = 1;
+            ledcyan();
         }
-        if (muscle == 5)
-        {
+        if (CurrentSubstate == mvc_biceps_leg) {
             mvc_value_leg = xmvc_value;
+            pc.printf("mvc_value_leg %f \n\r", mvc_value_leg);
             CurrentState = vertical_movement;
             StateChanged = true;
-            led_red = 1;
+            ledoff();
         }
         xmvc_value = 1e-11;
-        led_red = 1;
     }
 }
 
+float emgCalibrated_bl;
+float emgCalibrated_br;
+float emgCalibrated_leg;
+
 void WriteScope()
 {
-    //scope.set(0,theta_1);
-    // scope.set(1,CalculateError(theta_reference_1,theta_1));
-    // scope.set(2,theta_reference_1);
+    emgsample();
+    /*
+    if (emgFiltered_bl <= rest_value_bl) {
+        emgCalibrated_bl = 0;
+    }
+    if (emgFiltered_bl >= mvc_value_bl) {
+        emgCalibrated_bl = 1;
+    } else {
+        emgCalibrated_bl = (emgFiltered_bl - rest_value_bl) / (mvc_value_bl - rest_value_bl);
+    }
+    if (emgFiltered_br <= rest_value_br) {
+        emgCalibrated_br = 0;
+    }
+    if (emgFiltered_br >= mvc_value_br) {
+        emgCalibrated_br = 1;
+    } else {
+        emgCalibrated_br = (emgFiltered_br - rest_value_br) / (mvc_value_br - rest_value_br);
+    }
+    if (emgFiltered_leg <= rest_value_leg) {
+        emgCalibrated_leg = 0;
+    }
+    if (emgFiltered_leg >= mvc_value_leg) {
+        emgCalibrated_leg = 1;
+    } else {
+        emgCalibrated_leg = (emgFiltered_leg - rest_value_leg) / (mvc_value_leg - rest_value_leg);
+    }
+    */
+    /*
     scope.set(0, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl));
-    scope.set(1, EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br) );
+    scope.set(1, EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br));
     scope.set(2, EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg));
+    */
+    /*
+    scope.set(0, emgCalibrated_bl);
+    scope.set(1, emgCalibrated_br);
+    scope.set(2, emgCalibrated_leg);
+    */
+    scope.set(0, emg_bl.read());
+    scope.set(1, emgCalibrated_bl);
+    scope.set(2, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl));
     scope.send();
 }
 
 void SubstateTransition()
 {
-    pressed_1 = false;
-    pressed_2 = false;
-    SubstateChanged = false;
+    if (SubstateChanged == true) {
+        SubstateChanged = false;
+        pressed_1 = false;
+        pressed_2 = false;
+        if (CurrentSubstate == rest_biceps_left) {
+            ledgreen();
+            pc.printf("groen \n\r");
+            pc.printf("Initiating rest_biceps_left\n\r");
+        }
+        if (CurrentSubstate == mvc_biceps_left) {
+            //ledblue();
+            pc.printf("Initiating mvc_biceps_left\n\r");
+        }
+        if (CurrentSubstate == rest_biceps_right) {
+            //ledyellow();
+            pc.printf("Initiating rest_biceps_right\n\r");
+        }
+        if (CurrentSubstate == mvc_biceps_right) {
+            //ledmagenta();
+            pc.printf("Initiating mvc_biceps_right\n\r");
+        }
+        if (CurrentSubstate == rest_biceps_leg) {
+            //ledcyan();
+            pc.printf("Initiating rest_biceps_leg\n\r");
+        }
+        if (CurrentSubstate == mvc_biceps_leg) {
+            //ledwhite();
+            pc.printf("Initiating mvc_biceps_leg\n\r");
+        }
+    }
 }
 
 void while_start()
@@ -331,7 +441,7 @@
 void while_demo_mode()
 {
     // Do demo mode stuff
-    if (pressed_1 || pressed_2) {
+    if ((pressed_1) || (pressed_2)) {
         CurrentState = emg_calibration;
         StateChanged = true;
     }
@@ -343,72 +453,58 @@
     switch (CurrentSubstate) {
         case rest_biceps_left:
             SubstateTransition();
-            muscle = 0;
-            led_green = 0;
-            if (pressed_1 || pressed_2) 
-            {
-                led_green = 1;
-                led_red = 0;
+            if ((pressed_1) || (pressed_2)) {
+                pressed_1 = false;
+                pressed_2 = false;
+                n = 0;
+                sum = 0;
                 rest();
             }
             break;
         case mvc_biceps_left:
             SubstateTransition();
-            muscle = 1;
-            led_blue = 0;
-            if (pressed_1 || pressed_2) 
-            {
-                led_blue = 1;
-                led_red = 0;
+            if ((pressed_1) || (pressed_2)) {
+                pressed_1 = false;
+                pressed_2 = false;
+                n = 0;
                 mvc();
             }
             break;
         case rest_biceps_right:
             SubstateTransition();
-            muscle = 2;
-            led_red = 0;
-            led_green = 0;
-            if (pressed_1 || pressed_2) 
-            {
-                led_green = 1;
+            if ((pressed_1) || (pressed_2)) {
+                pressed_1 = false;
+                pressed_2 = false;
+                n = 0;
+                sum = 0;
                 rest();
             }
             break;
         case mvc_biceps_right:
             SubstateTransition();
-            muscle = 3;
-            led_red = 0;
-            led_blue = 0;
-            if (pressed_1 || pressed_2) 
-            {
-                led_blue = 1;
+            if ((pressed_1) || (pressed_2)) {
+                pressed_1 = false;
+                pressed_2 = false;
+                n = 0;
                 mvc();
             }
             break;
         case rest_biceps_leg:
             SubstateTransition();
-            muscle = 4;
-            led_green = 0;
-            led_blue = 0;
-            if (pressed_1 || pressed_2) 
-            {
-                led_green = 1;
-                led_blue = 1;
-                led_red = 0;
+            if ((pressed_1) || (pressed_2)) {
+                pressed_1 = false;
+                pressed_2 = false;
+                n = 0;
+                sum = 0;
                 rest();
             }
             break;
         case mvc_biceps_leg:
             SubstateTransition();
-            muscle = 5;
-            led_red = 0;
-            led_green = 0;
-            led_blue = 0;
-            if (pressed_1 || pressed_2) 
-            {
-                led_green = 1;
-                led_blue = 1;
-                led_red = 0;
+            if ((pressed_1) || (pressed_2)) {
+                pressed_1 = false;
+                pressed_2 = false;
+                n = 0;
                 mvc();
             }
             break;
@@ -420,7 +516,7 @@
 void while_vertical_movement()
 {
     // Do vertical movement stuff
-    if (pressed_1 || pressed_2) { // EMG gebaseerde threshold aanmaken
+    if ((pressed_1) || (pressed_2)) { // EMG gebaseerde threshold aanmaken
         CurrentState = horizontal_movement;
         StateChanged = true;
     }
@@ -429,7 +525,7 @@
 void while_horizontal_movement()
 {
     // Do horizontal movement stuff
-    if (pressed_1 || pressed_2) { // EMG gebaseerde threshold aanmaken
+    if ((pressed_1) || (pressed_2)) { // EMG gebaseerde threshold aanmaken
         CurrentState = vertical_movement;
         StateChanged = true;
     }
@@ -437,9 +533,9 @@
 
 void StateTransition()
 {
-    pressed_1 = false;
-    pressed_2 = false;
     if (StateChanged) {
+        pressed_1 = false;
+        pressed_2 = false;
         if (CurrentState == start) {
             pc.printf("Initiating start.\n\r");
         }
@@ -499,14 +595,15 @@
 {
     pc.baud(115200);
     pc.printf("Hello World!\n\r");
+    ledoff();
     button_1.fall(go_next_1);
     button_2.fall(go_next_2);
-    sinus_time.start();
-    PWM_motor_1.period_ms(10);
-    motor_control.attach(&MotorControl, Ts);
-    write_scope.attach(&WriteScope, 0.1);
+    //sinus_time.start();
+    //PWM_motor_1.period_ms(10);
+    //motor_control.attach(&MotorControl, Ts);
+    write_scope.attach(&WriteScope, 0.001);
     //TickerStateMachine.attach(StateMachine,1.00f);
     while(true) {
         StateMachine();
     }
-}
\ No newline at end of file
+}