Renate de Boer
Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 38:fb163733c147
- Parent:
- 37:ea621fdf306a
- Child:
- 39:37744ca57a58
--- a/main.cpp Tue Nov 05 10:03:58 2019 +0000
+++ b/main.cpp Tue Nov 05 12:00:00 2019 +0000
@@ -31,23 +31,25 @@
AnalogIn EMG_calf_raw (A2);
QEI Encoder1(D13, D12, NC, 64); // Definities voor de encoders op motor 1 (Encoder1) en 2 (Encoder2). Hiervoor wordt de QEI library gebruikt
-QEI Encoder2(D10, D9, NC, 64); // We gebruiken X2 encoding, wat standaard is en dus niet hoeft worden toegevoegd aan deze defninitie.
+QEI Encoder2(D10, D9, NC, 64); // We gebruiken X2 encoding, wat standaard is en dus niet hoeft worden toegevoegd aan deze defninitie.
// Het aantal counts per omwenteling is gelijk aan 64.
// OUTPUTS
PwmOut motor1(D6); // Definities voor de motorsnelheden door middel van PwmOut. Er kan een getal tussen 0 en 1 worden ingevoerd.
PwmOut motor2(D5);
-DigitalOut motor1_dir(D7); // Definities voor de richtingen van de motoren. Het getal 0 zorgt voor de ene richting, het getal 1 voor de andere.
+DigitalOut motor1_dir(D7); // Definities voor de richtingen van de motoren. Het getal 0 zorgt voor de ene richting, het getal 1 voor de andere.
DigitalOut motor2_dir(D4); // In ons geval zijn beide motoren rechtsom draaiend vanaf de assen bekeken, wanneer de richting op 1 wordt gezet.
// VARIABELEN VOOR ENCODER, MOTORHOEK ETC.
-double counts1; // Global variables definiëren voor het aantal counts dat uit de encoder komt en een begindefinitie voor
+double counts1; // Global variables definiëren voor het aantal counts dat uit de encoder komt en een begindefinitie voor
double counts2; // de offset opstellen.
double offset1 = 0.0;
double offset2 = 0.0;
const double conversion_factor = (2.0*M_PI)/((64.0*131.25)/2); // Omrekeningsfactor om de encoder counts om te zetten naar de huidige motorhoek.
double theta_h_1_rad; // Actuele motorhoek in radialen (motor 1).
double theta_h_2_rad; // Actuele motorhoek in radialen (motor 2).
+double q1; // Hoek joint 1 die volgt uit de actuele motorhoeken.
+double q2; // Hoek joint 2 die volgt uit de actuele motorhoeken.
// DEFINITIES VOOR FILTERS
@@ -56,33 +58,33 @@
BiQuadChain bqcbr;
BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass filter met een cut off frequentie van 25 Hz.
BiQuad bqbr2(1, -1.6180, 1, -1.6019, 0.9801); // Notch filter met een frequentie van 50 Hz en een notchwidth van 0.01 Hz.
-// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
+// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
// toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
BiQuadChain bqcbr2;
BiQuad bqbr3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Twee low-pass filters met een cut off frequentie van 2 Hz.
-BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
+BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
// BICEPS-LINKS
// Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
BiQuadChain bqcbl;
BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass filter met een cut off frequentie van 25 Hz.
BiQuad bqbl2(1, -1.6180, 1, -1.6019, 0.9801); // Notch filter met een frequentie van 50 Hz en een notchwidth van 0.01 Hz.
-// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
+// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
// toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
BiQuadChain bqcbl2;
BiQuad bqbl3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Twee low-pass filters met een cut off frequentie van 2 Hz.
-BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
+BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
// KUIT
// Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
BiQuadChain bqck;
BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass filter met een cut off frequentie van 25 Hz.
BiQuad bqk2(1, -1.6180, 1, -1.6019, 0.9801); // Notch filter met een frequentie van 50 Hz en een notchwidth van 0.01 Hz.
-// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
+// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
// toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
BiQuadChain bqck2;
BiQuad bqk3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Twee low-pass filters met een cut off frequentie van 2 Hz.
-BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
+BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
// VARIABELEN VOOR EMG + FILTEREN
double filtered_EMG_biceps_right_1; // Definities voor ruwe EMG-signalen, gefilterd met de high-pass en notch filter.
@@ -98,98 +100,88 @@
double filtered_EMG_calf;
// Variabelen voor HIDScope
-HIDScope scope(3); //
+HIDScope scope(3); // Op deze manier kunnen drie signalen worden weergegeven in HIDScope.
// VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP
-bool calib = false;
+bool calib = false; // Benodigde bool en 'runner' (i_calib) voor het uitvoeren van de EMG-kalbratie.
static int i_calib = 0;
double filtered_EMG_biceps_right_total; // Benodigde variabelen voor het berekenen van een gemiddelde maximale EMG-waarde tijdens de EMG-kalibratie.
-double filtered_EMG_biceps_left_total; // Dit totaal is een sommatie van de signalen over 5 seconden.
+double filtered_EMG_biceps_left_total; // Dit totaal is een sommatie van de signalen over 5 seconden.
double filtered_EMG_calf_total;
-double mean_EMG_biceps_right;
+double mean_EMG_biceps_right; // Global variables definiëren voor het gemiddelde maximale EMG-signaal, verkregen d.m.v. de EMG-kalibratie.
double mean_EMG_biceps_left;
double mean_EMG_calf;
// VARIABELEN VOOR OPERATION MODE (EMG)
-double normalized_EMG_biceps_right;
+double normalized_EMG_biceps_right; // Global variables definiëren voor het normaliseren van het gemeten EMG-signaal naar het gemiddelde maximale EMG-signaal.
double normalized_EMG_biceps_left;
double normalized_EMG_calf;
// VARIABELEN VOOR OPERATION MODE (RKI)
-double vx; // Geeft de 'desired velocity' in x-richting
-double vy; // Geeft de 'desired velocity' in y-richting
+double vx; // Geeft de definitie voor de 'desired velocity' in x-richting.
+double vy; // Geeft de definitie voor de 'desired velocity' in y-richting.
-double Inverse_jacobian[2][2];
+double Inverse_jacobian[2][2]; // Matrixen opstellen voor de inverse jacobian en de gewenste snelheden (vx en vy).
double desired_velocity[2][1];
-const double delta_t = 0.002;
+double Joint_velocity[2][1] = {{0.0}, {0.0}}; // Nulmatrix opstellen voor de joint snelheden, beginwaarden.
-double Joint_1_position = 0.0;
+const double delta_t = 0.002; // Tijdsverschil dat wordt gebruikt om de joint velocities te integreren, is gelijk aan de duur van een 'ticker ronde'.
+
+double Joint_1_position = 0.0; // Begindefinities opstellen voor de joint en motor posities.
double Joint_2_position = 0.0;
double Joint_1_position_prev = 0.0;
double Joint_2_position_prev = 0.0;
-double Joint_velocity[2][1] = {{0.0}, {0.0}};
-
-double q1_dot;
-double q2_dot;
-
-double q1;
-double q2;
-
double Motor_1_position = 0.0;
double Motor_2_position = 0.0;
-// VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
+double error_M1; // Definiëren van de motorerrors, het verschil tussen de daadwerkelijke hoek en de gewenste hoek.
+double error_M2;
-const double Kp = 12.5;
-const double Ki = 0.03;
-
-double theta_k_1 = 0.0;
+// VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
+const double Kp = 17.5; // Kp en Ki waarden voor de PI-controller definiëren.
+const double Ki = 1.02; // Zijn theoretisch getest met goede resultaten, in de praktijk konden geen goede tests worden verricht, doordat de
+ // robot oncontroleerbaar gedrag vertoonde.
+double theta_k_1 = 0.0; // Begin definities opstellen voor de motorhoeken na de proportional part.
double theta_k_2 = 0.0;
-double error_integral_1 = 0.0;
+double error_integral_1 = 0.0; // Begin definities opstellen voor de integralen van de errors.
double error_integral_2 = 0.0;
-double u_i_1;
+double u_i_1; // Uitkomst variabelen definiëren voor de vermenigvuldiging van de error integraal met Ki.
double u_i_2;
-double theta_t_1;
+double theta_t_1; // Variabelen opstellen voor de sommatie van theta_k en u_i.
double theta_t_2;
-double error_M1;
-double error_M2;
-
-double abs_theta_t_1;
+double abs_theta_t_1; // Variabele opstellen voor de absolute waarde van theta_t.
double abs_theta_t_2;
// VOIDS
-// Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden).
-// Enige optie is resetten, dan wordt het script opnieuw opgestart.
-void emergency()
-{
- loop_ticker.detach();
+void emergency() // Noodfunctie waarbij de motoren uit worden gezet en de ProcessStateMachine wordt losgekoppeld
+{ // van de ticker. De robot doet dan niks meer. De enige optie is om de mbed te resetten, waarna
+ loop_ticker.detach(); // het script opnieuw moet worden opgestart.
motor1.write(0);
motor2.write(0);
pc.printf("Ik ga exploderen!!!\r\n");
}
-
-// Motoren uitzetten
-void motors_off()
+
+void motors_off() // Functie waarbij beide motoren worden uitgezet.
{
motor1.write(0);
motor2.write(0);
pc.printf("Motoren uit functie\r\n");
}
-void EMG_calibration()
-{
- if (i_calib == 0) {
- filtered_EMG_biceps_right_total=0;
+void EMG_calibration() // Functie die wordt uitgevoerd tijdens de EMG kalibratie. Wordt geinitialiseerd door de bool calib,
+{ // die alleen waar is tijdens de EMG kalibratie. Er wordt gebruikgemaakt van een runner i_calib, zodat
+ if (i_calib == 0) { // EMG-waarden tijdens maximale spierspanning gedurende 5 seconden bij elkaar op worden geteld. Het gemiddelde
+ filtered_EMG_biceps_right_total=0; // hiervan kan worden bepaald door te delen door het aantal samples dat genomen is in dit interval, namelijk 2500.
filtered_EMG_biceps_left_total=0;
filtered_EMG_calf_total=0;
}
@@ -209,10 +201,10 @@
}
}
-void Homing_function()
-{
- if (theta_h_1_rad != 0.0) {
- if (theta_h_1_rad < 0) {
+void Homing_function() // Functie die ervoor zorgt dat homing wordt uitgevoerd. De motoren worden met een lage snelheid bewogen, tot de motorhoeken
+{ // weer in de referentie positie staan (0 in ons geval, doordat we compenseren met een offset voor encoder counts). Wanneer
+ if (theta_h_1_rad != 0.0) { // deze positie is bereikt, wordt de snelheid op nul gezet. In de ProcessStateMachine wordt overgegaan naar de volgende state,
+ if (theta_h_1_rad < 0) { // wanneer dit voor beide motoren is gebeurd.
motor1.write(0.3);
motor1_dir.write(0);
} else {
@@ -237,9 +229,9 @@
}
}
-void Inverse_Kinematics()
-{
- // Defining joint velocities based on calculations of matlab
+void Inverse_Kinematics() // Functie die de inverse kinematica uitvoert. De inverse Jacobian is afkosmtig uit eerdere berekeningen in Matlab.
+{ // The desired velocity komt voort uit de operation mode, waar hier een waarde aan wordt toegekend.
+ // Defining joint velocities based on calculations of matlab // Outputs van deze functie zijn motorposities, die later worden gebruikt bij het berekenen van een positie error.
Inverse_jacobian[0][0] = ((cos(q1+3.141592653589793/6.0)*-8.5E2-sin(q1)*4.25E2+cos(q1)*cos(q2)*2.25E2+cos(q1)*sin(q2)*6.77E2+cos(q2)*sin(q1)*6.77E2-sin(q1)*sin(q2)*2.25E2+sqrt(3.0)*cos(q1)*4.25E2-sqrt(3.0)*cos(q1)*cos(q2)*4.25E2+sqrt(3.0)*sin(q1)*sin(q2)*4.25E2)*(4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
Inverse_jacobian[0][1] = ((cos(q1)*4.25E2-sin(q1+3.141592653589793/6.0)*8.5E2-cos(q1)*cos(q2)*6.77E2+cos(q1)*sin(q2)*2.25E2+cos(q2)*sin(q1)*2.25E2+sin(q1)*sin(q2)*6.77E2+sqrt(3.0)*sin(q1)*4.25E2-sqrt(3.0)*cos(q1)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*sin(q1)*4.25E2)*(4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
Inverse_jacobian[1][0] = ((sin(q1)*-4.25E2+cos(q1)*cos(q2)*2.25E2+cos(q1)*sin(q2)*6.77E2+cos(q2)*sin(q1)*6.77E2-sin(q1)*sin(q2)*2.25E2+sqrt(3.0)*cos(q1)*4.25E2-sqrt(3.0)*cos(q1)*cos(q2)*4.25E2+sqrt(3.0)*sin(q1)*sin(q2)*4.25E2)*(-4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
@@ -251,7 +243,7 @@
Joint_velocity[0][0] = Inverse_jacobian[0][0]*desired_velocity[0][0] + Inverse_jacobian[0][1]*desired_velocity[1][0];
Joint_velocity[1][0] = Inverse_jacobian[1][0]*desired_velocity[0][0] + Inverse_jacobian[1][1]*desired_velocity[1][0];
- // Integratie
+ // Integration
Joint_1_position = Joint_1_position_prev + Joint_velocity[0][0]*delta_t;
Joint_2_position = Joint_2_position_prev + Joint_velocity[1][0]*delta_t;
@@ -262,10 +254,9 @@
Motor_2_position = Joint_1_position + Joint_2_position;
}
-// PI-CONTROLLER
-void PI_controller()
-{
-// Proportional part:
+void PI_controller() // De PI-controller zorgt ervoor dat het proportionele en het integrale aandeel worden bepaald.
+{ // Na sommatie van beide aandelen, wordt theta_t als output verkregen. Deze wordt later als input
+// Proportional part: // voor de motorsnelheden gebruikt.
theta_k_1= Kp * error_M1;
theta_k_2= Kp * error_M2;
@@ -280,9 +271,9 @@
theta_t_2= theta_k_2 + u_i_2;
}
-void Define_motor_dir()
-{
- if (theta_t_1 >= 0 && theta_t_2 >= 0) {
+void Define_motor_dir() // Deze functie bepaalt de output richtingen die aan de motoren worden gegeven tijdens de operation mode.
+{ // Wanneer de input (wordt ook als snelheid gegeven aan de motoren) positief is, wordt de draairichting van
+ if (theta_t_1 >= 0 && theta_t_2 >= 0) { // de motoren op nul gezet en vice versa. Dit betekent dat de motoren linksom draaien, gezien vanaf de assen.
motor1_dir.write(0);
motor2_dir.write(0);
}
@@ -299,11 +290,11 @@
}
}
-void Controlling_system()
-{
- Inverse_Kinematics();
-
- error_M1 = Motor_1_position + theta_h_1_rad;
+void Controlling_system() // Hoofdfunctie die wordt toegepast tijdens de operation mode. Een gewenste snelheid in x- en y-richting wordt
+{ // hiervoor opgesteld en is vervolgens de input voor de inverse kinematica functie. Door middel van een gewenste
+ Inverse_Kinematics(); // motorhoek en de actuele motorhoek wordt een error bepaald. Deze errors worden als input gebruikt voor de
+ // PI-controller, wiens absolute output wordt gebruikt om de motorsnelheid aan te sturen. Tevens wordt een functie
+ error_M1 = Motor_1_position + theta_h_1_rad; // aangeroepen die ervoor zorgt dat de motoren in de juiste richting gaan draaien.
error_M2 = Motor_2_position + theta_h_2_rad;
PI_controller();
@@ -376,9 +367,7 @@
// 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
// waarna het gemiddelde wordt bepaald.
if (calib) {
-
EMG_calibration();
-
}
// Genormaliseerde EMG's berekenen
@@ -391,21 +380,21 @@
case Motors_off:
if (stateChanged) {
- motors_off(); // functie waarbij motoren uitgaan
+ motors_off(); // Functie waarbij motoren uitgaan.
stateChanged = false;
pc.printf("Motors off state\r\n");
}
- if (Emergency_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
- emergency();
+ if (Emergency_button_pressed.read() == false) { // Normaal krijgt de button een waarde 1 bij indrukken, nu nul -> false.
+ emergency(); // Bij het indrukken van de emergency button, wordt overgegaan op de emergency functie.
}
- if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
- currentState = Calib_motor;
+ if (Power_button_pressed.read() == false) { // Normaal krijgt de button een waarde 1 bij indrukken, nu nul -> false.
+ currentState = Calib_motor; // Er wordt doorgegaan naar de volgende state, wanneer de power button wordt ingedrukt.
stateChanged = true;
pc.printf("Moving to Calib_motor state\r\n");
}
break;
- case Calib_motor:
+ case Calib_motor: // In deze state wordt de robot arm in de juiste referentie (home) positie geplaatst.
if (stateChanged) {
pc.printf("Zet motoren handmatig in home positie\r\n");
@@ -464,7 +453,6 @@
stateChanged = true;
pc.printf("Moving to operation mode \r\n");
}
-
break;
case Operation_mode:
@@ -486,9 +474,6 @@
stateChanged = false;
pc.printf("einde operation mode init");
}
- // Hier moet een functie worden aangeroepen die ervoor zorgt dat
- // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
- // zodat de robotarm kan bewegen
if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
motors_off();