Astrid Schut
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 @@
+
+
+
+
+
+