Volledig besturingssysteem voor de myoelectrische prothese arm van mijn bachelor opdracht
Dependencies: mbed QEI MODSERIAL FastPWM biquadFilter
Diff: main.cpp
- Revision:
- 3:2aaf54ce090b
- Parent:
- 2:7c9974f0947a
- Child:
- 4:babe09a69296
--- a/main.cpp Wed Mar 27 17:33:56 2019 +0000 +++ b/main.cpp Thu Apr 04 15:37:37 2019 +0000 @@ -3,7 +3,7 @@ #include "MODSERIAL.h" #include "QEI.h" #include "BiQuad.h" - +#include "FastPWM.h" // Algemeen DigitalIn button3(SW3); DigitalIn button2(SW2); @@ -19,8 +19,8 @@ Timer t2; //Motoren DigitalOut direction1(D4); -PwmOut pwmpin1(D5); -PwmOut pwmpin2(D6); +FastPWM pwmpin1(D5); +FastPWM pwmpin2(D6); DigitalOut direction2(D7); volatile float pwm1; volatile float pwm2; @@ -33,6 +33,7 @@ double Pulses2; double motor_position2; double error1; +double u1; //Pot meter AnalogIn pot(A1); @@ -45,6 +46,15 @@ Ticker PotRead; Ticker Kdc; +// EMG +float EMG1; // Rotatie +float EMG2; // Elleboog +float EMG3; // Hand +float EMG4; // Reverse +float Input1; // Voor zonder EMG +float Input2; +int count = 0; + //Kinematica double stap1; double stap2; @@ -66,32 +76,32 @@ float Hoeknieuw2; //Limiet in graden -float lowerlim1 = 0; -float upperlim1 = 748.8; -float lowerlim2 = 0; -float upperlim2 = 1300; +float lowerlim1 = -900; +float upperlim1 = 900; +float lowerlim2 = -50; +float upperlim2 = 1500; // VARIABLES PID CONTROLLER -double Kp1 = 5; +double Kp1 = 12.5; double Ki1 = 0; double Kd1 = 1; -double Kp2 = 6; // Zonder arm: 6,0,1 +double Kp2 = 12; // Zonder arm: 6,0,1, met rotatie: 10, 0, 1 double Ki2 = 0; double Kd2 = 1; double Ts = 0.0005; // Sample time in seconds // Functies Kinematica -float Kinematics1(float KPot) +float Kinematics1(float EMG1) { - if (KPot > 0.45f) { - stap1 = KPot*150*Ts; + if (EMG1 > 0.45f) { + stap1 = EMG1*450*Ts; Hoeknieuw1 = PolsReference + stap1; return Hoeknieuw1; } - else if (KPot < -0.45f) { - stap1 = KPot*150*Ts; + else if (EMG1 < -0.45f) { + stap1 = EMG1*450*Ts; Hoeknieuw1 = PolsReference + stap1; return Hoeknieuw1; } @@ -100,17 +110,17 @@ return PolsReference; } } -float Kinematics2(float KPot) +float Kinematics2(float EMG2) { - if (KPot > 0.45f) { - stap2 = KPot*300*Ts; + if (EMG2 > 0.45f) { + stap2 = EMG2*300*Ts; Hoeknieuw2 = ElbowReference + stap2; return Hoeknieuw2; } - else if (KPot < -0.45f) { - stap2 = KPot*300*Ts; + else if (EMG2 < -0.45f) { + stap2 = EMG2*300*Ts; Hoeknieuw2 = ElbowReference + stap2; return Hoeknieuw2; } @@ -208,12 +218,12 @@ void moter1_control(double u1) { direction1= u1 > 0.0f; //positief = CW - if (fabs(u1)> 0.5f) { - u1 = 0.5f; + if (fabs(u1)> 0.6f) { + u1 = 0.6f; } else { u1= u1; } - pwmpin1= fabs(u1); //pwmduty cycle canonlybepositive, floatingpoint absolute value + pwmpin1.write(fabs(u1)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value } void moter2_control(double u2) @@ -224,20 +234,20 @@ } else { u2= u2; } - pwmpin2= fabs(u2); //pwmduty cycle canonlybepositive, floatingpoint absolute value + pwmpin2.write(fabs(u2)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value } void PwmMotor(void) { + float Polshoek1 = Kinematics1(Input1); + float Polshoek4 = Limits1(Polshoek1); + PolsReference = Polshoek4; + // Reference hoek berekenen, in graden - float Ellebooghoek1 = Kinematics2(pwm2); + float Ellebooghoek1 = Kinematics2(Input2); float Ellebooghoek4 = Limits2(Ellebooghoek1); ElbowReference = Ellebooghoek4; - float Polshoek1 = Kinematics1(pwm2); - float Polshoek4 = Limits1(Polshoek1); - PolsReference = Polshoek4; - // Positie motor berekenen, in graden Pulses1 = encoder1.getPulses(); motor_position1 = -(Pulses1/1200)*360; @@ -341,18 +351,19 @@ if (!But1) { led1 = 1; - float H1 = 0.98f; + float H1 = 0.7f; moter1_control(H1); wait(0.001f); } else if (!But2) { led1 = 1; - float H1 = -0.98f; + float H1 = -0.7f; moter1_control(H1); wait(0.001f); } - Pulses1 = 0; + encoder1.reset(); + motor_position1 = 0; pwmpin1 = 0; pwmpin2 = 0; ; @@ -360,7 +371,7 @@ stateChanged = false; } - // State transition logic: naar DEMO (button2), naar MOVEMENT(button3) + // State transition logic: naar HOMING (button2), na 5 min naar MOTORS_OFF if (!button3) { @@ -386,11 +397,11 @@ // Actions if (stateChanged) { - // state initialization: green + // state initialization: white t.start(); led1 = 0; - led2 = 1; - led3 = 1; + led2 = 0; + led3 = 0; if (!But1) { @@ -406,7 +417,8 @@ moter2_control(H2); wait(0.001f); } - Pulses2 = 0; + encoder2.reset(); + motor_position2 = 0; pwmpin1 = 0; pwmpin2 = 0; ; @@ -467,8 +479,21 @@ { // state initialization: purple t.start(); + pwmpin1 = 0; pwmpin2 = 0; + Input1 = pwm1; + Input2 = pwm2; + + // printen + if(count==500) + { + float tmp = t2.read(); + pc.printf(" Elleboog positie: %f reference: %f, rotatie positie: %f reference: \r\n", motor_position2, ElbowReference, motor_position1, PolsReference); + count = 0; + } + count++; + Pwm.attach (PwmMotor, Ts); led1 = 1; led2 = 0; @@ -541,6 +566,7 @@ led1 = 1; led2 =1; led3 =1; + /* if(counter==10) { float tmp = t2.read(); @@ -548,7 +574,7 @@ counter = 0; } counter++; - + */ ProcessStateMachine();