Astrid Schut
Volledig besturingssysteem voor de myoelectrische prothese arm van mijn bachelor opdracht
Dependencies: mbed QEI MODSERIAL FastPWM biquadFilter
Diff: main.cpp
- Revision:
- 14:f8e84d287a22
- Parent:
- 12:20ae1d20148d
- Child:
- 15:26dd70c4a837
--- a/main.cpp Fri Apr 19 11:51:14 2019 +0000
+++ b/main.cpp Sat Apr 20 16:31:07 2019 +0000
@@ -1,11 +1,11 @@
-//Voor het toevoegen van een button:
+//libraries
#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "BiQuad.h"
#include "FastPWM.h"
-// Algemeen
+//~~~~~~~~~~~~ Algemeen ~~~~~~~~~~~~~~
DigitalIn button3(SW3);
DigitalIn button2(SW2);
DigitalIn But2(D12);
@@ -14,49 +14,52 @@
DigitalOut led1(LED_GREEN);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_BLUE);
-float counts = 0;
MODSERIAL pc(USBTX, USBRX);
-Timer t;
-Timer t2;
+Timer t; // State timer, na vijf minuten gaat de state automatisch terug naar een vorige state
+Timer t2; // Timer voor de grafiek in matlab
+int count = 0; // Counter voor de grafiek in matlab
+float counts = 0; // Counter voor de MOVEMENT state, zorgt ervoor dat de tickers niet steeds opnieuw attached worden
+
-//Motoren
-DigitalOut direction1(D4);
+//~~~~~~~~ Motor waardes ~~~~~~~~~~~~~~~
+DigitalOut direction1(D4); // Motor 1 is de rotatie motor
FastPWM pwmpin1(D5);
-FastPWM pwmpin2(D6);
+FastPWM pwmpin2(D6); // Motor 2 is de motor in de elleboog
DigitalOut direction2(D7);
-volatile float pwm1;
+volatile float pwm1; //Deze 'pwm' wordt gemaakt met de potmeters, wordt gebruikt voor het testen, maar niet voor de uiteindelijke besturing
volatile float pwm2;
//Encoder
QEI encoder1 (D1, D0, NC, 1200, QEI::X4_ENCODING);
QEI encoder2 (D3, D2, NC, 4800, QEI::X4_ENCODING);
-double Pulses1;
-double motor_position1;
+double Pulses1; // Afgelezen Encoder pulses
+double motor_position1; // De berekende hoek
double Pulses2;
double motor_position2;
-double error1;
-double u1;
+double error1; // Berekende error tussen de berekende hoek en de referentie hoek
+double error2;
+double u1; // PWM signaal dat naar de motor gaat
+double u2;
//Pot meter
-AnalogIn pot(A5);
+AnalogIn pot(A5); // De potmeters worden alleen gebruikt bij het testen
AnalogIn pot0(A0);
float Pot2;
float Pot1;
//Ticker
-Ticker Pwm;
-Ticker PotRead;
-Ticker Kdc;
+Ticker Pwm; // Ticker om de motoren te laten bewegen
+Ticker PotRead; // Ticker voor de Continuous read functie
+Ticker ServoTick; // Ticker om een PWM signaal met een periode van 20 ms bij een DigitalOut pin te creeëren
//Servo
-Ticker ServoTick;
-DigitalOut myservo1(D8); //Duim
-DigitalOut myservo2(D11); //Pink tot Middel vinger
-DigitalOut myservo3(D10); //wijsvinger
+DigitalOut myservo1(D8); //Duim
+DigitalOut myservo2(D11); //Pink tot Middel vinger
+DigitalOut myservo3(D10); //wijsvinger
-float Periodlength = 0.02; // de MG996R heeft een PWM periode van 20 ms
-double servo_position1; // in percentage van 0 tot 1, 0 is met de klok mee, 1 is tegen de klok in.
+float Periodlength = 0.02; // de MG996R Servo heeft een PWM periode van 20 ms
+double servo_position1; // in percentage van 0 tot 1, 0 is met de klok mee, 1 is tegen de klok in.
double servo_position2;
double servo_position3;
@@ -69,54 +72,42 @@
float Wijsvinger_recht = 0.93;
-// EMG
-float EMG1; // Rotatie
-float EMG2; // Elleboog
-float EMG3; // Hand
-float EMG4; // Reverse
-float Input1; // Voor zonder EMG
-float Input2;
-int count = 0;
+
-//------------- EMG gloabals -------------//
-// Tickers
-Ticker sample_ticker; //ticker voor filteren met 1000Hz
-Ticker sample_timer;
-Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 1000Hz
-Timer timer_calibration; //timer voor EMG Kalibratie
-double ts = 0.001; //tijdsstap !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-double calibration_time = 37; //Kalibratie tijd
+//~~~~~~~~ EMG waardes ~~~~~~~~~~~~~~~~~
//Input
AnalogIn emg1( A1 ); //Duim
AnalogIn emg2( A2 ); //Bicep
AnalogIn emg3( A3 ); //Dorsaal
AnalogIn emg4( A4 ); //Palmair
-// GLOBALS EMG
-//Gefilterde EMG signalen
-volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
+// Tickers
+Ticker sample_ticker; //ticker voor filteren met 1000Hz
+Ticker sample_timer; //ticker voor printen van de uiteindelijke EMG waardes met 2 Hz
+Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 100Hz
+Timer timer_calibration; //timer voor EMG Kalibratie
+double ts = 0.001; //tijdsstap
-bool thresholdreach1 = false;
-bool thresholdreach2 = false;
-bool thresholdreach3 = false;
-bool thresholdreach4 = false;
+float Input1; //Input voor motoren
+float Input2;
-volatile double temp_highest_emg1 = 0; //Hoogste waarde gevonden tijdens kalibratie
+//Gefilterde EMG signalen
+volatile double temp_highest_emg1 = 0; //Hoogste waarde gevonden tijdens kalibratie
volatile double temp_highest_emg2 = 0;
volatile double temp_highest_emg3 = 0;
volatile double temp_highest_emg4 = 0;
//Percentage van de hoogste waarde waar de bovenste treshold gezet moet worden
-double Duim_p_t = 0.5;
-double Bicep_p_t = 0.4;
-double Dorsaal_p_t = 0.6;
+double Duim_p_t = 0.5;
+double Bicep_p_t = 0.4;
+double Dorsaal_p_t = 0.6;
double Palmair_p_t = 0.5;
//Percentage van de hoogste waarde waar de onderste treshold gezet moet worden
-double Duim_p_tL = 0.5;
-double Bicep_p_tL = 0.4;
-double Dorsaal_p_tL = 0.5;
+double Duim_p_tL = 0.5;
+double Bicep_p_tL = 0.4;
+double Dorsaal_p_tL = 0.5;
double Palmair_p_tL = 0.5;
// Waarde bovenste treshold waar het signaal overheen moet om de arm te activeren
@@ -131,6 +122,11 @@
volatile double threshold3L;
volatile double threshold4L;
+bool thresholdreach1 = false; // Of de waarde boven de bovenste threshold is geweest
+bool thresholdreach2 = false;
+bool thresholdreach3 = false;
+bool thresholdreach4 = false;
+
// thresholdreads bools
bool Duim;
bool Bicep;
@@ -138,7 +134,9 @@
bool Palmair;
// filters
-//EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
+
+//EMG1
//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
BiQuadChain highp1;
BiQuad highp1_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
@@ -148,7 +146,7 @@
//Lowpass first order cutoff 0.4Hz
BiQuad lowp1( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
-//EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//EMG2
//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
BiQuadChain highp2;
BiQuad highp2_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
@@ -158,7 +156,7 @@
//Lowpass first order cutoff 0.4Hz
BiQuad lowp2( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
-//EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//EMG3
//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
BiQuadChain highp3;
BiQuad highp3_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
@@ -168,7 +166,7 @@
//Lowpass first order cutoff 0.4Hz
BiQuad lowp3( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
-//EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//EMG4
//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
BiQuadChain highp4;
BiQuad highp4_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
@@ -178,12 +176,12 @@
//Lowpass first order cutoff 0.4Hz
BiQuad lowp4( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
-//Kinematica
-double stap1;
+//~~~~~~~~ Kinematica ~~~~~~~~~~~~~~~~~~
+double stap1; //Stap die bij de referentie hoek wordt opgeteld bij een EMG signaal van 1
double stap2;
double KPot;
-float ElbowReference;
+float ElbowReference;
float Ellebooghoek1;
float Ellebooghoek2;
float Ellebooghoek3;
@@ -199,31 +197,31 @@
float Hoeknieuw2;
//Limiet in graden
-float lowerlim1 = -900;
+float lowerlim1 = -900; //Rotatie motor gewricht limiet, gelijk aan 180 graden rotatie beide kanten op
float upperlim1 = 900;
-float lowerlim2 = 0;
+float lowerlim2 = 0; // Elleboog limiet, van 0 graden (helemaal gebogen) tot 110 graden (helemaal gestrekt)
float upperlim2 = 1500;
-// VARIABLES PID CONTROLLER
+//~~~~~~~~ PID controler waardes ~~~~~~
double Kp1 = 12.5;
double Ki1 = 0;
double Kd1 = 1;
-double Kp2 = 20; // Zonder arm: 6,0,1, met rotatie: 10, 0, 1
+double Kp2 = 20;
double Ki2 = 0;
double Kd2 = 1;
-double Ts = 0.0005; // Sample time in seconds
+double Ts = 0.0005; // tijdstap voor pwm ticker
-// ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~
+// ~~~~~~~~~~~~~~~~~~~ EMG functies ~~~~~~~~~~~~~~~~~~
void emgsample()
{
-
+
// EMG signaal lezen
double emgread1 = emg1.read();
double emgread2 = emg2.read();
double emgread3 = emg3.read();
double emgread4 = emg4.read();
-
+
// Vierde orde highpass filter + notch filter
double emg1_highpassed = highp1.step(emgread1);
double emg2_highpassed = highp2.step(emgread2);
@@ -248,104 +246,100 @@
void threshold_check()
{
-// EMG1 Check
-if (thresholdreach1 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest
- //bovenste threshold check
- if(emg1_filtered>threshold1) {
- Duim = 1;
- thresholdreach1 = true;
-
- } else {
- Duim= 0;
- }
- }
-else{ //Als emg_filtered boven de bovenste threshold is geweest
- //onderste threshold check
- if(emg1_filtered<threshold1L) {
- Duim = 0;
- thresholdreach1 = false;
-
- } else {
- Duim= 1;
- }
+// EMG1 Check
+ if (thresholdreach1 == false) { //Als emg_filtered nog niet boven de bovenste threshold is geweest
+ //bovenste threshold check
+ if(emg1_filtered>threshold1) {
+ Duim = 1;
+ thresholdreach1 = true;
+
+ } else {
+ Duim= 0;
+ }
+ } else { //Als emg_filtered boven de bovenste threshold is geweest
+ //onderste threshold check
+ if(emg1_filtered<threshold1L) {
+ Duim = 0;
+ thresholdreach1 = false;
+
+ } else {
+ Duim= 1;
+ }
}
-// EMG2 Check
-if (thresholdreach2 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest
- //bovenste threshold check
- if(emg2_filtered>threshold2) {
- Bicep = 1;
- thresholdreach2 = true;
- } else {
- Bicep= 0;
- }
- }
-else{ //Als emg_filtered boven de bovenste threshold is geweest
- //onderste threshold check
- if(emg2_filtered<threshold2L) {
- Bicep = 0;
- thresholdreach2 = false;
-
- } else {
- Bicep= 1;
- }
+// EMG2 Check
+ if (thresholdreach2 == false) { //Als emg_filtered nog niet boven de bovenste threshold is geweest
+ //bovenste threshold check
+ if(emg2_filtered>threshold2) {
+ Bicep = 1;
+ thresholdreach2 = true;
+ } else {
+ Bicep= 0;
+ }
+ } else { //Als emg_filtered boven de bovenste threshold is geweest
+ //onderste threshold check
+ if(emg2_filtered<threshold2L) {
+ Bicep = 0;
+ thresholdreach2 = false;
+
+ } else {
+ Bicep= 1;
+ }
}
-// EMG3 Check
-if (thresholdreach3 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest
- //bovenste threshold check
- if(emg3_filtered>threshold3) {
- Dorsaal = 1;
- thresholdreach3 = true;
- } else {
- Dorsaal= 0;
- }
- }
-else{ //Als emg_filtered boven de bovenste threshold is geweest
- //onderste threshold check
- if(emg3_filtered<threshold3L) {
- Dorsaal = 0;
- thresholdreach3 = false;
-
- } else {
- Dorsaal= 1;
- }
+// EMG3 Check
+ if (thresholdreach3 == false) { //Als emg_filtered nog niet boven de bovenste threshold is geweest
+ //bovenste threshold check
+ if(emg3_filtered>threshold3) {
+ Dorsaal = 1;
+ thresholdreach3 = true;
+ } else {
+ Dorsaal= 0;
+ }
+ } else { //Als emg_filtered boven de bovenste threshold is geweest
+ //onderste threshold check
+ if(emg3_filtered<threshold3L) {
+ Dorsaal = 0;
+ thresholdreach3 = false;
+
+ } else {
+ Dorsaal= 1;
+ }
}
-// EMG4 Check
-if (thresholdreach4 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest
- //bovenste threshold check
- if(emg4_filtered>threshold4) {
- Palmair = 1;
- thresholdreach4 = true;
- } else {
- Palmair= 0;
- }
- }
-else{ //Als emg_filtered boven de bovenste threshold is geweest
- //onderste threshold check
- if(emg4_filtered<threshold4L) {
- Palmair = 0;
- thresholdreach4 = false;
-
- } else {
- Palmair= 1;
- }
+// EMG4 Check
+ if (thresholdreach4 == false) { //Als emg_filtered nog niet boven de bovenste threshold is geweest
+ //bovenste threshold check
+ if(emg4_filtered>threshold4) {
+ Palmair = 1;
+ thresholdreach4 = true;
+ } else {
+ Palmair= 0;
+ }
+ } else { //Als emg_filtered boven de bovenste threshold is geweest
+ //onderste threshold check
+ if(emg4_filtered<threshold4L) {
+ Palmair = 0;
+ thresholdreach4 = false;
+
+ } else {
+ Palmair= 1;
+ }
}
- }
+}
-// Functies Kinematica
-float Kinematics1(float EMG1)
+//~~~~~~~~~ Functies Kinematica ~~~~~~~~~~~~
+float Kinematics1(float Bicep)
{
- if (Dorsaal == 1 && Duim == 0) {
+ if (Bicep == 1 && Duim == 0) { //Als alleen Bicep flexie wordt afgelezen gaat de waarde van de referentie hoek omhoog
stap1 = 450*Ts;
Hoeknieuw1 = PolsReference + stap1;
return Hoeknieuw1;
}
- else if (Dorsaal == 1 && Duim == 1) {
+ else if (Bicep == 1 && Duim == 1) { //Als Bicep en Duim flexie wordt afgelezen gaat de waarde van de referentie hoek omlaag
stap1 = 450*Ts;
Hoeknieuw1 = PolsReference - stap1;
return Hoeknieuw1;
@@ -358,13 +352,13 @@
float Kinematics2(bool Dorsaal)
{
- if (Dorsaal == 1 && Duim == 0) {
+ if (Dorsaal == 1 && Duim == 0) { //Als alleen Dorsaal flexie wordt afgelezen gaat de waarde van de referentie hoek omhoog
stap2 = 300*Ts;
Hoeknieuw2 = ElbowReference + stap2;
return Hoeknieuw2;
}
- else if (Dorsaal == 1 && Duim == 1) {
+ else if (Dorsaal == 1 && Duim == 1) { //Als Dorsaal en Duim flexie wordt afgelezen gaat de waarde van de referentie hoek omhoog
stap2 = 300*Ts;
Hoeknieuw2 = ElbowReference - stap2;
return Hoeknieuw2;
@@ -378,12 +372,12 @@
float Limits1(float Polshoek2)
{
- if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) { //Binnen de limieten
+ if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) { //Binnen de limieten
Polshoek3 = Polshoek2;
}
else {
- if (Polshoek2 >= upperlim1) { //Boven de limiet
+ if (Polshoek2 >= upperlim1) { //Boven de limiet
Polshoek3 = upperlim1;
} else { //Onder de limiet
Polshoek3 = lowerlim1;
@@ -397,12 +391,12 @@
float Limits2(float Ellebooghoek2)
{
- if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) { //Binnen de limieten
+ if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) { //Binnen de limieten
Ellebooghoek3 = Ellebooghoek2;
}
else {
- if (Ellebooghoek2 >= upperlim2) { //Boven de limiet
+ if (Ellebooghoek2 >= upperlim2) { //Boven de limiet
Ellebooghoek3 = upperlim2;
} else { //Onder de limiet
Ellebooghoek3 = lowerlim2;
@@ -413,12 +407,12 @@
}
-// PID Controller
+//~~~~~~~~ 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);
+ static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
// Proportional part:
double u_k1 = Kp1 * error1;
@@ -459,39 +453,40 @@
return u_k2 + u_i2 + u_d2;
}
-// Functies Motor
+//~~~~~~~~~~~~ Functies Motor ~~~~~~
void moter1_control(double u1)
{
- direction1= u1 > 0.0f; //positief = CW
+ direction1= u1 > 0.0f; //positief = CW
if (fabs(u1)> 0.7f) {
u1 = 0.7f;
} else {
u1= u1;
}
- pwmpin1.write(fabs(u1)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
+ pwmpin1.write(fabs(u1)) ;
}
void moter2_control(double u2)
{
- direction2= u2 < 0.0f; //positief = CW
+ direction2= u2 < 0.0f; //positief = CW
if (fabs(u2)> 0.99f) {
u2 = 0.99f;
} else {
u2= u2;
}
- pwmpin2.write(fabs(u2)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
+ pwmpin2.write(fabs(u2)) ;
}
void PwmMotor(void)
{
//Continues Read
- Pot2 = pot.read();
+ Pot2 = pot.read(); //Potmeters alleen voor testen
Pot1 = pot0.read();
pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
pwm1 =(Pot1*2)-1;
- Input1 = Bicep;
+ Input1 = Bicep; //Voor testen hier potmeter waarde 'pwm1'invullen
Input2 = Palmair;
-
+
+ // Reference hoek berekenen, in graden
float Polshoek1 = Kinematics1(Input1);
float Polshoek4 = Limits1(Polshoek1);
PolsReference = Polshoek4;
@@ -506,7 +501,8 @@
motor_position1 = -(Pulses1/1200)*360;
Pulses2 = encoder2.getPulses();
motor_position2 = -(Pulses2/4800)*360;
-
+
+ // Error berekenen, naar PID controller en dan naar motor
double error1 = PolsReference - motor_position1;
double u1 = PID_controller1(error1);
moter1_control(u1);
@@ -525,8 +521,8 @@
}
-//Servo functies
-void servowait1(void)
+//~~~~~~~~~~ Servo functies ~~~~~~~~~~~~
+void servowait1(void)~ //creeërt een PWM periode van 20 ms met een pulslengte tussen de 500 en 2500 microseconden
{
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;
@@ -559,20 +555,21 @@
-void ContinuousReader(void)
+void ContinuousReader(void) //leest potmeters af
{
Pot2 = pot.read();
Pot1 = pot0.read();
- pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
+ pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
pwm1 =(Pot1*2)-1;
}
-void sample(){
-pc.printf("Duim = %i\r\n", Duim);
-pc.printf("Bicep = %i\r\n",Bicep);
-pc.printf("Dorsaal = %i\r\n", Dorsaal);
-pc.printf("Palmair = %i\r\n", Palmair);
-
+void sample() //Print uiteindelijke EMG output
+{
+ pc.printf("Duim = %i\r\n", Duim);
+ pc.printf("Bicep = %i\r\n",Bicep);
+ pc.printf("Dorsaal = %i\r\n", Dorsaal);
+ pc.printf("Palmair = %i\r\n", Palmair);
+
}
// StateMachine
@@ -597,7 +594,7 @@
stateChanged = false;
}
- // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
+ // State transition logic: Als button 3 word ingedrukt --> calibratie, anders motor uithouden
if (!button3) {
currentState = CALIBRATION ;
stateChanged = true;
@@ -618,7 +615,7 @@
sample_ticker.attach(&emgsample, 0.001); // Leest het ruwe EMG signaal af met een frequentie van 1000Hz
timer_calibration.reset();
timer_calibration.start();
- while(timer_calibration<20) { //Duim
+ while(timer_calibration<20) { //Binnen 20 seconden alle spieren maximaal aanspannen, hoogste waarde wordt opgeslagen
if(timer_calibration>0 && timer_calibration<20) {
if(emg1_filtered>temp_highest_emg1) {
@@ -645,7 +642,7 @@
}
pc.printf("threshold calculation\r\n");
- threshold1 = temp_highest_emg1*Duim_p_t;
+ threshold1 = temp_highest_emg1*Duim_p_t; //Percentage van de hoogste waarde wordt gebruikt om de bovenste en onderste treshold aan te geven
threshold2 = temp_highest_emg2*Bicep_p_t;
threshold3 = temp_highest_emg3*Dorsaal_p_t;
threshold4 = temp_highest_emg4*Palmair_p_t;
@@ -659,13 +656,13 @@
}
sample_ticker.detach();
timer_calibration.stop();
- // State transition logic: automatisch terug naar motors off.
+ // State transition logic: automatisch naar HOMING 1
currentState = HOMING1;
stateChanged = true;
break;
- case HOMING1:
+ case HOMING1: //Gebruik de knoppen op het motor shield om het rotatie gewricht naar de beginpositie te brengen
// Actions
if (stateChanged) {
// state initialization: green
@@ -694,7 +691,7 @@
stateChanged = false;
}
- // State transition logic: naar HOMING (button2), na 5 min naar MOTORS_OFF
+ // State transition logic: naar HOMING 2(button3), na 5 min naar MOTORS_OFF
if (!button3) {
t.stop();
@@ -713,9 +710,9 @@
}
break;
- case HOMING2:
+ case HOMING2: // gebruik knoppen op het motor shield om het elleboog gewricht naar de begin positie te brengen ( helemaal gebogen)
// Actions
-
+
if (stateChanged) {
// state initialization: white
t.start();
@@ -743,7 +740,7 @@
stateChanged = false;
}
- // State transition logic: naar DEMO (button2), naar MOVEMENT(button3)
+ // State transition logic: naar DEMO (button2), naar MOVEMENT(button3), na 5 min. automatisch naar MOTORS OFF
if (!button2) {
currentState = DEMO;
stateChanged = true;
@@ -765,7 +762,7 @@
}
break;
- case DEMO:
+ case DEMO: // Mogelijkheid om hier je demo routine in te zetten
// Actions
if (stateChanged) {
// state initialization: light blue
@@ -796,12 +793,12 @@
stateChanged = false;
}
- // State transition logic: automatisch terug naar HOMING
+ // State transition logic: automatisch terug naar HOMING2
currentState = HOMING2;
stateChanged = true;
break;
- case MOVEMENT:
+ case MOVEMENT: // Hier worden de motoren bestuurd met EMG input
// Actions
if (stateChanged) {
@@ -810,29 +807,30 @@
led1 = 1;
led2 = 0;
led3 = 0;
-
+
// Tickers aan
if (counts == 0) {
pwmpin1 = 0;
pwmpin2 = 0;
Input1 = pwm1;
Input2 = pwm2;
- Pwm.attach (PwmMotor, Ts);
+
+ Pwm.attach (PwmMotor, Ts); // Motoren gaan aan
sample_ticker.attach(&emgsample, 0.001); // Leest het ruwe EMG signaal af met een frequentie van 1000Hz
- threshold_check_ticker.attach(&threshold_check, 0.01);
- sample_timer.attach(&sample, 0.5);
+ threshold_check_ticker.attach(&threshold_check, 0.01); //Checkt of threshold overgeschreden wordt
+ sample_timer.attach(&sample, 0.5); // print de EMG waardes
servo_position1 = Duim_krom;
servo_position2 = MWP_krom;
servo_position3 = Wijsvinger_krom;
-
- ServoTick.attach(&ServoPeriod, Periodlength);
+
+ ServoTick.attach(&ServoPeriod, Periodlength); //Servo's gaan aan
counts++;
- wait(1);
+ wait(1); //Zorgt ervoor dat hij niet doorschiet naar de volgende staat als je te lang de knop indrukt
}
-
+
// Servo positie
- if (Dorsaal == 1 && Duim == 0) {
+ if (Dorsaal == 1 && Duim == 0) {
servo_position1 = Duim_krom;
servo_position2 = MWP_krom;
servo_position3 = Wijsvinger_krom;
@@ -845,20 +843,6 @@
led1 = !led1;
}
- /*
-
- // 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++;
-
-
- */
-
stateChanged = false;
}
@@ -919,12 +903,11 @@
currentState = MOVEMENT;
stateChanged = true;
counts = 0;
- }
- else {
+ } else {
currentState = FREEZE ;
stateChanged = false;
- }
-
+ }
+
break;
}