Project of Biorobotics
Dependencies: HIDScope MODSERIAL QEI mbed biquadFilter
Fork of TutorialPES by
main.cpp
- Committer:
- ThomBMT
- Date:
- 2018-10-31
- Revision:
- 10:05ad15c48388
- Parent:
- 9:355dd95199c3
- Child:
- 11:d525527c0b7d
File content as of revision 10:05ad15c48388:
#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; 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 { 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 == 0) { pc.printf("Relax your muscles please. \r\n"); i = 2; } else if (ii == 2250) { 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 == 5750) { 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 == 9250) { 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 == 12750) { 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) { Active_State = Homing_M2; } else if (counts1 > 0) { PwmPin1 = 0.8f; DirectionPin1 = false; } else { PwmPin1 = 0.8f; DirectionPin1 = true; } } void Going_Home_Motor2() { if (counts2 == 0) { Active_State = Post_Homing; } else if (counts2 > 0) { PwmPin2 = 0.8f; DirectionPin2 = true; } else { PwmPin2 = 0.8f; DirectionPin2 = false; } } 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); } } 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; /* Threshold_Bi_R = 0.75f * Flex_Bi_R; Threshold_Bi_L = 0.75f * Flex_Bi_L; Threshold_Tri_R = 0.75f * Flex_Tri_R; Threshold_Tri_L = 0.75f * Flex_Tri_L; */ if (Filtered_Bi_R >= Threshold_Bi_R) { led_R = 0; } if (Filtered_Bi_L >= Threshold_Bi_L) { led_B = 0; } if (Filtered_Tri_R >= Threshold_Tri_R) { led_G = 0; } if (Filtered_Tri_L >= Threshold_Tri_L) { led_B = 0; led_R = 0; } } void StateMachine() { switch (Active_State) { case Starting: Start_Up(); 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"); } else if (fabs(rad_m1)>(3.0f *pi) || fabs(rad_m2)>(3.0f *pi)) // pi/4 is a safe value, can/will be editted { pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n"); Active_State = Safe; } break; case Calibration: //calibration actions //pc.printf("Calibration State"); 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: //Homing actions //pc.printf("Homing State"); Going_Home_Motor1(); OFF_m2(); 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"); } else if (fabs(rad_m1)>(3.0f *pi) || fabs(rad_m2)>(3.0f *pi)) // 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 (counts1 == 0) { Active_State = Homing_M2; } Inverse(); sample(); EMG_Read(); Encoding(); break; case Homing_M2: Going_Home_Motor2(); OFF_m1(); if (counts2 == 0 && counts1 == 0) { Active_State = Post_Homing; } else if (counts2 == 0 && counts1 == !0); { // dit zou niet moeten kunnen maar er gebeuren wel meer rare dingen Active_State = Homing_M1; } 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 } break; case Function: //pc.printf("Funtioning State"); 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; } else if (fabs(rad_m1)>(3.0f *pi) || fabs(rad_m2)>(3.0f *pi)) // pi/4 is a safe value, can/will be editted { pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n"); Active_State = Safe; } EMG_test(); Filter(); Inverse(); sample(); EMG_Read(); Encoding(); velocity1(); velocity2(); motor1(); motor2(); PID_controller(); 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) { } }