![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Volledig besturingssysteem voor de myoelectrische prothese arm van mijn bachelor opdracht
Dependencies: mbed QEI MODSERIAL FastPWM biquadFilter
Diff: main.cpp
- Revision:
- 6:464d2fdfd7de
- Parent:
- 5:ef77da99d0d1
- Child:
- 7:d7aafc5b9efc
- Child:
- 8:7fd9ac522ea9
--- a/main.cpp Fri Apr 05 13:11:44 2019 +0000 +++ b/main.cpp Thu Apr 11 12:32:20 2019 +0000 @@ -6,31 +6,31 @@ #include "FastPWM.h" // Algemeen -DigitalIn button3(SW3); -DigitalIn button2(SW2); +DigitalIn button3(SW3); +DigitalIn button2(SW2); AnalogIn But2(A5); AnalogIn But1(A3); DigitalOut led1(LED_GREEN); DigitalOut led2(LED_RED); DigitalOut led3(LED_BLUE); +float counts = 0; MODSERIAL pc(USBTX, USBRX); Timer t; Timer t2; + //Motoren DigitalOut direction1(D4); -FastPWM pwmpin1(D5); +FastPWM pwmpin1(D5); FastPWM pwmpin2(D6); -//PwmOut pwmpin1(D5); -//PwmOut pwmpin2(D6); DigitalOut direction2(D7); volatile float pwm1; volatile float pwm2; //Encoder -QEI encoder1 (D10, D9, NC, 1200, QEI::X4_ENCODING); -QEI encoder2 (D13, D12, NC, 4800, QEI::X4_ENCODING); +QEI encoder1 (D15, D14, NC, 1200, QEI::X4_ENCODING); +QEI encoder2 (D1, D0, NC, 4800, QEI::X4_ENCODING); double Pulses1; double motor_position1; double Pulses2; @@ -50,11 +50,24 @@ Ticker Kdc; //Servo -FastPWM myservo1(D3); Ticker ServoTick; -float servo_position; -float Periodlength = 0.00006; -int counts = 0; +DigitalOut myservo1(D8); //Duim +DigitalOut myservo2(D9); //Pink tot Middel vinger +DigitalOut myservo3(D10); //wijsvinger + +float Periodlength = 0.02; // de MG996R heeft een PWM periode van 20 ms +float servo_position1; // in percentage van 0 tot 1, 0 is met de klok mee, 1 is tegen de klok in. +float servo_position2; +float servo_position3; + +// Vinger posities +float Duim_krom = 0.05; +float Duim_recht = 0.85; +float MWP_krom = 0.06; +float MWP_recht = 0.89; +float Wijsvinger_krom = 0.15; +float Wijsvinger_recht = 0.93; + // EMG float EMG1; // Rotatie @@ -88,16 +101,16 @@ //Limiet in graden float lowerlim1 = -900; float upperlim1 = 900; -float lowerlim2 = -50; -float upperlim2 = 1500; +float lowerlim2 = 0; +float upperlim2 = 1500; // VARIABLES PID CONTROLLER -double Kp1 = 12.5; -double Ki1 = 0; +double Kp1 = 12.5; +double Ki1 = 0; double Kd1 = 1; double Kp2 = 12; // Zonder arm: 6,0,1, met rotatie: 10, 0, 1 -double Ki2 = 0; -double Kd2 = 1; +double Ki2 = 0; +double Kd2 = 1; double Ts = 0.0005; // Sample time in seconds // Functies Kinematica @@ -124,7 +137,7 @@ { if (EMG2 > 0.45f) { - stap2 = EMG2*300*Ts; + stap2 = EMG2*300*Ts; Hoeknieuw2 = ElbowReference + stap2; return Hoeknieuw2; } @@ -184,7 +197,7 @@ static double error1_integral = 0; static double error1_prev = error1; // initialization with this value only done once! static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth); - + // Proportional part: double u_k1 = Kp1 * error1; @@ -206,7 +219,7 @@ static double error2_integral = 0; static double error2_prev = error2; // initialization with this value only done once! static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth); - + // Proportional part: double u_k2 = Kp2 * error2; @@ -228,8 +241,8 @@ void moter1_control(double u1) { direction1= u1 > 0.0f; //positief = CW - if (fabs(u1)> 0.6f) { - u1 = 0.6f; + if (fabs(u1)> 0.7f) { + u1 = 0.7f; } else { u1= u1; } @@ -256,16 +269,16 @@ pwm1 =(Pot1*2)-1; Input1 = pwm1; Input2 = pwm2; - + float Polshoek1 = Kinematics1(Input1); float Polshoek4 = Limits1(Polshoek1); PolsReference = Polshoek4; - + // Reference hoek berekenen, in graden float Ellebooghoek1 = Kinematics2(Input2); float Ellebooghoek4 = Limits2(Ellebooghoek1); ElbowReference = Ellebooghoek4; - + // Positie motor berekenen, in graden Pulses1 = encoder1.getPulses(); motor_position1 = -(Pulses1/1200)*360; @@ -275,7 +288,7 @@ double error1 = PolsReference - motor_position1; double u1 = PID_controller1(error1); moter1_control(u1); - + double error2 = ElbowReference - motor_position2; double u2 = PID_controller2(error2); moter2_control(u2); @@ -290,25 +303,40 @@ } -//Servo functie +//Servo functies +void servowait1(void) +{ + double Pulslength1 = 0.0005 + (servo_position1 * 0.002); //in seconden, waarde tussen de 500 en de 2500 microseconden, als de waarde omhoog gaat beweegt de servo tegen de klok in + myservo1 = true; + wait(Pulslength1); + myservo1 = false; +} + +void servowait2(void) +{ + double Pulslength2 = 0.0005 + (servo_position2 * 0.002); //in seconden + myservo2 = true; + wait(Pulslength2); + myservo2 = false; +} + +void servowait3(void) +{ + double Pulslength3 = 0.0005 + (servo_position3 * 0.002); //in seconden + myservo3 = true; + wait(Pulslength3); + myservo3 = false; +} + void ServoPeriod() { - led1 = 1; - led2 = 1; - double Pulslength = 0.0005 + (servo_position * 0.002); //in seconden - if (counts <= (Pulslength/Periodlength)) { - myservo1.pulsewidth(0.00006); - counts++; - } - else if (counts <= (0.02/Periodlength)){ - myservo1.pulsewidth(0); - counts++; - } - else { - counts = 0; - } + servowait1(); + servowait2(); + servowait3(); } + + void ContinuousReader(void) { Pot2 = pot.read(); @@ -319,320 +347,321 @@ // StateMachine -enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE}; +enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE}; int f = 1; -states currentState = MOTORS_OFF; +states currentState = MOTORS_OFF; bool stateChanged = true; // Make sure the initialization of first state is executed void ProcessStateMachine(void) { - switch (currentState) - { - case MOTORS_OFF: - // Actions - if (stateChanged) - { - // state initialization: rood + switch (currentState) { + case MOTORS_OFF: + // Actions + if (stateChanged) { + // state initialization: rood + + led1 = 1; + led2 = 0; + led3 = 1; + wait (1); + stateChanged = false; + } - led1 = 1; - led2 = 0; - led3 = 1; - wait (1); - stateChanged = false; - } - - // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden - if (!button3) - { - currentState = CALIBRATION ; - stateChanged = true; - } - else - { - currentState = MOTORS_OFF; - stateChanged = true; - } - - break; - - case CALIBRATION: - // Actions - if (stateChanged) - { - // state initialization: oranje + // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden + if (!button3) { + currentState = CALIBRATION ; + stateChanged = true; + } else { + currentState = MOTORS_OFF; + stateChanged = true; + } + + break; + + case CALIBRATION: + // Actions + if (stateChanged) { + // state initialization: oranje led1 = 0; led2 = 0; led3 = 1; - - servo_position = 0.5; - ServoTick.attach(ServoPeriod, Periodlength); - wait(1); - servo_position = 0.9; - wait(1); - servo_position = 0.5; + wait(1); - servo_position = 0.1; - wait (1); stateChanged = false; - ServoTick.detach(); - } - - // State transition logic: automatisch terug naar motors off. + + } + + // State transition logic: automatisch terug naar motors off. + + currentState = HOMING1; + stateChanged = true; + break; + + case HOMING1: + // Actions + if (stateChanged) { + // state initialization: green + t.start(); + led1 = 0; + led2 = 1; + led3 = 1; - currentState = HOMING1; - stateChanged = true; - break; + if (!But1) { + led1 = 1; + float H1 = 0.99f; + moter1_control(H1); + wait(0.001f); + } else if (!But2) { + led1 = 1; + float H1 = -0.99f; + moter1_control(H1); + wait(0.001f); + } + encoder1.reset(); + motor_position1 = 0; + pwmpin1 = 0; + pwmpin2 = 0; + ; + + stateChanged = false; + } + + // State transition logic: naar HOMING (button2), na 5 min naar MOTORS_OFF + + if (!button3) { + currentState = HOMING2 ; + stateChanged = true; + wait(1); + } else if (t>300) { + t.stop(); + t.reset(); + currentState = MOTORS_OFF ; + stateChanged = true; + } else { + currentState = HOMING1 ; + stateChanged = true; + } + break; -case HOMING1: - // Actions - if (stateChanged) - { - // state initialization: green - t.start(); - led1 = 0; - led2 = 1; - led3 = 1; - - if (!But1) - { - led1 = 1; - float H1 = 0.7f; - moter1_control(H1); - wait(0.001f); - } - else if (!But2) - { - led1 = 1; - float H1 = -0.7f; - moter1_control(H1); - wait(0.001f); - } - encoder1.reset(); - motor_position1 = 0; - pwmpin1 = 0; - pwmpin2 = 0; - ; - - stateChanged = false; - } - - // State transition logic: naar HOMING (button2), na 5 min naar MOTORS_OFF + case HOMING2: + // Actions + if (stateChanged) { + // state initialization: white + t.start(); + led1 = 0; + led2 = 0; + led3 = 0; + + if (!But1) { + led1 = 1; + float H2 = 0.98f; + moter2_control(H2); + wait(0.001f); + } else if (!But2) { + led1 = 1; + float H2 = -0.98f; + moter2_control(H2); + wait(0.001f); + } + encoder2.reset(); + motor_position2 = 0; + pwmpin1 = 0; + pwmpin2 = 0; + ; + + stateChanged = false; + } + + // State transition logic: naar DEMO (button2), naar MOVEMENT(button3) + if (!button2) { + currentState = DEMO; + stateChanged = true; + } else if (!button3) { + currentState = MOVEMENT ; + stateChanged = true; + + } else if (t>300) { + t.stop(); + t.reset(); + currentState = MOTORS_OFF ; + stateChanged = true; + } else { + currentState = HOMING2 ; + stateChanged = true; + } + break; + + case DEMO: + // Actions + if (stateChanged) { + // state initialization: light blue + led1 = 0; + led2 = 1; + led3 = 0; + + servo_position1 = 0.5; + servo_position2 = 0.5; + servo_position3 = 0.5; + ServoTick.attach(&ServoPeriod, Periodlength); - if (!button3) - { - currentState = HOMING2 ; - stateChanged = true; - wait(1); - } - else if (t>300) - { - t.stop(); - t.reset(); - currentState = MOTORS_OFF ; - stateChanged = true; - } - else - { - currentState = HOMING1 ; - stateChanged = true; - } - break; - - case HOMING2: - // Actions - if (stateChanged) - { - // state initialization: white - t.start(); - led1 = 0; - led2 = 0; - led3 = 0; - - if (!But1) - { - led1 = 1; - float H2 = 0.98f; - moter2_control(H2); - wait(0.001f); - } - else if (!But2) - { - led1 = 1; - float H2 = -0.98f; - moter2_control(H2); - wait(0.001f); - } - encoder2.reset(); - motor_position2 = 0; - pwmpin1 = 0; - pwmpin2 = 0; - ; - - stateChanged = false; - } - - // State transition logic: naar DEMO (button2), naar MOVEMENT(button3) - if (!button2) - { - currentState = DEMO; - stateChanged = true; - } - else if (!button3) - { - currentState = MOVEMENT ; - stateChanged = true; - led1 = 1; - led2 = 0; - led3 = 0; - wait(1); - } - else if (t>300) - { - t.stop(); - t.reset(); - currentState = MOTORS_OFF ; - stateChanged = true; - } - else - { - currentState = HOMING2 ; - stateChanged = true; - } - break; - - case DEMO: - // Actions - if (stateChanged) - { - // state initialization: light blue - led1 = 0; - led2 = 1; - led3 = 0; - wait (1); - - stateChanged = false; - } - - // State transition logic: automatisch terug naar HOMING - currentState = HOMING1; - stateChanged = true; - break; - - case MOVEMENT: - // Actions - if (stateChanged) - { - // state initialization: purple - t.start(); + wait(1.5 ); + servo_position1 = 0.9; + servo_position2 = 0.9; + servo_position3 = 0.9; + wait(1.5); + servo_position1 = 0.1; + servo_position2 = 0.1; + servo_position3 = 0.1; + wait(1); + + ServoTick.detach(); + + + wait (1); + + stateChanged = false; + } + + // State transition logic: automatisch terug naar HOMING + currentState = HOMING2; + stateChanged = true; + break; + + case MOVEMENT: + // Actions + + if (stateChanged) { + // state initialization: purple + t.start(); // na 5 minuten terug naar Homing + led1 = 1; + led2 = 0; + led3 = 0; + + // Tickers aan + if (counts == 0) { + pwmpin1 = 0; + pwmpin2 = 0; + Input1 = pwm1; + Input2 = pwm2; + Pwm.attach (PwmMotor, Ts); + + servo_position1 = Duim_krom; + servo_position2 = MWP_krom; + servo_position3 = Wijsvinger_krom; + + ServoTick.attach(&ServoPeriod, Periodlength); + counts++; + wait(1); + } + + // Servo positie + + if (!But1) { + servo_position1 = Duim_krom; + servo_position2 = MWP_krom; + servo_position3 = Wijsvinger_krom; + led1 = !led1; + } + if (!But2) { + servo_position1 = Duim_recht; + servo_position2 = MWP_recht; + servo_position3 = Wijsvinger_recht; + led1 = !led1; + } - 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++; + + + */ - // 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; - led3 = 0; - - stateChanged = false; - } - - // State transition logic: naar FREEZE (button2), naar MOTORS_OFF(button3) anders naar MOVEMENT - if (!button2) - { - currentState = FREEZE; - stateChanged = true; - } - else if (!button3) - { - Pwm.detach (); - pwmpin2 = 0; - pwmpin1 = 0; - currentState = MOTORS_OFF ; - stateChanged = true; - } - else if (t>300) - { - t.stop(); - t.reset(); - currentState = HOMING1 ; - stateChanged = true; - } - else - { - currentState = MOVEMENT ; - stateChanged = true; - } - break; - + stateChanged = false; + } + + // State transition logic: naar FREEZE (button2), naar MOTORS_OFF(button3) anders naar MOVEMENT + if (!button2) { + currentState = FREEZE; + stateChanged = true; + } else if (!button3) { + Pwm.detach (); + ServoTick.detach(); + pwmpin2 = 0; + pwmpin1 = 0; + counts = 0; + currentState = MOTORS_OFF ; + stateChanged = true; + } else if (t>300) { + t.stop(); + t.reset(); + Pwm.detach (); + ServoTick.detach(); + counts = 0; + currentState = HOMING1 ; + stateChanged = true; + } else { + currentState = MOVEMENT ; + stateChanged = true; + } + break; + case FREEZE: - // Actions - if (stateChanged) - { - // state initialization: blue - led1 = 1; - led2 = 1; - led3 = 0; - wait (1); - - stateChanged = false; - } - - // State transition logic: automatisch terug naar MOVEMENT. + // Actions + if (stateChanged) { + // state initialization: blue + led1 = 1; + led2 = 1; + led3 = 0; + + wait (1); - currentState = MOVEMENT; - stateChanged = true; - break; - + stateChanged = false; + } + + // State transition logic: automatisch terug naar MOVEMENT. + + currentState = MOVEMENT; + stateChanged = true; + break; + + } } -} - + int main() { - + t2.start(); int counter = 0; - myservo1.period_us(60); + pwmpin1.period_us(60); //PotRead.attach(ContinuousReader,Ts); pc.baud(115200); - - - while(true) - { - led1 = 1; - led2 =1; - led3 =1; - /* - if(counter==10) - { - float tmp = t2.read(); - printf("%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Pulses2); - counter = 0; - } - counter++; - */ - ProcessStateMachine(); - - wait(0.001); - } + while(true) { + led1 = 1; + led2 =1; + led3 =1; + /* + if(counter==10) + { + float tmp = t2.read(); + printf("%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Pulses2); + counter = 0; + } + counter++; + */ + ProcessStateMachine(); + + + wait(0.001); } - - - - - - +} @@ -640,3 +669,9 @@ + + + + + +