Thom Kuenen
Biorobotics 2018: Project group 16
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 1:ba14d8f4d444
- Parent:
- 0:94cf6b327972
- Child:
- 2:bc6043623fb7
--- a/main.cpp Thu Nov 01 10:06:58 2018 +0000
+++ b/main.cpp Thu Nov 01 10:08:23 2018 +0000
@@ -1,2 +1,927 @@
#include "mbed.h"
+#include "MODSERIAL.h"
+#include "HIDScope.h"
+#include "QEI.h"
+#include "BiQuad.h"
+MODSERIAL pc(USBTX, USBRX);
+DigitalOut DirectionPin1(D4);
+DigitalOut DirectionPin2(D7);
+PwmOut PwmPin1(D5);
+PwmOut PwmPin2(D6);
+DigitalIn Knop1(D2);
+DigitalIn Knop2(D3);
+DigitalIn Knop3(PTA4);
+DigitalIn Knop4(PTC6);
+AnalogIn pot1 (A5);
+AnalogIn pot2 (A4);
+AnalogIn emg0( A0 );
+AnalogIn emg1( A1 );
+AnalogIn emg2( A2 );
+AnalogIn emg3( A3 );
+DigitalOut led_G(LED_GREEN);
+DigitalOut led_R(LED_RED);
+DigitalOut led_B(LED_BLUE);
+
+QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
+QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
+
+BiQuadChain bqc1;
+BiQuadChain bqc2;
+BiQuadChain bqc3;
+BiQuadChain bqc4;
+BiQuadChain bqc5;
+BiQuadChain bqc6;
+BiQuadChain bqc7;
+BiQuadChain bqc8;
+
+BiQuad BqNotch1_1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
+BiQuad BqNotch2_1( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad BqNotch1_2( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
+BiQuad BqNotch2_2( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad BqNotch1_3( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
+BiQuad BqNotch2_3( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad BqNotch1_4( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
+BiQuad BqNotch2_4( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad BqHP1( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+BiQuad BqHP2( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+BiQuad BqHP3( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+BiQuad BqHP4( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+
+BiQuad BqLP1( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+BiQuad BqLP2( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+BiQuad BqLP3( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+BiQuad BqLP4( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+
+BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+
+Ticker StateTicker;
+Ticker printTicker;
+
+HIDScope scope( 4 );
+
+volatile float Bicep_Right = 0.0;
+volatile float Bicep_Left = 0.0;
+volatile float Tricep_Right = 0.0;
+volatile float Tricep_Left = 0.0;
+volatile float FilterHP_Bi_R;
+volatile float Filterabs_Bi_R;
+volatile float Filtered_Bi_R;
+volatile float FilterHP_Bi_L;
+volatile float Filterabs_Bi_L;
+volatile float Filtered_Bi_L;
+volatile float FilterHP_Tri_R;
+volatile float Filterabs_Tri_R;
+volatile float Filtered_Tri_R;
+volatile float FilterHP_Tri_L;
+volatile float Filterabs_Tri_L;
+volatile float Filtered_Tri_L;
+
+volatile float error_1_integral = 0;
+volatile float error_2_integral = 0;
+volatile float error_1_prev; // initialization with this value only done once!
+volatile float error_2_prev;
+
+volatile const float pi = 3.1415926;
+volatile const float rad_count = 0.0007479; // 2pi/8400;
+volatile const float maxVelocity = 2.0f * pi; // in rad/s
+volatile const float r_3 = 0.035;
+
+volatile float referenceVelocity1 = 0.5; // dit is de gecentreerde waarde en dus de nulstand
+volatile float referenceVelocity2 = 0.5;
+
+volatile int i = 0; // Led blink status
+volatile int ii = 0; // Calibratie timer
+volatile int iii = 0; // Start up timer
+
+volatile float Kp = 17.5;
+volatile float Ki = 1.02;
+volatile float Kd = 23.2;
+volatile float Ts = 0.01; // Sample time in seconds
+volatile float error_1;
+volatile float error_2;
+volatile float U_1;
+volatile float U_2;
+
+volatile float q_1;
+volatile float q_2;
+volatile float r_1;
+volatile float r_2;
+volatile float w_1;
+volatile float w_2;
+
+volatile float Flex_Bi_R;
+volatile float Flex_Bi_L;
+volatile float Flex_Tri_R;
+volatile float Flex_Tri_L;
+volatile float Threshold_Value;
+volatile float Threshold_Bi_R;
+volatile float Threshold_Bi_L;
+volatile float Threshold_Tri_R;
+volatile float Threshold_Tri_L;
+volatile bool Checking_Bi_R = false;
+volatile bool Checking_Bi_L = false;
+volatile bool Checking_Tri_R = false;
+volatile bool Checking_Tri_L = false;
+
+enum states{Starting, Calibration, Homing_M1, Homing_M2, Post_Homing, Function, Safe};
+
+volatile states Active_State = Starting;
+
+volatile float vx;
+volatile float vy;
+volatile int counts1;
+volatile int counts2;
+volatile float rad_m1;
+volatile float rad_m2;
+
+void Encoding()
+{
+
+ counts1 = Encoder1.getPulses();
+ counts2 = Encoder2.getPulses();
+
+ rad_m1 = rad_count * (float)counts1;
+ rad_m2 = rad_count * (float)counts2;
+}
+
+void Filter()
+{
+ FilterHP_Bi_R = bqc1.step( emg0.read() );
+ Filterabs_Bi_R = fabs(FilterHP_Bi_R);
+ Filtered_Bi_R = bqc2.step( Filterabs_Bi_R );
+
+ FilterHP_Bi_L = bqc3.step( emg1.read() );
+ Filterabs_Bi_L = fabs(FilterHP_Bi_L);
+ Filtered_Bi_L = bqc4.step( Filterabs_Bi_L );
+
+ FilterHP_Tri_R = bqc5.step( emg2.read() );
+ Filterabs_Tri_R = fabs(FilterHP_Tri_R);
+ Filtered_Tri_R = bqc6.step( Filterabs_Tri_R );
+
+ FilterHP_Tri_L = bqc7.step( emg3.read() );
+ Filterabs_Tri_L = fabs(FilterHP_Tri_L);
+ Filtered_Tri_L = bqc8.step( Filterabs_Tri_L );
+}
+
+void BlinkLed()
+{
+ if(i==1)
+ {
+ led_G=led_B=1;
+ static int rr = 0;
+ rr++;
+ if (rr == 1)
+ {
+ led_R = !led_R;
+ }
+ else if (rr == 50)
+ {
+ rr = 0;
+ }
+ }
+ else if(i==2)
+ {
+ led_R=led_B=1;
+
+ static int gg = 0;
+ gg++;
+ if (gg == 1)
+ {
+ led_G = !led_G;
+ }
+ else if (gg == 250)
+ {
+ gg = 0;
+ }
+ }
+ else if (i==3)
+ {
+ led_R=1;
+ static int bb = 0;
+ bb++;
+ if (bb == 1)
+ {
+ led_B = !led_B;
+ }
+ else if (bb == 500)
+ {
+ bb = 0;
+ }
+ }
+ else if (i==4)
+ {
+ led_G=1;
+ static int rr = 0;
+ rr++;
+ if (rr == 1)
+ {
+ led_R = !led_R;
+ led_B = !led_B;
+ }
+ else if (rr == 250)
+ {
+ rr = 0;
+ }
+ }
+ else
+ {
+ led_R=led_G=led_B=1;
+ }
+}
+
+void EMG_Read()
+{
+ Bicep_Right = emg0.read();
+ Bicep_Left = emg1.read();
+ Tricep_Right = emg2.read();
+ Tricep_Left = emg3.read();
+}
+
+void sample()
+{
+
+ scope.set(0, Filtered_Bi_R*10.0f );
+ scope.set(1, Filtered_Bi_L*10.0f );
+ scope.set(2, Filtered_Tri_R*10.0f );
+ scope.set(3, Filtered_Tri_L*10.0f );
+
+ scope.send();
+}
+
+void Inverse()
+{
+ q_1= rad_m1+(pi/6.0f); // uit Encoder
+ q_2= rad_m2+(pi/6.0f); // uit Encoder
+ r_1= -0.2f;
+ r_2= -0.2f;
+
+ float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2);
+ float z = 2.0f*(r_2*cos(q_1)*cos(q_2))-r_3;
+ float y = r_2*cos(q_1)*cos(q_2)-r_2*sin(q_1)*sin(q_2)+2.0f*(r_1*cos(q_1))-r_3;
+ float x = (-2.0f)*r_2*sin(q_1)*cos(q_2);
+ float D = 1.0f/(u*z-x*y); // Determinant
+ //printf("Determinant is %f\r\n", D);
+
+ float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
+ float b = -D*x; // Inverse jacobian
+ float c = -D*y; // Inverse jacobian
+ float d = D*u; // Inverse jacobian
+
+ vx = pot1.read()/5.0f; // uit emg data
+ vy = pot2.read()/5.0f; // uit emg data
+
+ w_1 = vx*a+vy*b;
+ w_2 = vx*c+vy*d;
+
+ /*
+ printf("%f\r\n", vx);
+ printf("%f\r\n", vy);
+ */
+}
+
+void PID_controller()
+{
+ error_1 = (w_1*0.002f) - rad_m1;
+ error_2 = (w_2*0.002f) - rad_m2;
+
+ error_1_prev = error_1;
+ error_2_prev = error_2;
+
+ // Proportional part:
+ float u_k_1 = Kp * error_1;
+ float u_k_2 = Kp * error_2;
+
+ // Integral part
+ error_1_integral = error_1_integral + error_1 * Ts;
+ error_2_integral = error_2_integral + error_2 * Ts;
+ float u_i_1 = Ki * error_1_integral;
+ float u_i_2 = Ki * error_2_integral;
+
+ // Derivative part
+ float error_1_derivative = (error_1 - error_1_prev)/Ts;
+ float error_2_derivative = (error_2 - error_2_prev)/Ts;
+ float filtered_error_1_derivative = LowPassFilter.step(error_1_derivative);
+ float filtered_error_2_derivative = LowPassFilter.step(error_2_derivative);
+ float u_d_1 = Kd * filtered_error_1_derivative;
+ float u_d_2 = Kd * filtered_error_2_derivative;
+ error_1_prev = error_1;
+ error_2_prev = error_2;
+
+ // Sum all parts and return it
+ U_1 = u_k_1 + u_i_1 + u_d_1;
+ U_2 = u_k_2 + u_i_2 + u_d_2;
+}
+
+void velocity1()
+{
+ if (pot1.read()>0.5f)
+ {
+ // Clockwise rotation
+ referenceVelocity1 = (pot1.read()-0.5f) * 2.0f;
+ }
+
+ else if (pot1.read() == 0.5f)
+ {
+ referenceVelocity1 = pot1.read() * 0.0f;
+ }
+
+ else if (pot1.read() < 0.5f)
+ {
+ // Counterclockwise rotation
+ referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ;
+ }
+}
+
+void velocity2()
+{
+ if (pot2.read()>0.5f)
+ {
+ // Clockwise rotation
+ referenceVelocity2 = (pot2.read()-0.5f) * 2.0f;
+ }
+
+ else if (pot2.read() == 0.5f)
+ {
+ referenceVelocity2 = pot2.read() * 0.0f;
+ }
+
+ else if (pot2.read() < 0.5f)
+ {
+ // Counterclockwise rotation
+ referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
+ }
+}
+
+void motor1()
+{
+ float u_v1 = referenceVelocity1 ; //w_1
+ float u = u_v1;// (2.0f * pi);
+ DirectionPin1 = u < 0.0f;
+ PwmPin1 = fabs(u);
+}
+
+void motor2()
+{
+ float u_v2 = referenceVelocity2 ; //w_2
+ float u = u_v2;// (2.0f * pi);
+ DirectionPin2 = u > 0.0f;
+ PwmPin2 = fabs(u);
+}
+
+void Calibrating()
+{
+ static float n = 0.0;
+ static float m = 0.0;
+ static float l = 0.0;
+ static float k = 0.0;
+
+ //static int ii;
+ ii++;
+
+ if (ii<=2500)
+ {
+ if (ii == 1)
+ {
+ pc.printf("Relax your muscles please. \r\n");
+ i = 2;
+ }
+ else if (ii == 1750)
+ {
+ pc.printf("Flex your right bicep now please.\r\n");
+ i = 3;
+ }
+ //chillen
+ }
+ else if (ii>2500 && ii<5000) //
+ {
+ n = n + Filtered_Bi_R; // dit wordt de variable naam na het filter
+ i = 1;
+ }
+ else if (ii == 5000)
+ {
+ Flex_Bi_R = n / 2500.0f;
+ pc.printf("You can relax your right bicep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Bi_R);
+ i = 2;
+ }
+ else if (ii>5000 && ii<=6000)
+ {
+ if (ii == 5500)
+ {
+ pc.printf("Flex your left bicep now please. \r\n");
+ i = 3;
+ }
+ //chillen
+ }
+ else if(ii>6000 && ii<8500)
+ {
+ m = m + Filtered_Bi_L;
+ i = 1;
+ }
+ else if (ii == 8500)
+ {
+ Flex_Bi_L = m / 2500.0f;
+ pc.printf("You can relax your left bicep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Bi_L);
+ i = 2;
+ }
+ else if (ii>8500 && ii<=9500)
+ {
+ if (ii == 9000)
+ {
+ pc.printf("Flex your right tricep now please. \r\n");
+ i = 3;
+ }
+ //chillen
+ }
+ else if (ii>9500 && ii<12000)
+ {
+ l = l + Filtered_Tri_R;
+ i = 1;
+ }
+ else if (ii == 12000)
+ {
+ Flex_Tri_R = l / 2500.0f;
+ pc.printf("You can relax your right tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Tri_R);
+ i = 2;
+ }
+ else if (ii>12000 && ii <=13000)
+ {
+ if (ii == 12500)
+ {
+ pc.printf("Flex your left tricep now please. \r\n");
+ i = 3;
+ }
+ //chillen
+ }
+ else if (ii>13000 && ii<15500)
+ {
+ k = k + Filtered_Tri_L;
+ i = 1;
+ }
+ else if (ii == 15500)
+ {
+ Flex_Tri_L = k / 2500.0f;
+ pc.printf("You can relax your left tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\nThe calibration has been completed, the system is now operatable. \r\n",Flex_Tri_L);
+ i = 2;
+ }
+
+Threshold_Value = 0.8f;
+
+Threshold_Bi_R = Threshold_Value * Flex_Bi_R;
+Threshold_Bi_L = Threshold_Value * Flex_Bi_L;
+Threshold_Tri_R = Threshold_Value * Flex_Tri_R;
+Threshold_Tri_L = Threshold_Value * Flex_Tri_L;
+
+ if (ii == 16500)
+ {
+ pc.printf("\r\nThreshold value right bicep = %f\r\nThreshold value left bicep = %f\r\nThreshold value right tricep = %f\r\nThreshold value left tricep = %f\r\n\r\n",Threshold_Bi_R,Threshold_Bi_L,Threshold_Tri_R,Threshold_Tri_L);
+ }
+ else if (ii == 20000)
+ {
+ pc.printf("\r\nAutomatic switch to Homing State\r\n");
+ Active_State = Homing_M1;
+ i = 0;
+ }
+}
+
+
+
+void Start_Up()
+{
+ i++;
+ iii++;
+ if (iii == 1)
+ {
+ pc.printf("\r\n\r\nSystem is starting...\r\nWaiting for further input...\r\n");
+ }
+
+ else if (iii == 30000)
+ {
+ pc.printf("1 minute without input..\r\nReseting start-up...\r\n");
+ iii = 0;
+ }
+ else if (iii == 40001) // sleeping state is only added for designing purposes and will most likely never be used
+ { // when working with patients. Furthermore it cannot be reached automaticly
+ pc.printf("Sleeping... Press button 4 to wake me up!\r\n\r\n");
+ iii++;
+ }
+ else if (iii == 45000)
+ {
+ iii = 40000;
+ }
+}
+
+void OFF_m1()
+{
+ PwmPin1 = 0;
+}
+void OFF_m2()
+{
+ PwmPin2 = 0;
+}
+
+void Going_Home_Motor1()
+{
+ if (counts1 == 0)
+ {
+ pc.printf("Switching to Homing state for motor 2\r\n");
+ Active_State = Homing_M2; // Statement here instead of statemachine because of checking speed
+ }
+ else if (counts1 > 0)
+ {
+ PwmPin1 = 0.8f;
+ DirectionPin1 = false;
+ }
+ else
+ {
+ PwmPin1 = 0.8f;
+ DirectionPin1 = true;
+ }
+}
+
+void Going_Home_Motor2()
+{
+ pc.printf("Entered homing state 2");
+ if (counts2 == 0)
+ {
+ // als het goed is hoeft hier niets te staan // Statement in statemachine because of the double check
+ }
+ else if (counts2 > 0)
+ {
+ PwmPin2 = 0.8f;
+ DirectionPin2 = true;
+ }
+ else
+ {
+ PwmPin2 = 0.8f;
+ DirectionPin2 = false;
+ }
+}
+
+void Checking_EMG()
+{
+ // This function will make the the setting of signal to movement easier
+
+ if (Filtered_Bi_R >= Threshold_Bi_R)
+ {
+ Checking_Bi_R = true;
+ }
+ if (Filtered_Bi_L >= Threshold_Bi_L)
+ {
+ Checking_Bi_L = true;
+ }
+ if (Filtered_Tri_R >= Threshold_Tri_R)
+ {
+ Checking_Tri_R = true;
+ }
+ if (Filtered_Tri_L >= Threshold_Tri_L)
+ {
+ Checking_Tri_L = true;
+ }
+}
+
+void Setting_Movement()
+{
+ // Here we will set the emg values to the movement of the arm
+
+ if (Checking_Bi_R && Checking_Bi_L) // sloping y = x, y > 0
+ {
+ vx = 0.5f;
+ vy = 0.5f;
+ }
+ else if (Checking_Bi_R && Checking_Tri_L) // sloping y = -x, y > 0
+ {
+ vx = -0.5f;
+ vy = 0.5f;
+ }
+ else if (Checking_Bi_L && Checking_Tri_R) // sloping y = -x, y < 0
+ {
+ vx = 0.5f;
+ vy = -0.5f;
+ }
+ else if (Checking_Tri_R && Checking_Tri_L) // sloping y = x, y < 0
+ {
+ vx = -0.5f;
+ vy = -0.5f;
+ }
+ else if (Checking_Bi_R) // y > 0
+ {
+ vx = 0.0f;
+ vy = 0.5f;
+ }
+ else if (Checking_Bi_L) // x > 0
+ {
+ vx = 0.5f;
+ vy = 0.0f;
+ }
+ else if (Checking_Tri_R) // y < 0
+ {
+ vx = 0.0f;
+ vy = -0.5f;
+ }
+ else if (Checking_Tri_L) // y < 0
+ {
+ vx = -0.5f;
+ vy = 0.0f;
+ }
+}
+
+void Printing()
+{
+ float v1 = PwmPin1 * maxVelocity;
+ float v2 = PwmPin2 * maxVelocity;
+
+ if (Active_State == Function || Active_State == Homing_M1)
+ {
+ pc.printf("q1 = %f [rad] \r\nq2 = %f [rad] \r\ncount1= %i\r\ncount2= %i\r\nq1dot = %f [rad/s] \r\nq2dot = %f [rad/s] \r\n\r\n\r\n\r\n\r\n", rad_m1, rad_m2,counts1, counts2, v1, v2);
+ }
+
+ pc.printf("%i \r\n%i \r\n\r\n",counts1,counts2);
+}
+
+void Test()
+{/*
+ if (fabs(rad_m1) == 0.01f)
+ {
+ PwmPin1 = 0.0f;
+ }
+ else if (rad_m1 < 0.01f)
+ {
+ DirectionPin1 = true;
+ PwmPin1=0.5f;
+ }
+ else if (rad_m1 > -0.01f)
+ {
+ DirectionPin1 = false;
+ PwmPin1=0.5f;
+ }
+ if (fabs(rad_m2) == 0.01f)
+ {
+ PwmPin2 = 0.0f;
+ }
+ else if (rad_m2 < 0.01f)
+ {
+ DirectionPin2 = false;
+ }*/
+
+
+
+ if (counts1 == 0)
+ {
+ PwmPin1 = 0.0f;
+ }
+ else if (counts1 > 0)
+ {
+ DirectionPin1 = true;
+ PwmPin1 = 0.4f * ((float)counts1/1000.0f);
+ }
+ else if (counts1 < 0)
+ {
+ DirectionPin1 = false;
+ PwmPin1 = 0.4f * ((float)counts1/1000.0f);
+ }
+
+ if (counts2 == 0)
+ {
+ PwmPin2 = 0.0f;
+ }
+ if (counts2 < 0)
+ {
+ DirectionPin2 = false;
+ PwmPin2 = 0.4f * ((float)counts2/1000.0f);
+ }
+ else if (counts2 > 0)
+ {
+ DirectionPin2 = true;
+ PwmPin2 = 0.4f * ((float)counts2/1000.0f);
+ }
+
+}
+
+void EMG_test()
+{
+ led_G=led_R=led_B=1;
+
+ if (Filtered_Bi_R >= Threshold_Bi_R)
+ {
+ i = 1;
+ //led_R = 0; // rood
+ }
+ if (Filtered_Bi_L >= Threshold_Bi_L)
+ {
+ i = 3;
+ //led_B = 0; // blauw
+ }
+ if (Filtered_Tri_R >= Threshold_Tri_R)
+ {
+ i = 2;
+ //led_G = 0; // groen
+ }
+ if (Filtered_Tri_L >= Threshold_Tri_L)
+ {
+ i = 4;
+ //led_B = 0;
+ //led_R = 0; // paars
+ }
+
+}
+
+void StateMachine()
+{
+ switch (Active_State)
+ {
+ case Starting:
+ Start_Up();
+ OFF_m1();
+ OFF_m2();
+
+ if (!Knop4 == true)
+ {
+ Active_State = Calibration;
+ pc.printf("Entering Calibration State \r\n");
+ }
+ else if (!Knop3 == true)
+ {
+ Active_State = Homing_M1;
+ pc.printf("Entering Homing State \r\n");
+ }
+
+
+ break;
+
+ case Calibration:
+
+ Filter();
+ Calibrating();
+ OFF_m1();
+ OFF_m2();
+ BlinkLed();
+
+ if (!Knop1 && !Knop2)
+ {
+ pc.printf("Switched to Sleeping State\r\n");
+ Active_State = Starting;
+ iii = 40001;
+ }
+ else if (Knop1==false)
+ {
+ pc.printf("Manual switch to Homing state \r\n");
+ Active_State = Homing_M1;
+ }
+
+
+ Inverse();
+ sample();
+ EMG_Read();
+ Encoding();
+ break;
+
+ case Homing_M1:
+
+ Going_Home_Motor1();
+ OFF_m2();
+
+ if (fabs(rad_m1)>(pi/6.0f) || fabs(rad_m2)>(pi/6.0f)) // pi/4 is a safe value, can/will be editted
+ {
+ pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
+ Active_State = Safe;
+ }
+ else if (!Knop1 && !Knop2)
+ {
+ pc.printf("Switched to Sleeping State\r\n");
+ Active_State = Starting;
+ iii = 40000;
+ }
+ else if (Knop2==false)
+ {
+ pc.printf("Manual switch to Funtioning State \r\n");
+ Active_State = Function;
+ }
+ else if (Knop4==false)
+ {
+ Active_State = Calibration;
+ pc.printf("Re-entering Calibration State \r\n");
+ }
+
+
+ Inverse();
+ sample();
+ EMG_Read();
+ Encoding();
+ break;
+
+ case Homing_M2:
+
+ Going_Home_Motor2();
+ OFF_m1();
+
+ if (fabs(rad_m1)>(pi/6.0f) || fabs(rad_m2)>(pi/6.0f)) // pi/4 is a safe value, can/will be editted
+ {
+ pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
+ Active_State = Safe;
+ }
+ else if (counts2 == 0 && counts1 == 0)
+ {
+ Active_State = Post_Homing;
+ }
+
+
+ Inverse();
+ sample();
+ EMG_Read();
+ Encoding();
+
+ break;
+
+ case Post_Homing:
+
+ static int mm = 0;
+ mm++;
+ if (mm == 1000);
+ {
+ Active_State = Function;
+ pc.printf("Homing was succesfull\r\n\r\nAutomatic switch to Funtioning state\r\n\r\n");
+ mm=0; // reseting the state
+ }
+
+ OFF_m1();
+ OFF_m2();
+ break;
+
+ case Function:
+
+ if (fabs(rad_m1)>(pi/6.0f) || fabs(rad_m2)>(pi/6.0f)) // pi/4 is a safe value, can/will be editted
+ {
+ pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
+ Active_State = Safe;
+ }
+ else if (Knop4==false)
+ {
+ pc.printf("Re-entering Calibration State \r\n");
+ Active_State = Calibration;
+ ii=0;
+ }
+ else if (Knop3==false)
+ {
+ pc.printf("Re-entering Homing State \r\n");
+ Active_State = Homing_M1;
+ }
+ else if (!Knop1 && !Knop2)
+ {
+ pc.printf("Switched to Sleeping State\r\n");
+ Active_State = Starting;
+ iii = 40000;
+ }
+
+
+ EMG_test();
+ Filter();
+ Inverse();
+ sample();
+ EMG_Read();
+ Encoding();
+ velocity1();
+ velocity2();
+ Checking_EMG();
+ Setting_Movement();
+ PID_controller();
+ motor1();
+ motor2();
+
+
+ break;
+
+ case Safe:
+
+ OFF_m1();
+ OFF_m2();
+
+ break;
+
+ default:
+ pc.printf("UNKNOWN COMMAND");
+ }
+}
+
+int main()
+{
+ pc.baud(115200);
+ PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz
+
+ bqc1.add( &BqNotch1_1 ).add( &BqNotch2_1 ).add( &BqHP1 ); //Oh wat lelijk...
+ bqc2.add(&BqLP1);
+ bqc3.add( &BqNotch1_2 ).add( &BqNotch2_2 ).add( &BqHP2 );
+ bqc4.add(&BqLP2);
+ bqc5.add( &BqNotch1_3 ).add( &BqNotch2_3 ).add( &BqHP3 );
+ bqc6.add(&BqLP3);
+ bqc7.add( &BqNotch1_4 ).add( &BqNotch2_4 ).add( &BqHP4 );
+ bqc8.add(&BqLP4);
+
+ StateTicker.attach(&StateMachine, 0.002);
+
+ printTicker.attach(&Printing, 2);
+
+ while(true)
+ {
+ }
+}
\ No newline at end of file