Diff: main.cpp

Revision:

24:a9ec9b836fd9

Parent:

23:78898ddfb103

Child:

25:832b26afbe0b

Child:

26:088e397ec26f

--- a/main.cpp	Mon Oct 21 10:14:44 2019 +0000
+++ b/main.cpp	Mon Oct 21 15:11:14 2019 +0000
@@ -8,24 +8,39 @@
 // Pins
 MODSERIAL pc(USBTX, USBRX);
 InterruptIn stepresponse(D0);
+
 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 );
+
 InterruptIn button_1(SW2);
 InterruptIn button_2(SW3);
 
+
 // variables
 Ticker          TickerStateMachine;
 Ticker          motor_control;
 Ticker          write_scope;
 Timer           sinus_time;
+Timeout         rest_timeout;
+Timeout         mvc_timeout;
 enum    states  {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement};
 states  CurrentState = start;
 bool    StateChanged = true;
-bool    demo    = false;
-bool    emg     = false;
-bool    next    = false;
+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;
 HIDScope scope(3);
 QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
 QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
@@ -40,6 +55,31 @@
 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);
+int n = 0;
+
+double emgFiltered_bl;
+double emgFiltered_br;
+double emgFiltered_leg;
+double emg;
+double xmvc_value = 1e-11;
+
+int   muscle;
+float sum = 0;
+float rest_value;
+float rest_value_bl;
+float rest_value_br;
+float rest_value_leg;
+
+float mvc_value_bl;
+float mvc_value_br;
+float mvc_value_leg;
+
 // functies
 float CalculateError(float theta_reference,float theta)
 {
@@ -49,17 +89,14 @@
 
 float Controller(float theta_error, bool motor)
 {
-    if (motor == false)
-    {
+    if (motor == false) {
         float K     = 1;
         float ti    = 0.1;
         float td    = 10;
         Kp    = K*(1+td/ti);
         Ki    = K/ti;
         Kd    = K*td;
-    }
-    else
-    {
+    } else {
         float K     = 1;
         float ti    = 0.1;
         float td    = 10;
@@ -70,46 +107,38 @@
     static float error_integral = 0;
     static float error_prev = 0;
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-    
+
     // Proportional part:
     float torque_p = Kp * theta_error;
-    
+
     // Integral part:
     error_integral = error_integral + theta_error * Ts;
     float torque_i = Ki * error_integral;
-    
+
     // Derivative part:
     float error_derivative = (theta_error - error_prev)/Ts;
     float filtered_error_derivative = LowPassFilter.step(error_derivative);
     float torque_d = Kd * filtered_error_derivative;
     error_prev = theta_error;
-    
+
     // Sum all parts and return it
     float torque = torque_p + torque_i + torque_d;
     return torque;
 }
 
-void CalculateDirectionMotor() 
+void CalculateDirectionMotor()
 {
     direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1),0) <= 0.0f;
     direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2),1) <= 0.0f;
 }
 
-void WriteScope()
-{
-    scope.set(0,theta_1);
-    scope.set(1,CalculateError(theta_reference_1,theta_1));
-    scope.set(2,theta_reference_1);
-    scope.send();
-}
-
 void ReadEncoder()
 {
     theta_1 = ((360.0f/64.0f)*(float)encoder_1.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
     theta_2 = ((360.0f/64.0f)*(float)encoder_2.getPulses())/131.25f;
 }
 
-void MotorControl() 
+void MotorControl()
 {
     ReadEncoder();
     theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f); // voor test, moet weg in eindscript
@@ -120,13 +149,163 @@
 
 void go_next_1()
 {
-    demo = !demo;
+    pressed_1 = !pressed_1;
 }
 
 void go_next_2()
 {
-    emg = !emg;
-    next = emg;
+    pressed_2 = !pressed_2;
+}
+
+float EmgCalibration(double emgFiltered, float mvc_value, float rest_value)
+{
+    float emgCalibrated;
+    if (emgFiltered <= rest_value) {
+        emgCalibrated = 0;
+    }
+    if (emgFiltered >= mvc_value) {
+        emgCalibrated = 1;
+    } else {
+        emgCalibrated = (emgFiltered-rest_value)/(mvc_value-rest_value);
+    }
+    return emgCalibrated;
+}
+
+void emgsample()
+{
+    emgFiltered_bl = highnotch.step(emg_bl.read());
+    emgFiltered_bl = fabs(emgFiltered_bl);
+    emgFiltered_bl = low.step(emgFiltered_bl);
+
+    emgFiltered_br = highnotch.step(emg_br.read());
+    emgFiltered_br = fabs(emgFiltered_br);
+    emgFiltered_br = low.step(emgFiltered_br);
+
+    emgFiltered_leg = highnotch.step(emg_leg.read());
+    emgFiltered_leg = fabs(emgFiltered_leg);
+    emgFiltered_leg = low.step(emgFiltered_leg);
+}
+
+void rest()
+{
+    if (muscle == 0)
+    {
+        emg = emgFiltered_bl;
+    }
+    if (muscle == 2)
+    {
+        emg = emgFiltered_br;
+    }
+    if (muscle == 4)
+    {
+        emg = emgFiltered_leg;
+    }
+    if (n < 50) 
+    {
+        sum = sum + emg;
+        n++;
+        rest_value = float (sum/n);
+        rest_timeout.attach(rest,0.01f);
+    }
+    if (n == 50) 
+    {
+        sum = sum + emg;
+        n++;
+        rest_value = float (sum/n);
+        n = 0;
+        sum = 0;
+        if (muscle == 0) 
+        {
+            rest_value_bl = rest_value;
+            CurrentSubstate = mvc_biceps_left;
+            SubstateChanged = true;
+            led_red = 1;
+        }
+        if (muscle == 2) 
+        {
+            rest_value_br = rest_value;
+            CurrentSubstate = mvc_biceps_right;
+            SubstateChanged = true;
+            led_red = 1;
+        }
+        if (muscle == 4) 
+        {
+            rest_value_leg = rest_value;
+            CurrentSubstate = mvc_biceps_leg;
+            SubstateChanged = true;
+            led_red = 1;
+        }
+    }
+}
+
+void mvc()
+{
+    if (muscle == 1)
+    {
+        emg = emgFiltered_bl;
+    }
+    if (muscle == 3)
+    {
+        emg = emgFiltered_br;
+    }
+    if (muscle == 5)
+    {
+        emg = emgFiltered_leg;
+    }
+    if (emg >= xmvc_value)
+    {
+        xmvc_value = emg;
+    }  
+    n++;
+    if (n < 100) 
+    {
+        mvc_timeout.attach(mvc,0.01f);
+    }
+    if (n == 100)
+    {
+        n = 0;
+        if (muscle == 1)
+        {
+            mvc_value_bl = xmvc_value;
+            CurrentSubstate = rest_biceps_right;
+            SubstateChanged = true;
+            led_red = 1;
+        }
+        if (muscle == 3)
+        {
+            mvc_value_br = xmvc_value;
+            CurrentSubstate = rest_biceps_leg;
+            SubstateChanged = true;
+            led_red = 1;
+        }
+        if (muscle == 5)
+        {
+            mvc_value_leg = xmvc_value;
+            CurrentState = vertical_movement;
+            StateChanged = true;
+            led_red = 1;
+        }
+        xmvc_value = 1e-11;
+        led_red = 1;
+    }
+}
+
+void WriteScope()
+{
+    //scope.set(0,theta_1);
+    // scope.set(1,CalculateError(theta_reference_1,theta_1));
+    // scope.set(2,theta_reference_1);
+    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(2, EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg));
+    scope.send();
+}
+
+void SubstateTransition()
+{
+    pressed_1 = false;
+    pressed_2 = false;
+    SubstateChanged = false;
 }
 
 void while_start()
@@ -139,13 +318,11 @@
 void while_motor_calibration()
 {
     // Do motor calibration stuff
-    if (demo)   // bool aanmaken voor demo (switch oid aanmaken)
-    {
+    if (pressed_1) { // bool aanmaken voor demo (switch oid aanmaken)
         CurrentState = demo_mode;
         StateChanged = true;
     }
-    if (emg)    // bool aanmaken voor EMG (switch oid aanmaken)
-    {
+    if (pressed_2) {  // bool aanmaken voor EMG (switch oid aanmaken)
         CurrentState = emg_calibration;
         StateChanged = true;
     }
@@ -154,12 +331,7 @@
 void while_demo_mode()
 {
     // Do demo mode stuff
-    if (!demo)  // bool aanmaken voor demo (switch oid)
-    {
-        emg = true;
-    }
-    if (emg)    // bool aanmaken voor EMG (switch oid aanmaken)
-    {
+    if (pressed_1 || pressed_2) {
         CurrentState = emg_calibration;
         StateChanged = true;
     }
@@ -168,18 +340,87 @@
 void while_emg_calibration()
 {
     // Do emg calibration stuff
-    if (!emg)   // bool aanmaken voor EMG (switch)
-    {
-        CurrentState = vertical_movement;
-        StateChanged = true;
+    switch (CurrentSubstate) {
+        case rest_biceps_left:
+            SubstateTransition();
+            muscle = 0;
+            led_green = 0;
+            if (pressed_1 || pressed_2) 
+            {
+                led_green = 1;
+                led_red = 0;
+                rest();
+            }
+            break;
+        case mvc_biceps_left:
+            SubstateTransition();
+            muscle = 1;
+            led_blue = 0;
+            if (pressed_1 || pressed_2) 
+            {
+                led_blue = 1;
+                led_red = 0;
+                mvc();
+            }
+            break;
+        case rest_biceps_right:
+            SubstateTransition();
+            muscle = 2;
+            led_red = 0;
+            led_green = 0;
+            if (pressed_1 || pressed_2) 
+            {
+                led_green = 1;
+                rest();
+            }
+            break;
+        case mvc_biceps_right:
+            SubstateTransition();
+            muscle = 3;
+            led_red = 0;
+            led_blue = 0;
+            if (pressed_1 || pressed_2) 
+            {
+                led_blue = 1;
+                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;
+                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;
+                mvc();
+            }
+            break;
+        default:
+            pc.printf("Unknown or unimplemented state reached!\n\r");
     }
 }
 
 void while_vertical_movement()
 {
     // Do vertical movement stuff
-    if (next)  // EMG gebaseerde threshold aanmaken
-    {
+    if (pressed_1 || pressed_2) { // EMG gebaseerde threshold aanmaken
         CurrentState = horizontal_movement;
         StateChanged = true;
     }
@@ -188,8 +429,7 @@
 void while_horizontal_movement()
 {
     // Do horizontal movement stuff
-    if (!next) // EMG gebaseerde threshold aanmaken
-    {
+    if (pressed_1 || pressed_2) { // EMG gebaseerde threshold aanmaken
         CurrentState = vertical_movement;
         StateChanged = true;
     }
@@ -197,30 +437,25 @@
 
 void StateTransition()
 {
-    if (StateChanged)
-    {
-        if (CurrentState == start)
-        {
+    pressed_1 = false;
+    pressed_2 = false;
+    if (StateChanged) {
+        if (CurrentState == start) {
             pc.printf("Initiating start.\n\r");
         }
-        if (CurrentState == motor_calibration)
-        {
+        if (CurrentState == motor_calibration) {
             pc.printf("Initiating motor_calibration.\n\r");
         }
-        if (CurrentState == demo_mode)
-        {
+        if (CurrentState == demo_mode) {
             pc.printf("Initiating demo_mode.\n\r");
         }
-        if (CurrentState == emg_calibration)
-        {
+        if (CurrentState == emg_calibration) {
             pc.printf("Initiating emg_calibration.\n\r");
         }
-        if (CurrentState == vertical_movement)
-        {
+        if (CurrentState == vertical_movement) {
             pc.printf("Initiating vertical_movement.\n\r");
         }
-        if (CurrentState == horizontal_movement)
-        {
+        if (CurrentState == horizontal_movement) {
             pc.printf("Initiating horizontal_movement.\n\r");
         }
         StateChanged = false;
@@ -229,8 +464,7 @@
 
 void StateMachine()
 {
-    switch(CurrentState)
-    {
+    switch(CurrentState) {
         case start:
             StateTransition();
             while_start();
@@ -261,7 +495,8 @@
 }
 
 // main
-int main(){
+int main()
+{
     pc.baud(115200);
     pc.printf("Hello World!\n\r");
     button_1.fall(go_next_1);