Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@37:ea621fdf306a, 2019-11-05 (annotated)

Committer:
Renate
Date:
Tue Nov 05 10:03:58 2019 +0000
Revision:
37:ea621fdf306a
Parent:
32:d651c23bbb77
Child:
38:fb163733c147
Bezig met het script netter maken

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
Rosalie 3:6ee0b20c23b0 2 #include "HIDScope.h"
Rosalie 3:6ee0b20c23b0 3 #include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
Rosalie 3:6ee0b20c23b0 5 #include "BiQuad.h"
Renate 37:ea621fdf306a 6 //#include "FastPWM.h"
Renate 32:d651c23bbb77 7 #define M_PI 3.14159265358979323846 /* pi */
WiesjeRoskamp 2:aee655d11b6d 8 #include <math.h>
Renate 37:ea621fdf306a 9 //#include "Servo.h"
Renate 21:456acc79726c 10 #include <cmath>
Renate 37:ea621fdf306a 11 //#include <complex>
RobertoO 0:67c50348f842 12
WiesjeRoskamp 2:aee655d11b6d 13 Serial pc(USBTX, USBRX);
Rosalie 3:6ee0b20c23b0 14
Renate 23:4572750a5c59 15 // TICKERS
Renate 37:ea621fdf306a 16 Ticker loop_ticker; // Ticker aanmaken die ervoor zorgt dat de ProcessStateMachine met een frequentie vsn 500 Hz kan worden aangeroepen.
Renate 15:ad065ab92d11 17
Renate 23:4572750a5c59 18 // BENODIGD VOOR PROCESS STATE MACHINE
Renate 37:ea621fdf306a 19 enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode}; // Alle states definiëren.
Renate 37:ea621fdf306a 20 states currentState = Motors_off; // State waarin wordt begonnen definiëren.
Renate 37:ea621fdf306a 21 bool stateChanged = true; // Toevoegen zodat de initialisatie van de eerste state plaatsvindt.
Renate 23:4572750a5c59 22
Renate 23:4572750a5c59 23 // INPUTS
Renate 37:ea621fdf306a 24 DigitalIn Power_button_pressed(D1); // Definiëren van alle buttons, we gebruiken hiervoor geen InterruptIn, maar DigitalIn.
Renate 9:4de589636f50 25 DigitalIn Emergency_button_pressed(D2);
Renate 22:8585d41a670b 26 DigitalIn Motor_calib_button_pressed(SW2);
Renate 37:ea621fdf306a 27 DigitalIn Homing_button_pressed(SW3);
WiesjeRoskamp 2:aee655d11b6d 28
Renate 37:ea621fdf306a 29 AnalogIn EMG_biceps_right_raw (A0); // Definiëren van de ruwe EMG-signalen die binnenkomen via AnalogIn.
Renate 37:ea621fdf306a 30 AnalogIn EMG_biceps_left_raw (A1); // We gebruiken signalen van de kuit en de linker en rechter biceps.
Renate 19:1fd39a2afc30 31 AnalogIn EMG_calf_raw (A2);
Renate 15:ad065ab92d11 32
Renate 37:ea621fdf306a 33 QEI Encoder1(D13, D12, NC, 64); // Definities voor de encoders op motor 1 (Encoder1) en 2 (Encoder2). Hiervoor wordt de QEI library gebruikt
Renate 37:ea621fdf306a 34 QEI Encoder2(D10, D9, NC, 64); // We gebruiken X2 encoding, wat standaard is en dus niet hoeft worden toegevoegd aan deze defninitie.
Renate 37:ea621fdf306a 35 // Het aantal counts per omwenteling is gelijk aan 64.
Renate 23:4572750a5c59 36 // OUTPUTS
Renate 37:ea621fdf306a 37 PwmOut motor1(D6); // Definities voor de motorsnelheden door middel van PwmOut. Er kan een getal tussen 0 en 1 worden ingevoerd.
Renate 37:ea621fdf306a 38 PwmOut motor2(D5);
Renate 21:456acc79726c 39
Renate 37:ea621fdf306a 40 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.
Renate 37:ea621fdf306a 41 DigitalOut motor2_dir(D4); // In ons geval zijn beide motoren rechtsom draaiend vanaf de assen bekeken, wanneer de richting op 1 wordt gezet.
Renate 23:4572750a5c59 42
Renate 23:4572750a5c59 43 // VARIABELEN VOOR ENCODER, MOTORHOEK ETC.
Renate 37:ea621fdf306a 44 double counts1; // Global variables definiëren voor het aantal counts dat uit de encoder komt en een begindefinitie voor
Renate 37:ea621fdf306a 45 double counts2; // de offset opstellen.
Renate 37:ea621fdf306a 46 double offset1 = 0.0;
Renate 37:ea621fdf306a 47 double offset2 = 0.0;
Renate 37:ea621fdf306a 48 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.
Renate 37:ea621fdf306a 49 double theta_h_1_rad; // Actuele motorhoek in radialen (motor 1).
Renate 37:ea621fdf306a 50 double theta_h_2_rad; // Actuele motorhoek in radialen (motor 2).
Renate 21:456acc79726c 51
Renate 23:4572750a5c59 52 // DEFINITIES VOOR FILTERS
Renate 20:a6a5bdd7d118 53
Renate 21:456acc79726c 54 // BICEPS-RECHTS
Renate 37:ea621fdf306a 55 // Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
Renate 28:7c7508bdb21f 56 BiQuadChain bqcbr;
Renate 37:ea621fdf306a 57 BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass filter met een cut off frequentie van 25 Hz.
Renate 37:ea621fdf306a 58 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.
Renate 37:ea621fdf306a 59 // Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
Renate 37:ea621fdf306a 60 // toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
Renate 21:456acc79726c 61 BiQuadChain bqcbr2;
Renate 37:ea621fdf306a 62 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.
Renate 37:ea621fdf306a 63 BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
Renate 20:a6a5bdd7d118 64
Renate 21:456acc79726c 65 // BICEPS-LINKS
Renate 37:ea621fdf306a 66 // Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
Renate 28:7c7508bdb21f 67 BiQuadChain bqcbl;
Renate 37:ea621fdf306a 68 BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass filter met een cut off frequentie van 25 Hz.
Renate 37:ea621fdf306a 69 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.
Renate 37:ea621fdf306a 70 // Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
Renate 37:ea621fdf306a 71 // toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
Renate 21:456acc79726c 72 BiQuadChain bqcbl2;
Renate 37:ea621fdf306a 73 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.
Renate 37:ea621fdf306a 74 BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
Renate 21:456acc79726c 75
Renate 21:456acc79726c 76 // KUIT
Renate 37:ea621fdf306a 77 // Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
Renate 28:7c7508bdb21f 78 BiQuadChain bqck;
Renate 37:ea621fdf306a 79 BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass filter met een cut off frequentie van 25 Hz.
Renate 37:ea621fdf306a 80 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.
Renate 37:ea621fdf306a 81 // Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
Renate 37:ea621fdf306a 82 // toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
Renate 21:456acc79726c 83 BiQuadChain bqck2;
Renate 37:ea621fdf306a 84 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.
Renate 37:ea621fdf306a 85 BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
Renate 20:a6a5bdd7d118 86
Renate 28:7c7508bdb21f 87 // VARIABELEN VOOR EMG + FILTEREN
Renate 37:ea621fdf306a 88 double filtered_EMG_biceps_right_1; // Definities voor ruwe EMG-signalen, gefilterd met de high-pass en notch filter.
Renate 37:ea621fdf306a 89 double filtered_EMG_biceps_left_1;
Renate 37:ea621fdf306a 90 double filtered_EMG_calf_1;
Renate 37:ea621fdf306a 91
Renate 37:ea621fdf306a 92 double filtered_EMG_biceps_right_abs; // Definities voor de signalen, waarbij de absolute waarden genomen zijn van de eerste filterketen.
Renate 37:ea621fdf306a 93 double filtered_EMG_biceps_left_abs;
Renate 37:ea621fdf306a 94 double filtered_EMG_calf_abs;
Renate 37:ea621fdf306a 95
Renate 37:ea621fdf306a 96 double filtered_EMG_biceps_right; // Definities voor de gefilterde EMG-signalen, na de tweede filter keten.
Renate 23:4572750a5c59 97 double filtered_EMG_biceps_left;
Renate 23:4572750a5c59 98 double filtered_EMG_calf;
Renate 23:4572750a5c59 99
Renate 23:4572750a5c59 100 // Variabelen voor HIDScope
Renate 37:ea621fdf306a 101 HIDScope scope(3); //
Renate 23:4572750a5c59 102
Renate 23:4572750a5c59 103 // VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP
Renate 28:7c7508bdb21f 104 bool calib = false;
Renate 23:4572750a5c59 105 static int i_calib = 0;
Renate 21:456acc79726c 106
Renate 37:ea621fdf306a 107 double filtered_EMG_biceps_right_total; // Benodigde variabelen voor het berekenen van een gemiddelde maximale EMG-waarde tijdens de EMG-kalibratie.
Renate 37:ea621fdf306a 108 double filtered_EMG_biceps_left_total; // Dit totaal is een sommatie van de signalen over 5 seconden.
Renate 37:ea621fdf306a 109 double filtered_EMG_calf_total;
Renate 37:ea621fdf306a 110
Renate 23:4572750a5c59 111 double mean_EMG_biceps_right;
Renate 23:4572750a5c59 112 double mean_EMG_biceps_left;
Renate 23:4572750a5c59 113 double mean_EMG_calf;
Renate 23:4572750a5c59 114
Renate 29:8e0a7c33e4e7 115 // VARIABELEN VOOR OPERATION MODE (EMG)
Renate 23:4572750a5c59 116 double normalized_EMG_biceps_right;
Renate 23:4572750a5c59 117 double normalized_EMG_biceps_left;
Renate 23:4572750a5c59 118 double normalized_EMG_calf;
Renate 23:4572750a5c59 119
Renate 29:8e0a7c33e4e7 120 // VARIABELEN VOOR OPERATION MODE (RKI)
Renate 37:ea621fdf306a 121 double vx; // Geeft de 'desired velocity' in x-richting
Renate 37:ea621fdf306a 122 double vy; // Geeft de 'desired velocity' in y-richting
Renate 37:ea621fdf306a 123
Renate 37:ea621fdf306a 124 double Inverse_jacobian[2][2];
Renate 37:ea621fdf306a 125 double desired_velocity[2][1];
Renate 29:8e0a7c33e4e7 126
Renate 29:8e0a7c33e4e7 127 const double delta_t = 0.002;
Renate 29:8e0a7c33e4e7 128
Renate 37:ea621fdf306a 129 double Joint_1_position = 0.0;
Renate 37:ea621fdf306a 130 double Joint_2_position = 0.0;
Renate 30:0a328a9a4788 131
Renate 37:ea621fdf306a 132 double Joint_1_position_prev = 0.0;
Renate 37:ea621fdf306a 133 double Joint_2_position_prev = 0.0;
Renate 29:8e0a7c33e4e7 134
Renate 37:ea621fdf306a 135 double Joint_velocity[2][1] = {{0.0}, {0.0}};
Renate 29:8e0a7c33e4e7 136
Renate 37:ea621fdf306a 137 double q1_dot;
Renate 37:ea621fdf306a 138 double q2_dot;
Renate 29:8e0a7c33e4e7 139
Renate 29:8e0a7c33e4e7 140 double q1;
Renate 29:8e0a7c33e4e7 141 double q2;
Renate 29:8e0a7c33e4e7 142
Renate 37:ea621fdf306a 143 double Motor_1_position = 0.0;
Renate 37:ea621fdf306a 144 double Motor_2_position = 0.0;
Renate 30:0a328a9a4788 145
Renate 30:0a328a9a4788 146 // VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
Renate 29:8e0a7c33e4e7 147
Renate 37:ea621fdf306a 148 const double Kp = 12.5;
Renate 32:d651c23bbb77 149 const double Ki = 0.03;
Renate 30:0a328a9a4788 150
Renate 37:ea621fdf306a 151 double theta_k_1 = 0.0;
Renate 37:ea621fdf306a 152 double theta_k_2 = 0.0;
Renate 29:8e0a7c33e4e7 153
Renate 37:ea621fdf306a 154 double error_integral_1 = 0.0;
Renate 37:ea621fdf306a 155 double error_integral_2 = 0.0;
Renate 31:967b455bc328 156
Renate 37:ea621fdf306a 157 double u_i_1;
Renate 37:ea621fdf306a 158 double u_i_2;
Renate 32:d651c23bbb77 159
Renate 37:ea621fdf306a 160 double theta_t_1;
Renate 37:ea621fdf306a 161 double theta_t_2;
Renate 30:0a328a9a4788 162
Renate 37:ea621fdf306a 163 double error_M1;
Renate 37:ea621fdf306a 164 double error_M2;
Renate 31:967b455bc328 165
Renate 37:ea621fdf306a 166 double abs_theta_t_1;
Renate 37:ea621fdf306a 167 double abs_theta_t_2;
Renate 29:8e0a7c33e4e7 168
Renate 23:4572750a5c59 169 // VOIDS
Renate 23:4572750a5c59 170
Renate 23:4572750a5c59 171 // Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden).
Renate 28:7c7508bdb21f 172 // Enige optie is resetten, dan wordt het script opnieuw opgestart.
Renate 8:c7d3b67346db 173 void emergency()
Renate 28:7c7508bdb21f 174 {
Renate 28:7c7508bdb21f 175 loop_ticker.detach();
Renate 28:7c7508bdb21f 176 motor1.write(0);
Renate 28:7c7508bdb21f 177 motor2.write(0);
Renate 28:7c7508bdb21f 178 pc.printf("Ik ga exploderen!!!\r\n");
Renate 28:7c7508bdb21f 179 }
Renate 11:4bc0304978e2 180
Renate 28:7c7508bdb21f 181 // Motoren uitzetten
Renate 8:c7d3b67346db 182 void motors_off()
Renate 28:7c7508bdb21f 183 {
Renate 28:7c7508bdb21f 184 motor1.write(0);
Renate 28:7c7508bdb21f 185 motor2.write(0);
Renate 28:7c7508bdb21f 186 pc.printf("Motoren uit functie\r\n");
Renate 28:7c7508bdb21f 187 }
Renate 28:7c7508bdb21f 188
Renate 37:ea621fdf306a 189 void EMG_calibration()
Renate 37:ea621fdf306a 190 {
Renate 37:ea621fdf306a 191 if (i_calib == 0) {
Renate 37:ea621fdf306a 192 filtered_EMG_biceps_right_total=0;
Renate 37:ea621fdf306a 193 filtered_EMG_biceps_left_total=0;
Renate 37:ea621fdf306a 194 filtered_EMG_calf_total=0;
Renate 37:ea621fdf306a 195 }
Renate 37:ea621fdf306a 196 if (i_calib <= 2500) {
Renate 37:ea621fdf306a 197 filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
Renate 37:ea621fdf306a 198 filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
Renate 37:ea621fdf306a 199 filtered_EMG_calf_total+=filtered_EMG_calf;
Renate 37:ea621fdf306a 200 i_calib++;
Renate 37:ea621fdf306a 201 }
Renate 37:ea621fdf306a 202 if (i_calib > 2500) {
Renate 37:ea621fdf306a 203 mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
Renate 37:ea621fdf306a 204 mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
Renate 37:ea621fdf306a 205 mean_EMG_calf=filtered_EMG_calf_total/2500.0;
Renate 37:ea621fdf306a 206 pc.printf("Ontspan spieren\r\n");
Renate 37:ea621fdf306a 207 pc.printf("Rechterbiceps_max = %f, Linkerbiceps_max = %f, Kuit_max = %f\r\n", mean_EMG_biceps_right, mean_EMG_biceps_left, mean_EMG_calf);
Renate 37:ea621fdf306a 208 calib = false;
Renate 37:ea621fdf306a 209 }
Renate 37:ea621fdf306a 210 }
Renate 37:ea621fdf306a 211
Renate 37:ea621fdf306a 212 void Homing_function()
Renate 28:7c7508bdb21f 213 {
Renate 37:ea621fdf306a 214 if (theta_h_1_rad != 0.0) {
Renate 37:ea621fdf306a 215 if (theta_h_1_rad < 0) {
Renate 37:ea621fdf306a 216 motor1.write(0.3);
Renate 37:ea621fdf306a 217 motor1_dir.write(0);
Renate 37:ea621fdf306a 218 } else {
Renate 37:ea621fdf306a 219 motor1.write(0.3);
Renate 37:ea621fdf306a 220 motor1_dir.write(1);
Renate 37:ea621fdf306a 221 }
Renate 37:ea621fdf306a 222 }
Renate 37:ea621fdf306a 223 if (theta_h_1_rad == 0.0) {
Renate 37:ea621fdf306a 224 motor1.write(0);
Renate 37:ea621fdf306a 225 }
Renate 37:ea621fdf306a 226 if (theta_h_2_rad != 0.0) {
Renate 37:ea621fdf306a 227 if (theta_h_2_rad < 0) {
Renate 37:ea621fdf306a 228 motor2.write(0.3);
Renate 37:ea621fdf306a 229 motor2_dir.write(0);
Renate 37:ea621fdf306a 230 } else {
Renate 37:ea621fdf306a 231 motor2.write(0.3);
Renate 37:ea621fdf306a 232 motor2_dir.write(1);
Renate 37:ea621fdf306a 233 }
Renate 37:ea621fdf306a 234 }
Renate 37:ea621fdf306a 235 if (theta_h_2_rad == 0.0) {
Renate 37:ea621fdf306a 236 motor2.write(0);
Renate 37:ea621fdf306a 237 }
Renate 28:7c7508bdb21f 238 }
Rosalie 3:6ee0b20c23b0 239
Renate 29:8e0a7c33e4e7 240 void Inverse_Kinematics()
Renate 29:8e0a7c33e4e7 241 {
Renate 30:0a328a9a4788 242 // Defining joint velocities based on calculations of matlab
Renate 37:ea621fdf306a 243 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);
Renate 37:ea621fdf306a 244 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);
Renate 37:ea621fdf306a 245 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);
Renate 37:ea621fdf306a 246 Inverse_jacobian[1][1] = ((cos(q1)*4.25E2-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);
Renate 29:8e0a7c33e4e7 247
Renate 37:ea621fdf306a 248 desired_velocity[0][0] = vx;
Renate 37:ea621fdf306a 249 desired_velocity[1][0] = vy;
Renate 37:ea621fdf306a 250
Renate 37:ea621fdf306a 251 Joint_velocity[0][0] = Inverse_jacobian[0][0]*desired_velocity[0][0] + Inverse_jacobian[0][1]*desired_velocity[1][0];
Renate 37:ea621fdf306a 252 Joint_velocity[1][0] = Inverse_jacobian[1][0]*desired_velocity[0][0] + Inverse_jacobian[1][1]*desired_velocity[1][0];
Renate 29:8e0a7c33e4e7 253
Renate 29:8e0a7c33e4e7 254 // Integratie
Renate 37:ea621fdf306a 255 Joint_1_position = Joint_1_position_prev + Joint_velocity[0][0]*delta_t;
Renate 37:ea621fdf306a 256 Joint_2_position = Joint_2_position_prev + Joint_velocity[1][0]*delta_t;
Renate 30:0a328a9a4788 257
Renate 37:ea621fdf306a 258 Joint_1_position_prev = Joint_1_position;
Renate 37:ea621fdf306a 259 Joint_2_position_prev = Joint_2_position;
Renate 30:0a328a9a4788 260
Renate 37:ea621fdf306a 261 Motor_1_position = Joint_1_position;
Renate 37:ea621fdf306a 262 Motor_2_position = Joint_1_position + Joint_2_position;
Renate 30:0a328a9a4788 263 }
Renate 29:8e0a7c33e4e7 264
Renate 37:ea621fdf306a 265 // PI-CONTROLLER
Renate 37:ea621fdf306a 266 void PI_controller()
Renate 31:967b455bc328 267 {
Renate 31:967b455bc328 268 // Proportional part:
Renate 37:ea621fdf306a 269 theta_k_1= Kp * error_M1;
Renate 37:ea621fdf306a 270 theta_k_2= Kp * error_M2;
Renate 31:967b455bc328 271
Renate 31:967b455bc328 272 // Integral part
Renate 37:ea621fdf306a 273 error_integral_1 = error_integral_1+ error_M1*delta_t;
Renate 37:ea621fdf306a 274 error_integral_2 = error_integral_2+ error_M2*delta_t;
Renate 37:ea621fdf306a 275 u_i_1= Ki * error_integral_1;
Renate 37:ea621fdf306a 276 u_i_2= Ki * error_integral_2;
Renate 31:967b455bc328 277
Renate 37:ea621fdf306a 278 // Sum all parts and return it
Renate 37:ea621fdf306a 279 theta_t_1= theta_k_1 + u_i_1;
Renate 37:ea621fdf306a 280 theta_t_2= theta_k_2 + u_i_2;
Renate 31:967b455bc328 281 }
Renate 31:967b455bc328 282
Renate 37:ea621fdf306a 283 void Define_motor_dir()
Renate 30:0a328a9a4788 284 {
Renate 37:ea621fdf306a 285 if (theta_t_1 >= 0 && theta_t_2 >= 0) {
Renate 37:ea621fdf306a 286 motor1_dir.write(0);
Renate 37:ea621fdf306a 287 motor2_dir.write(0);
Renate 37:ea621fdf306a 288 }
Renate 37:ea621fdf306a 289 if (theta_t_1 < 0 && theta_t_2 >= 0) {
Renate 37:ea621fdf306a 290 motor1_dir.write(1);
Renate 37:ea621fdf306a 291 motor1_dir.write(0);
Renate 37:ea621fdf306a 292 }
Renate 37:ea621fdf306a 293 if (theta_t_1 >= 0 && theta_t_2 < 0) {
Renate 32:d651c23bbb77 294 motor1_dir.write(0);
Renate 32:d651c23bbb77 295 motor2_dir.write(1);
Renate 32:d651c23bbb77 296 } else {
Renate 32:d651c23bbb77 297 motor1_dir.write(1);
Renate 37:ea621fdf306a 298 motor2_dir.write(1);
Renate 32:d651c23bbb77 299 }
Renate 32:d651c23bbb77 300 }
Renate 32:d651c23bbb77 301
Renate 37:ea621fdf306a 302 void Controlling_system()
Renate 31:967b455bc328 303 {
Renate 37:ea621fdf306a 304 Inverse_Kinematics();
Renate 37:ea621fdf306a 305
Renate 37:ea621fdf306a 306 error_M1 = Motor_1_position + theta_h_1_rad;
Renate 37:ea621fdf306a 307 error_M2 = Motor_2_position + theta_h_2_rad;
Renate 37:ea621fdf306a 308
Renate 37:ea621fdf306a 309 PI_controller();
Renate 37:ea621fdf306a 310
Renate 37:ea621fdf306a 311 abs_theta_t_1 = abs(theta_t_1);
Renate 37:ea621fdf306a 312 abs_theta_t_2 = abs(theta_t_2);
Renate 37:ea621fdf306a 313
Renate 37:ea621fdf306a 314 motor1.write(abs_theta_t_1);
Renate 37:ea621fdf306a 315 motor2.write(abs_theta_t_2);
Renate 37:ea621fdf306a 316 Define_motor_dir();
Renate 31:967b455bc328 317 }
Renate 37:ea621fdf306a 318
Renate 37:ea621fdf306a 319 // Aanmaken van een bool om te testen of de berekeningen in the ProcessStatemachine
Renate 37:ea621fdf306a 320 // meer tijd kosten dan wordt gegeven door de ticker. Dit zou mogelijk het encoder
Renate 37:ea621fdf306a 321 // probleem kunnen verklaren. Indien er te weinig tijd is, zou de loop zichzelf in moeten
Renate 37:ea621fdf306a 322 // halen. Start met een bool die true is, stel deze gelijk aan false in het begin van de loop
Renate 37:ea621fdf306a 323 // en verander deze weer in true wanneer de hele loop voltooid is. In het geval dat de loop zichzelf
Renate 37:ea621fdf306a 324 // inhaalt, blijft de bool false en wordt een string (There is a timing problem) geprint.
Renate 37:ea621fdf306a 325 // RESULTAAT: de string wordt niet geprint, er zouden geen timing issues moeten zijn.
Renate 37:ea621fdf306a 326 // Het script spreekt zichzelf dus tegen, experts komen ook niet uit dit probleem.
Renate 37:ea621fdf306a 327
Renate 37:ea621fdf306a 328 volatile bool loop_done = true;
Renate 37:ea621fdf306a 329
Renate 6:64146e16e10c 330 // Finite state machine programming (calibration servo motor?)
Renate 28:7c7508bdb21f 331 void ProcessStateMachine(void)
Renate 28:7c7508bdb21f 332 {
Renate 37:ea621fdf306a 333 if (!loop_done) {
Renate 37:ea621fdf306a 334 pc.printf("There is a timing problem\r\n");
Renate 37:ea621fdf306a 335
Renate 37:ea621fdf306a 336 return;
Renate 37:ea621fdf306a 337 }
Renate 37:ea621fdf306a 338
Renate 37:ea621fdf306a 339 loop_done = false;
Renate 37:ea621fdf306a 340
Renate 23:4572750a5c59 341 // Berekenen van de motorhoeken (in radialen)
Renate 37:ea621fdf306a 342 counts1 = Encoder1.getPulses();
Renate 37:ea621fdf306a 343 counts2 = Encoder2.getPulses();
Renate 37:ea621fdf306a 344 theta_h_1_rad = conversion_factor*(counts1-offset1);
Renate 37:ea621fdf306a 345 theta_h_2_rad = conversion_factor*(counts2-offset2);
Renate 28:7c7508bdb21f 346
Renate 29:8e0a7c33e4e7 347 // Calculating joint angles based on motor angles (current encoder values)
Renate 29:8e0a7c33e4e7 348 q1 = theta_h_1_rad; // Relative angle joint 1 (rad)
Renate 29:8e0a7c33e4e7 349 q2 = theta_h_2_rad - theta_h_1_rad; // Relative angle joint 2 (rad)
Renate 29:8e0a7c33e4e7 350
Renate 23:4572750a5c59 351 // Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal
Renate 28:7c7508bdb21f 352 // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled'
Renate 23:4572750a5c59 353 filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
Renate 23:4572750a5c59 354 filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
Renate 23:4572750a5c59 355 filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
Renate 28:7c7508bdb21f 356
Renate 23:4572750a5c59 357 // Vervolgens wordt de absolute waarde hiervan genomen
Renate 23:4572750a5c59 358 filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
Renate 23:4572750a5c59 359 filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
Renate 23:4572750a5c59 360 filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
Renate 28:7c7508bdb21f 361
Renate 23:4572750a5c59 362 // Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter)
Renate 23:4572750a5c59 363 // over het signaal gedaan
Renate 23:4572750a5c59 364 filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
Renate 23:4572750a5c59 365 filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
Renate 23:4572750a5c59 366 filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
Renate 28:7c7508bdb21f 367
Renate 28:7c7508bdb21f 368 // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope
Renate 28:7c7508bdb21f 369 scope.set(0, normalized_EMG_biceps_right);
Renate 28:7c7508bdb21f 370 scope.set(1, normalized_EMG_biceps_left);
Renate 28:7c7508bdb21f 371 scope.set(2, normalized_EMG_calf);
Renate 23:4572750a5c59 372 scope.send();
Renate 28:7c7508bdb21f 373
Renate 28:7c7508bdb21f 374 // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die
Renate 28:7c7508bdb21f 375 // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende
Renate 23:4572750a5c59 376 // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
Renate 28:7c7508bdb21f 377 // waarna het gemiddelde wordt bepaald.
Renate 28:7c7508bdb21f 378 if (calib) {
Renate 37:ea621fdf306a 379
Renate 37:ea621fdf306a 380 EMG_calibration();
Renate 37:ea621fdf306a 381
Renate 28:7c7508bdb21f 382 }
Renate 28:7c7508bdb21f 383
Renate 23:4572750a5c59 384 // Genormaliseerde EMG's berekenen
Renate 23:4572750a5c59 385 normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
Renate 23:4572750a5c59 386 normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
Renate 23:4572750a5c59 387 normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
Renate 28:7c7508bdb21f 388
Renate 28:7c7508bdb21f 389 // Finite state machine
Renate 28:7c7508bdb21f 390 switch (currentState) {
Renate 6:64146e16e10c 391 case Motors_off:
Renate 28:7c7508bdb21f 392
Renate 28:7c7508bdb21f 393 if (stateChanged) {
Renate 8:c7d3b67346db 394 motors_off(); // functie waarbij motoren uitgaan
Renate 11:4bc0304978e2 395 stateChanged = false;
Renate 9:4de589636f50 396 pc.printf("Motors off state\r\n");
Renate 28:7c7508bdb21f 397 }
Renate 29:8e0a7c33e4e7 398 if (Emergency_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 29:8e0a7c33e4e7 399 emergency();
Renate 29:8e0a7c33e4e7 400 }
Renate 28:7c7508bdb21f 401 if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 9:4de589636f50 402 currentState = Calib_motor;
Renate 11:4bc0304978e2 403 stateChanged = true;
Renate 11:4bc0304978e2 404 pc.printf("Moving to Calib_motor state\r\n");
Renate 6:64146e16e10c 405 }
Renate 6:64146e16e10c 406 break;
Renate 28:7c7508bdb21f 407
Renate 9:4de589636f50 408 case Calib_motor:
Renate 28:7c7508bdb21f 409
Renate 29:8e0a7c33e4e7 410 if (stateChanged) {
Renate 29:8e0a7c33e4e7 411 pc.printf("Zet motoren handmatig in home positie\r\n");
Renate 29:8e0a7c33e4e7 412 stateChanged = false;
Renate 29:8e0a7c33e4e7 413 }
Renate 29:8e0a7c33e4e7 414
Renate 29:8e0a7c33e4e7 415 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 416 emergency();
Renate 29:8e0a7c33e4e7 417 }
Renate 29:8e0a7c33e4e7 418 if (Motor_calib_button_pressed.read() == false) {
Renate 37:ea621fdf306a 419 offset1 = counts1;
Renate 37:ea621fdf306a 420 offset2 = counts2;
Renate 21:456acc79726c 421 pc.printf("Huidige hoek in radialen motor 1:%f en motor 2: %f (moet 0 zijn) \r\n", theta_h_1_rad, theta_h_2_rad);
Renate 11:4bc0304978e2 422 currentState = Calib_EMG;
Renate 11:4bc0304978e2 423 stateChanged = true;
Renate 9:4de589636f50 424 pc.printf("Moving to Calib_EMG state\r\n");
Renate 28:7c7508bdb21f 425 }
Renate 11:4bc0304978e2 426 break;
Renate 28:7c7508bdb21f 427
Renate 28:7c7508bdb21f 428 case Calib_EMG:
Renate 28:7c7508bdb21f 429
Renate 28:7c7508bdb21f 430 if (stateChanged) {
Renate 28:7c7508bdb21f 431 i_calib = 0;
Renate 28:7c7508bdb21f 432 calib = true;
Renate 28:7c7508bdb21f 433 pc.printf("Span spieren aan\r\n");
Renate 28:7c7508bdb21f 434 stateChanged = false;
Renate 28:7c7508bdb21f 435 }
Renate 28:7c7508bdb21f 436
Renate 29:8e0a7c33e4e7 437 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 438 emergency();
Renate 29:8e0a7c33e4e7 439 }
Renate 29:8e0a7c33e4e7 440
Renate 28:7c7508bdb21f 441 if (i_calib > 2500) {
Renate 28:7c7508bdb21f 442 calib = false;
Renate 28:7c7508bdb21f 443 currentState = Homing;
Renate 28:7c7508bdb21f 444 stateChanged = true;
Renate 28:7c7508bdb21f 445 pc.printf("Moving to Homing state\r\n");
Renate 28:7c7508bdb21f 446 }
Renate 28:7c7508bdb21f 447 break;
Renate 28:7c7508bdb21f 448
Renate 37:ea621fdf306a 449 case Homing:
Renate 28:7c7508bdb21f 450
Renate 28:7c7508bdb21f 451 if (stateChanged) {
Renate 28:7c7508bdb21f 452 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
Renate 11:4bc0304978e2 453 // (inclusief de end-effector) in de juiste home positie wordt gezet
Renate 28:7c7508bdb21f 454 stateChanged = false;
Renate 11:4bc0304978e2 455 }
Renate 28:7c7508bdb21f 456 if (Emergency_button_pressed.read() == false) {
Renate 10:83f3cec8dd1c 457 emergency();
Renate 28:7c7508bdb21f 458 }
Renate 29:8e0a7c33e4e7 459
Renate 37:ea621fdf306a 460 Homing_function();
Renate 29:8e0a7c33e4e7 461
Renate 37:ea621fdf306a 462 if (theta_h_1_rad == 0.0 && theta_h_2_rad == 0.0) {
Renate 28:7c7508bdb21f 463 currentState = Operation_mode;
Renate 28:7c7508bdb21f 464 stateChanged = true;
Renate 29:8e0a7c33e4e7 465 pc.printf("Moving to operation mode \r\n");
Renate 28:7c7508bdb21f 466 }
Renate 29:8e0a7c33e4e7 467
Renate 28:7c7508bdb21f 468 break;
Renate 28:7c7508bdb21f 469
Renate 37:ea621fdf306a 470 case Operation_mode:
Renate 28:7c7508bdb21f 471
Renate 29:8e0a7c33e4e7 472 if (stateChanged) {
Renate 29:8e0a7c33e4e7 473 motors_off();
Renate 37:ea621fdf306a 474 Joint_1_position = 0;
Renate 37:ea621fdf306a 475 Joint_2_position = 0;
Renate 37:ea621fdf306a 476 Joint_1_position_prev = Joint_1_position;
Renate 37:ea621fdf306a 477 Joint_2_position_prev = Joint_2_position;
Renate 37:ea621fdf306a 478 Joint_velocity[0][0] = 0;
Renate 37:ea621fdf306a 479 Joint_velocity[1][0] = 0;
Renate 37:ea621fdf306a 480 Motor_1_position = 0;
Renate 37:ea621fdf306a 481 Motor_2_position = 0;
Renate 37:ea621fdf306a 482 theta_k_1 = 0.0;
Renate 37:ea621fdf306a 483 theta_k_2 = 0.0;
Renate 37:ea621fdf306a 484 error_integral_1 = 0.0;
Renate 37:ea621fdf306a 485 error_integral_2 = 0.0;
Renate 29:8e0a7c33e4e7 486 stateChanged = false;
Renate 37:ea621fdf306a 487 pc.printf("einde operation mode init");
Renate 29:8e0a7c33e4e7 488 }
Renate 29:8e0a7c33e4e7 489 // Hier moet een functie worden aangeroepen die ervoor zorgt dat
Renate 29:8e0a7c33e4e7 490 // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
Renate 29:8e0a7c33e4e7 491 // zodat de robotarm kan bewegen
Renate 28:7c7508bdb21f 492
Renate 37:ea621fdf306a 493 if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 37:ea621fdf306a 494 motors_off();
Renate 37:ea621fdf306a 495 currentState = Motors_off;
Renate 37:ea621fdf306a 496 stateChanged = true;
Renate 37:ea621fdf306a 497 pc.printf("Terug naar de state Motors_off\r\n");
Renate 37:ea621fdf306a 498 }
Renate 29:8e0a7c33e4e7 499 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 500 emergency();
Renate 29:8e0a7c33e4e7 501 }
Renate 29:8e0a7c33e4e7 502 if (Motor_calib_button_pressed.read() == false) { // Is nu voor de homing
Renate 29:8e0a7c33e4e7 503 motors_off();
Renate 29:8e0a7c33e4e7 504 currentState = Homing;
Renate 29:8e0a7c33e4e7 505 stateChanged = true;
Renate 29:8e0a7c33e4e7 506 pc.printf("Terug naar de state Homing\r\n");
Renate 29:8e0a7c33e4e7 507 }
Renate 29:8e0a7c33e4e7 508 if (normalized_EMG_biceps_right >= 0.3) {
Renate 31:967b455bc328 509
Renate 32:d651c23bbb77 510 if (normalized_EMG_calf < 0.3) {
Renate 31:967b455bc328 511 vx = 0.0;
Renate 37:ea621fdf306a 512 vy = 0.02;
Renate 31:967b455bc328 513 }
Renate 32:d651c23bbb77 514 if (normalized_EMG_calf >= 0.3) {
Renate 31:967b455bc328 515 vx = 0.0;
Renate 37:ea621fdf306a 516 vy = -0.02;
Renate 31:967b455bc328 517 }
Renate 32:d651c23bbb77 518
Renate 37:ea621fdf306a 519 Controlling_system();
Renate 28:7c7508bdb21f 520
Renate 29:8e0a7c33e4e7 521 } else if (normalized_EMG_biceps_left >= 0.3) {
Renate 32:d651c23bbb77 522 if (normalized_EMG_calf < 0.3) {
Renate 32:d651c23bbb77 523 vx = 0.05;
Renate 32:d651c23bbb77 524 vy = 0.0;
Renate 32:d651c23bbb77 525 }
Renate 32:d651c23bbb77 526 if (normalized_EMG_calf >= 0.3) {
Renate 32:d651c23bbb77 527 vx = -0.05;
Renate 32:d651c23bbb77 528 vy = 0.0;
Renate 32:d651c23bbb77 529 }
Renate 31:967b455bc328 530
Renate 37:ea621fdf306a 531 Controlling_system();
Renate 32:d651c23bbb77 532
Renate 29:8e0a7c33e4e7 533 } else {
Renate 37:ea621fdf306a 534 vx = 0.0;
Renate 37:ea621fdf306a 535 vy = 0.0;
Renate 37:ea621fdf306a 536
Renate 37:ea621fdf306a 537 Controlling_system();
Renate 37:ea621fdf306a 538
Renate 28:7c7508bdb21f 539 }
Renate 21:456acc79726c 540 break;
Renate 28:7c7508bdb21f 541
Renate 7:1d57463393c6 542 default:
Renate 7:1d57463393c6 543 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 14:54343b9fd708 544 motors_off();
Renate 9:4de589636f50 545 pc.printf("Unknown or uninplemented state reached!\r\n");
Renate 28:7c7508bdb21f 546
WiesjeRoskamp 2:aee655d11b6d 547 }
Renate 37:ea621fdf306a 548 loop_done = true;
Renate 11:4bc0304978e2 549 }
WiesjeRoskamp 2:aee655d11b6d 550
Renate 8:c7d3b67346db 551 int main(void)
Renate 28:7c7508bdb21f 552 {
Renate 37:ea621fdf306a 553 pc.baud(115200);
Renate 37:ea621fdf306a 554
Renate 37:ea621fdf306a 555 motor1.period_us(56);
Renate 37:ea621fdf306a 556 motor2.period_us(56);
Renate 37:ea621fdf306a 557
Renate 28:7c7508bdb21f 558 pc.printf("Opstarten\r\n");
Renate 23:4572750a5c59 559
Renate 28:7c7508bdb21f 560 // Chain voor rechter biceps
Renate 28:7c7508bdb21f 561 bqcbr.add(&bqbr1).add(&bqbr2);
Renate 28:7c7508bdb21f 562 bqcbr2.add(&bqbr3).add(&bqbr4);
Renate 28:7c7508bdb21f 563 // Chain voor linker biceps
Renate 28:7c7508bdb21f 564 bqcbl.add(&bqbl1).add(&bqbl2);
Renate 28:7c7508bdb21f 565 bqcbl2.add(&bqbl3).add(&bqbl4);
Renate 28:7c7508bdb21f 566 // Chain voor kuit
Renate 28:7c7508bdb21f 567 bqck.add(&bqk1).add(&bqk2);
Renate 28:7c7508bdb21f 568 bqck2.add(&bqk3).add(&bqk4);
Renate 28:7c7508bdb21f 569
Renate 28:7c7508bdb21f 570 loop_ticker.attach(&ProcessStateMachine, 0.002f);
Renate 28:7c7508bdb21f 571
Renate 28:7c7508bdb21f 572 while(true) {
Renate 28:7c7508bdb21f 573 /* do nothing */
Renate 28:7c7508bdb21f 574 }
Renate 28:7c7508bdb21f 575 }