![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Project of Biorobotics
Dependencies: HIDScope MODSERIAL QEI mbed biquadFilter
Fork of TutorialPES by
Diff: main.cpp
- Revision:
- 8:e8734a254818
- Parent:
- 7:439940ae1197
- Child:
- 9:355dd95199c3
diff -r 439940ae1197 -r e8734a254818 main.cpp --- a/main.cpp Fri Oct 26 07:58:07 2018 +0000 +++ b/main.cpp Mon Oct 29 14:02:21 2018 +0000 @@ -10,18 +10,18 @@ PwmOut PwmPin2(D6); DigitalIn Knop1(D2); DigitalIn Knop2(D3); +DigitalIn Knop3(PTA4); AnalogIn pot1 (A5); AnalogIn pot2 (A4); AnalogIn emg0( A0 ); AnalogIn emg1( A1 ); AnalogIn emg2( A2 ); AnalogIn emg3( A3 ); +DigitalOut led(LED_GREEN); -QEI Encoder1(D12,D13,NC,64); +QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING); QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); -//DigitalOut LED(LED_RED); - Ticker StateTicker; Ticker printTicker; @@ -31,15 +31,31 @@ volatile float Bicep_Left = 0.0; volatile float Tricep_Right = 0.0; volatile float Tricep_Left = 0.0; + volatile const float maxVelocity = 8.4; // in rad/s volatile const float pi = 3.1415926; volatile const float rad_count = 0.0007479; // 2pi/8400; + volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand volatile float referenceVelocity2 = 0.5; -enum states{Calibration, Homing, Function}; +volatile float q_1; +volatile float q_2; +volatile float r_1; +volatile float r_2; +volatile const float r_3 = 0.035; +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; + + +enum states{Starting, Calibration, Homing, Function}; -volatile states Active_State = Calibration; +volatile states Active_State = Starting; volatile int counts1 ; volatile int counts2 ; @@ -75,6 +91,36 @@ scope.send(); } +void Inverse() +{ + q_1= rad_m1; // uit Encoder + q_2= rad_m2+(pi/2.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\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 + + float vx = 0.01f; // uit emg data + float vy = 0.0f; // uit emg data + + w_1 = vx*a+vy*b; + w_2 = vx*c+vy*d; + + /* + printf("%f\n", w_1); + printf("%f\n", w_2); + */ +} void velocity1() { @@ -117,37 +163,119 @@ } void motor1() - { - float u = referenceVelocity1; + { + float u_v1 = w_1; //referenceVelocity1 + float u = u_v1/ (2.0f * pi); DirectionPin1 = u < 0.0f; PwmPin1 = fabs(u); } void motor2() { - float u = referenceVelocity2; + float u_v2 = w_2; //referenceVelocity2 + 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; + + for(int ii=0; ii<=20000; ii++) + { + if (ii <2500) + { + n = n + emg0.read(); + } + else if (ii == 2500) + { + Flex_Bi_R = n / 2500.0f; + } + else if (ii>2500 && ii<=3500) + { + //chillen + } + else if(ii>3500 && ii<6000) + { + m = m + emg1.read(); + } + else if (ii == 6000) + { + Flex_Bi_L = m / 2500.0f; + } + else if (ii>6000 && ii<=7000) + { + //chillen + } + else if (ii>7000 && ii<9500) + { + l = l + emg2.read(); + } + else if (ii == 9500) + { + Flex_Tri_R = l / 2500.0f; + } + else if (ii>9500 && ii <=10500) + { + //chillen + } + else if (ii>10500 && ii<13000) + { + k = k + emg3.read(); + } + else if (ii == 13000) + { + Flex_Tri_L = k / 2500.0f; + } + } +} + +void OFF() +{ + PwmPin1 = PwmPin2 = 0; +} void Printing() { - float v1 = fabs(referenceVelocity1) * maxVelocity; - float v2 = fabs(referenceVelocity2) * maxVelocity; - - //eventueel nog counts -> rad/s - - //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2); - pc.printf("%f %f \n",rad_m1,rad_m2); + float v1 = PwmPin1 * maxVelocity; + float v2 = PwmPin2 * maxVelocity; + + pc.printf("q1 = %f [rad] \n q2 = %f [rad] \n q1dot = %f [rad/s] \n q2dot = %f [rad/s] \n\n\n\n\n", rad_m1, rad_m2, v1, v2); } void StateMachine() { switch (Active_State) { + case Starting: + OFF(); + for (int i = 0; i<=20; i++) + { + led = !led; + wait(0.05); + + if (i == 0) + { + pc.printf("Starting up..\n"); + } + else if (i == 20) + { + Active_State = Calibration; + pc.printf("Entering Calibration state \n"); + } + } + break; + case Calibration: //calibration actions //pc.printf("Calibration State"); + + Calibrating(); + OFF(); if (Knop1==false) { pc.printf("Entering Homing state \n"); @@ -157,7 +285,6 @@ sample(); EMG_Read(); Encoding(); - break; case Homing: @@ -176,27 +303,20 @@ case Function: //pc.printf("Funtioning State"); - /* - if (Knop2==false) - { - pc.printf("Re-entering Homing State \n"); - Active_State = Homing; - } - else if (Knop1==false) - { + + if (Knop3==false) + { pc.printf("Re-entering Calibration State \n"); Active_State = Calibration; } - */ - + sample(); EMG_Read(); Encoding(); velocity1(); velocity2(); motor1(); - motor2(); - + motor2(); break; default: @@ -209,9 +329,9 @@ pc.baud(115200); PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz - StateTicker.attach(StateMachine, 0.002); + StateTicker.attach(&StateMachine, 0.002); - printTicker.attach(&Printing, 2.0); + printTicker.attach(&Printing, 4.0); while(true) {