![](/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
main.cpp
- Committer:
- aschut
- Date:
- 2019-03-27
- Revision:
- 2:7c9974f0947a
- Parent:
- 1:070092564648
- Child:
- 3:2aaf54ce090b
File content as of revision 2:7c9974f0947a:
//Voor het toevoegen van een button: #include "mbed.h" #include "MODSERIAL.h" #include "QEI.h" #include "BiQuad.h" // Algemeen DigitalIn button3(SW3); DigitalIn button2(SW2); AnalogIn But2(A5); AnalogIn But1(A3); DigitalOut led1(LED_GREEN); DigitalOut led2(LED_RED); DigitalOut led3(LED_BLUE); MODSERIAL pc(USBTX, USBRX); Timer t; Timer t2; //Motoren DigitalOut direction1(D4); 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); double Pulses1; double motor_position1; double Pulses2; double motor_position2; double error1; //Pot meter AnalogIn pot(A1); AnalogIn pot0(A0); float Pot2; float Pot1; //Ticker Ticker Pwm; Ticker PotRead; Ticker Kdc; //Kinematica double stap1; double stap2; double KPot; float ElbowReference; float Ellebooghoek1; float Ellebooghoek2; float Ellebooghoek3; float Ellebooghoek4; float PolsReference; float Polshoek1; float Polshoek2; float Polshoek3; float Polshoek4; float Hoeknieuw1; float Hoeknieuw2; //Limiet in graden float lowerlim1 = 0; float upperlim1 = 748.8; float lowerlim2 = 0; float upperlim2 = 1300; // VARIABLES PID CONTROLLER double Kp1 = 5; double Ki1 = 0; double Kd1 = 1; double Kp2 = 6; // Zonder arm: 6,0,1 double Ki2 = 0; double Kd2 = 1; double Ts = 0.0005; // Sample time in seconds // Functies Kinematica float Kinematics1(float KPot) { if (KPot > 0.45f) { stap1 = KPot*150*Ts; Hoeknieuw1 = PolsReference + stap1; return Hoeknieuw1; } else if (KPot < -0.45f) { stap1 = KPot*150*Ts; Hoeknieuw1 = PolsReference + stap1; return Hoeknieuw1; } else { return PolsReference; } } float Kinematics2(float KPot) { if (KPot > 0.45f) { stap2 = KPot*300*Ts; Hoeknieuw2 = ElbowReference + stap2; return Hoeknieuw2; } else if (KPot < -0.45f) { stap2 = KPot*300*Ts; Hoeknieuw2 = ElbowReference + stap2; return Hoeknieuw2; } else { return ElbowReference; } } float Limits1(float Polshoek2) { if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) { //Binnen de limieten Polshoek3 = Polshoek2; } else { if (Polshoek2 >= upperlim1) { //Boven de limiet Polshoek3 = upperlim1; } else { //Onder de limiet Polshoek3 = lowerlim1; } } return Polshoek3; } float Limits2(float Ellebooghoek2) { if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) { //Binnen de limieten Ellebooghoek3 = Ellebooghoek2; } else { if (Ellebooghoek2 >= upperlim2) { //Boven de limiet Ellebooghoek3 = upperlim2; } else { //Onder de limiet Ellebooghoek3 = lowerlim2; } } return Ellebooghoek3; } // PID Controller double PID_controller1(double error1) { 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; // Integral part error1_integral = error1_integral + error1 * Ts; double u_i1 = Ki1* error1_integral; // Derivative part double error1_derivative = (error1 - error1_prev)/Ts; double filtered_error1_derivative = LowPassFilter.step(error1_derivative); double u_d1 = Kd1 * filtered_error1_derivative; error1_prev = error1; // Sum all parts and return it return u_k1 + u_i1 + u_d1; } double PID_controller2(double error2) { 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; // Integral part error2_integral = error2_integral + error2 * Ts; double u_i2 = Ki2 * error2_integral; // Derivative part double error2_derivative = (error2 - error2_prev)/Ts; double filtered_error2_derivative = LowPassFilter.step(error2_derivative); double u_d2 = Kd2 * filtered_error2_derivative; error2_prev = error2; // Sum all parts and return it return u_k2 + u_i2 + u_d2; } // Functies Motor void moter1_control(double u1) { direction1= u1 > 0.0f; //positief = CW if (fabs(u1)> 0.5f) { u1 = 0.5f; } else { u1= u1; } pwmpin1= fabs(u1); //pwmduty cycle canonlybepositive, floatingpoint absolute value } void moter2_control(double u2) { direction2= u2 < 0.0f; //positief = CW if (fabs(u2)> 0.99f) { u2 = 0.99f; } else { u2= u2; } pwmpin2= fabs(u2); //pwmduty cycle canonlybepositive, floatingpoint absolute value } void PwmMotor(void) { // Reference hoek berekenen, in graden float Ellebooghoek1 = Kinematics2(pwm2); 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; Pulses2 = encoder2.getPulses(); motor_position2 = -(Pulses2/4800)*360; 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); } void MotorOn(void) { pwmpin1 = 0; pwmpin2 = 0; Pwm.attach (PwmMotor, Ts); } void ContinuousReader(void) { Pot2 = pot.read(); Pot1 = pot0.read(); pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1 pwm1 =(Pot1*2)-1; } // StateMachine enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE}; int f = 1; 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 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 led1 = 0; led2 = 0; led3 = 1; wait (1); stateChanged = false; } // 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; if (!But1) { led1 = 1; float H1 = 0.98f; moter1_control(H1); wait(0.001f); } else if (!But2) { led1 = 1; float H1 = -0.98f; moter1_control(H1); wait(0.001f); } Pulses1 = 0; pwmpin1 = 0; pwmpin2 = 0; ; stateChanged = false; } // State transition logic: naar DEMO (button2), naar MOVEMENT(button3) 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: green t.start(); led1 = 0; led2 = 1; led3 = 1; 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); } Pulses2 = 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(); pwmpin1 = 0; pwmpin2 = 0; 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; 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. currentState = MOVEMENT; stateChanged = true; break; } } int main() { t2.start(); int counter = 0; pwmpin2.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); } }