Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@39:37744ca57a58, 2019-11-05 (annotated)

Committer:
Renate
Date:
Tue Nov 05 12:25:04 2019 +0000
Revision:
39:37744ca57a58
Parent:
38:fb163733c147
Child:
40:b26d19d52d40
Derde deel aanvullingen

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 38:fb163733c147 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 38:fb163733c147 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 38:fb163733c147 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 38:fb163733c147 51 double q1; // Hoek joint 1 die volgt uit de actuele motorhoeken.
Renate 38:fb163733c147 52 double q2; // Hoek joint 2 die volgt uit de actuele motorhoeken.
Renate 21:456acc79726c 53
Renate 23:4572750a5c59 54 // DEFINITIES VOOR FILTERS
Renate 20:a6a5bdd7d118 55
Renate 21:456acc79726c 56 // BICEPS-RECHTS
Renate 37:ea621fdf306a 57 // Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
Renate 28:7c7508bdb21f 58 BiQuadChain bqcbr;
Renate 37:ea621fdf306a 59 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 60 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 38:fb163733c147 61 // Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
Renate 37:ea621fdf306a 62 // toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
Renate 21:456acc79726c 63 BiQuadChain bqcbr2;
Renate 37:ea621fdf306a 64 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 38:fb163733c147 65 BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
Renate 20:a6a5bdd7d118 66
Renate 21:456acc79726c 67 // BICEPS-LINKS
Renate 37:ea621fdf306a 68 // Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
Renate 28:7c7508bdb21f 69 BiQuadChain bqcbl;
Renate 37:ea621fdf306a 70 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 71 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 38:fb163733c147 72 // Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
Renate 37:ea621fdf306a 73 // toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
Renate 21:456acc79726c 74 BiQuadChain bqcbl2;
Renate 37:ea621fdf306a 75 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 38:fb163733c147 76 BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
Renate 21:456acc79726c 77
Renate 21:456acc79726c 78 // KUIT
Renate 37:ea621fdf306a 79 // Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
Renate 28:7c7508bdb21f 80 BiQuadChain bqck;
Renate 37:ea621fdf306a 81 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 82 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 38:fb163733c147 83 // Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden
Renate 37:ea621fdf306a 84 // toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
Renate 21:456acc79726c 85 BiQuadChain bqck2;
Renate 37:ea621fdf306a 86 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 38:fb163733c147 87 BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);
Renate 20:a6a5bdd7d118 88
Renate 28:7c7508bdb21f 89 // VARIABELEN VOOR EMG + FILTEREN
Renate 37:ea621fdf306a 90 double filtered_EMG_biceps_right_1; // Definities voor ruwe EMG-signalen, gefilterd met de high-pass en notch filter.
Renate 37:ea621fdf306a 91 double filtered_EMG_biceps_left_1;
Renate 37:ea621fdf306a 92 double filtered_EMG_calf_1;
Renate 37:ea621fdf306a 93
Renate 37:ea621fdf306a 94 double filtered_EMG_biceps_right_abs; // Definities voor de signalen, waarbij de absolute waarden genomen zijn van de eerste filterketen.
Renate 37:ea621fdf306a 95 double filtered_EMG_biceps_left_abs;
Renate 37:ea621fdf306a 96 double filtered_EMG_calf_abs;
Renate 37:ea621fdf306a 97
Renate 37:ea621fdf306a 98 double filtered_EMG_biceps_right; // Definities voor de gefilterde EMG-signalen, na de tweede filter keten.
Renate 23:4572750a5c59 99 double filtered_EMG_biceps_left;
Renate 23:4572750a5c59 100 double filtered_EMG_calf;
Renate 23:4572750a5c59 101
Renate 23:4572750a5c59 102 // Variabelen voor HIDScope
Renate 38:fb163733c147 103 HIDScope scope(3); // Op deze manier kunnen drie signalen worden weergegeven in HIDScope.
Renate 23:4572750a5c59 104
Renate 23:4572750a5c59 105 // VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP
Renate 38:fb163733c147 106 bool calib = false; // Benodigde bool en 'runner' (i_calib) voor het uitvoeren van de EMG-kalbratie.
Renate 23:4572750a5c59 107 static int i_calib = 0;
Renate 21:456acc79726c 108
Renate 37:ea621fdf306a 109 double filtered_EMG_biceps_right_total; // Benodigde variabelen voor het berekenen van een gemiddelde maximale EMG-waarde tijdens de EMG-kalibratie.
Renate 38:fb163733c147 110 double filtered_EMG_biceps_left_total; // Dit totaal is een sommatie van de signalen over 5 seconden.
Renate 37:ea621fdf306a 111 double filtered_EMG_calf_total;
Renate 37:ea621fdf306a 112
Renate 38:fb163733c147 113 double mean_EMG_biceps_right; // Global variables definiëren voor het gemiddelde maximale EMG-signaal, verkregen d.m.v. de EMG-kalibratie.
Renate 23:4572750a5c59 114 double mean_EMG_biceps_left;
Renate 23:4572750a5c59 115 double mean_EMG_calf;
Renate 23:4572750a5c59 116
Renate 29:8e0a7c33e4e7 117 // VARIABELEN VOOR OPERATION MODE (EMG)
Renate 38:fb163733c147 118 double normalized_EMG_biceps_right; // Global variables definiëren voor het normaliseren van het gemeten EMG-signaal naar het gemiddelde maximale EMG-signaal.
Renate 23:4572750a5c59 119 double normalized_EMG_biceps_left;
Renate 23:4572750a5c59 120 double normalized_EMG_calf;
Renate 23:4572750a5c59 121
Renate 29:8e0a7c33e4e7 122 // VARIABELEN VOOR OPERATION MODE (RKI)
Renate 38:fb163733c147 123 double vx; // Geeft de definitie voor de 'desired velocity' in x-richting.
Renate 38:fb163733c147 124 double vy; // Geeft de definitie voor de 'desired velocity' in y-richting.
Renate 37:ea621fdf306a 125
Renate 38:fb163733c147 126 double Inverse_jacobian[2][2]; // Matrixen opstellen voor de inverse jacobian en de gewenste snelheden (vx en vy).
Renate 37:ea621fdf306a 127 double desired_velocity[2][1];
Renate 29:8e0a7c33e4e7 128
Renate 38:fb163733c147 129 double Joint_velocity[2][1] = {{0.0}, {0.0}}; // Nulmatrix opstellen voor de joint snelheden, beginwaarden.
Renate 29:8e0a7c33e4e7 130
Renate 38:fb163733c147 131 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'.
Renate 38:fb163733c147 132
Renate 38:fb163733c147 133 double Joint_1_position = 0.0; // Begindefinities opstellen voor de joint en motor posities.
Renate 37:ea621fdf306a 134 double Joint_2_position = 0.0;
Renate 30:0a328a9a4788 135
Renate 37:ea621fdf306a 136 double Joint_1_position_prev = 0.0;
Renate 37:ea621fdf306a 137 double Joint_2_position_prev = 0.0;
Renate 29:8e0a7c33e4e7 138
Renate 37:ea621fdf306a 139 double Motor_1_position = 0.0;
Renate 37:ea621fdf306a 140 double Motor_2_position = 0.0;
Renate 30:0a328a9a4788 141
Renate 38:fb163733c147 142 double error_M1; // Definiëren van de motorerrors, het verschil tussen de daadwerkelijke hoek en de gewenste hoek.
Renate 38:fb163733c147 143 double error_M2;
Renate 29:8e0a7c33e4e7 144
Renate 38:fb163733c147 145 // VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
Renate 38:fb163733c147 146 const double Kp = 17.5; // Kp en Ki waarden voor de PI-controller definiëren.
Renate 38:fb163733c147 147 const double Ki = 1.02; // Zijn theoretisch getest met goede resultaten, in de praktijk konden geen goede tests worden verricht, doordat de
Renate 38:fb163733c147 148 // robot oncontroleerbaar gedrag vertoonde.
Renate 38:fb163733c147 149 double theta_k_1 = 0.0; // Begin definities opstellen voor de motorhoeken na de proportional part.
Renate 37:ea621fdf306a 150 double theta_k_2 = 0.0;
Renate 29:8e0a7c33e4e7 151
Renate 38:fb163733c147 152 double error_integral_1 = 0.0; // Begin definities opstellen voor de integralen van de errors.
Renate 37:ea621fdf306a 153 double error_integral_2 = 0.0;
Renate 31:967b455bc328 154
Renate 38:fb163733c147 155 double u_i_1; // Uitkomst variabelen definiëren voor de vermenigvuldiging van de error integraal met Ki.
Renate 37:ea621fdf306a 156 double u_i_2;
Renate 32:d651c23bbb77 157
Renate 38:fb163733c147 158 double theta_t_1; // Variabelen opstellen voor de sommatie van theta_k en u_i.
Renate 37:ea621fdf306a 159 double theta_t_2;
Renate 30:0a328a9a4788 160
Renate 38:fb163733c147 161 double abs_theta_t_1; // Variabele opstellen voor de absolute waarde van theta_t.
Renate 37:ea621fdf306a 162 double abs_theta_t_2;
Renate 29:8e0a7c33e4e7 163
Renate 23:4572750a5c59 164 // VOIDS
Renate 23:4572750a5c59 165
Renate 38:fb163733c147 166 void emergency() // Noodfunctie waarbij de motoren uit worden gezet en de ProcessStateMachine wordt losgekoppeld
Renate 38:fb163733c147 167 { // van de ticker. De robot doet dan niks meer. De enige optie is om de mbed te resetten, waarna
Renate 38:fb163733c147 168 loop_ticker.detach(); // het script opnieuw moet worden opgestart.
Renate 28:7c7508bdb21f 169 motor1.write(0);
Renate 28:7c7508bdb21f 170 motor2.write(0);
Renate 28:7c7508bdb21f 171 pc.printf("Ik ga exploderen!!!\r\n");
Renate 28:7c7508bdb21f 172 }
Renate 38:fb163733c147 173
Renate 38:fb163733c147 174 void motors_off() // Functie waarbij beide motoren worden uitgezet.
Renate 28:7c7508bdb21f 175 {
Renate 28:7c7508bdb21f 176 motor1.write(0);
Renate 28:7c7508bdb21f 177 motor2.write(0);
Renate 28:7c7508bdb21f 178 pc.printf("Motoren uit functie\r\n");
Renate 28:7c7508bdb21f 179 }
Renate 28:7c7508bdb21f 180
Renate 38:fb163733c147 181 void EMG_calibration() // Functie die wordt uitgevoerd tijdens de EMG kalibratie. Wordt geinitialiseerd door de bool calib,
Renate 38:fb163733c147 182 { // die alleen waar is tijdens de EMG kalibratie. Er wordt gebruikgemaakt van een runner i_calib, zodat
Renate 38:fb163733c147 183 if (i_calib == 0) { // EMG-waarden tijdens maximale spierspanning gedurende 5 seconden bij elkaar op worden geteld. Het gemiddelde
Renate 38:fb163733c147 184 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.
Renate 37:ea621fdf306a 185 filtered_EMG_biceps_left_total=0;
Renate 37:ea621fdf306a 186 filtered_EMG_calf_total=0;
Renate 37:ea621fdf306a 187 }
Renate 37:ea621fdf306a 188 if (i_calib <= 2500) {
Renate 37:ea621fdf306a 189 filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
Renate 37:ea621fdf306a 190 filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
Renate 37:ea621fdf306a 191 filtered_EMG_calf_total+=filtered_EMG_calf;
Renate 37:ea621fdf306a 192 i_calib++;
Renate 37:ea621fdf306a 193 }
Renate 37:ea621fdf306a 194 if (i_calib > 2500) {
Renate 37:ea621fdf306a 195 mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
Renate 37:ea621fdf306a 196 mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
Renate 37:ea621fdf306a 197 mean_EMG_calf=filtered_EMG_calf_total/2500.0;
Renate 37:ea621fdf306a 198 pc.printf("Ontspan spieren\r\n");
Renate 37:ea621fdf306a 199 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 200 calib = false;
Renate 37:ea621fdf306a 201 }
Renate 37:ea621fdf306a 202 }
Renate 37:ea621fdf306a 203
Renate 38:fb163733c147 204 void Homing_function() // Functie die ervoor zorgt dat homing wordt uitgevoerd. De motoren worden met een lage snelheid bewogen, tot de motorhoeken
Renate 38:fb163733c147 205 { // weer in de referentie positie staan (0 in ons geval, doordat we compenseren met een offset voor encoder counts). Wanneer
Renate 38:fb163733c147 206 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,
Renate 38:fb163733c147 207 if (theta_h_1_rad < 0) { // wanneer dit voor beide motoren is gebeurd.
Renate 37:ea621fdf306a 208 motor1.write(0.3);
Renate 37:ea621fdf306a 209 motor1_dir.write(0);
Renate 37:ea621fdf306a 210 } else {
Renate 37:ea621fdf306a 211 motor1.write(0.3);
Renate 37:ea621fdf306a 212 motor1_dir.write(1);
Renate 37:ea621fdf306a 213 }
Renate 37:ea621fdf306a 214 }
Renate 37:ea621fdf306a 215 if (theta_h_1_rad == 0.0) {
Renate 37:ea621fdf306a 216 motor1.write(0);
Renate 37:ea621fdf306a 217 }
Renate 37:ea621fdf306a 218 if (theta_h_2_rad != 0.0) {
Renate 37:ea621fdf306a 219 if (theta_h_2_rad < 0) {
Renate 37:ea621fdf306a 220 motor2.write(0.3);
Renate 37:ea621fdf306a 221 motor2_dir.write(0);
Renate 37:ea621fdf306a 222 } else {
Renate 37:ea621fdf306a 223 motor2.write(0.3);
Renate 37:ea621fdf306a 224 motor2_dir.write(1);
Renate 37:ea621fdf306a 225 }
Renate 37:ea621fdf306a 226 }
Renate 37:ea621fdf306a 227 if (theta_h_2_rad == 0.0) {
Renate 37:ea621fdf306a 228 motor2.write(0);
Renate 37:ea621fdf306a 229 }
Renate 28:7c7508bdb21f 230 }
Rosalie 3:6ee0b20c23b0 231
Renate 38:fb163733c147 232 void Inverse_Kinematics() // Functie die de inverse kinematica uitvoert. De inverse Jacobian is afkosmtig uit eerdere berekeningen in Matlab.
Renate 38:fb163733c147 233 { // The desired velocity komt voort uit de operation mode, waar hier een waarde aan wordt toegekend.
Renate 38:fb163733c147 234 // 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.
Renate 37:ea621fdf306a 235 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 236 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 237 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 238 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 239
Renate 37:ea621fdf306a 240 desired_velocity[0][0] = vx;
Renate 37:ea621fdf306a 241 desired_velocity[1][0] = vy;
Renate 37:ea621fdf306a 242
Renate 37:ea621fdf306a 243 Joint_velocity[0][0] = Inverse_jacobian[0][0]*desired_velocity[0][0] + Inverse_jacobian[0][1]*desired_velocity[1][0];
Renate 37:ea621fdf306a 244 Joint_velocity[1][0] = Inverse_jacobian[1][0]*desired_velocity[0][0] + Inverse_jacobian[1][1]*desired_velocity[1][0];
Renate 29:8e0a7c33e4e7 245
Renate 38:fb163733c147 246 // Integration
Renate 37:ea621fdf306a 247 Joint_1_position = Joint_1_position_prev + Joint_velocity[0][0]*delta_t;
Renate 37:ea621fdf306a 248 Joint_2_position = Joint_2_position_prev + Joint_velocity[1][0]*delta_t;
Renate 30:0a328a9a4788 249
Renate 37:ea621fdf306a 250 Joint_1_position_prev = Joint_1_position;
Renate 37:ea621fdf306a 251 Joint_2_position_prev = Joint_2_position;
Renate 30:0a328a9a4788 252
Renate 37:ea621fdf306a 253 Motor_1_position = Joint_1_position;
Renate 37:ea621fdf306a 254 Motor_2_position = Joint_1_position + Joint_2_position;
Renate 30:0a328a9a4788 255 }
Renate 29:8e0a7c33e4e7 256
Renate 38:fb163733c147 257 void PI_controller() // De PI-controller zorgt ervoor dat het proportionele en het integrale aandeel worden bepaald.
Renate 38:fb163733c147 258 { // Na sommatie van beide aandelen, wordt theta_t als output verkregen. Deze wordt later als input
Renate 38:fb163733c147 259 // Proportional part: // voor de motorsnelheden gebruikt.
Renate 37:ea621fdf306a 260 theta_k_1= Kp * error_M1;
Renate 37:ea621fdf306a 261 theta_k_2= Kp * error_M2;
Renate 31:967b455bc328 262
Renate 31:967b455bc328 263 // Integral part
Renate 37:ea621fdf306a 264 error_integral_1 = error_integral_1+ error_M1*delta_t;
Renate 37:ea621fdf306a 265 error_integral_2 = error_integral_2+ error_M2*delta_t;
Renate 37:ea621fdf306a 266 u_i_1= Ki * error_integral_1;
Renate 37:ea621fdf306a 267 u_i_2= Ki * error_integral_2;
Renate 31:967b455bc328 268
Renate 37:ea621fdf306a 269 // Sum all parts and return it
Renate 37:ea621fdf306a 270 theta_t_1= theta_k_1 + u_i_1;
Renate 37:ea621fdf306a 271 theta_t_2= theta_k_2 + u_i_2;
Renate 31:967b455bc328 272 }
Renate 31:967b455bc328 273
Renate 38:fb163733c147 274 void Define_motor_dir() // Deze functie bepaalt de output richtingen die aan de motoren worden gegeven tijdens de operation mode.
Renate 38:fb163733c147 275 { // Wanneer de input (wordt ook als snelheid gegeven aan de motoren) positief is, wordt de draairichting van
Renate 38:fb163733c147 276 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.
Renate 37:ea621fdf306a 277 motor1_dir.write(0);
Renate 37:ea621fdf306a 278 motor2_dir.write(0);
Renate 37:ea621fdf306a 279 }
Renate 37:ea621fdf306a 280 if (theta_t_1 < 0 && theta_t_2 >= 0) {
Renate 37:ea621fdf306a 281 motor1_dir.write(1);
Renate 37:ea621fdf306a 282 motor1_dir.write(0);
Renate 37:ea621fdf306a 283 }
Renate 37:ea621fdf306a 284 if (theta_t_1 >= 0 && theta_t_2 < 0) {
Renate 32:d651c23bbb77 285 motor1_dir.write(0);
Renate 32:d651c23bbb77 286 motor2_dir.write(1);
Renate 32:d651c23bbb77 287 } else {
Renate 32:d651c23bbb77 288 motor1_dir.write(1);
Renate 37:ea621fdf306a 289 motor2_dir.write(1);
Renate 32:d651c23bbb77 290 }
Renate 32:d651c23bbb77 291 }
Renate 32:d651c23bbb77 292
Renate 38:fb163733c147 293 void Controlling_system() // Hoofdfunctie die wordt toegepast tijdens de operation mode. Een gewenste snelheid in x- en y-richting wordt
Renate 38:fb163733c147 294 { // hiervoor opgesteld en is vervolgens de input voor de inverse kinematica functie. Door middel van een gewenste
Renate 38:fb163733c147 295 Inverse_Kinematics(); // motorhoek en de actuele motorhoek wordt een error bepaald. Deze errors worden als input gebruikt voor de
Renate 38:fb163733c147 296 // PI-controller, wiens absolute output wordt gebruikt om de motorsnelheid aan te sturen. Tevens wordt een functie
Renate 38:fb163733c147 297 error_M1 = Motor_1_position + theta_h_1_rad; // aangeroepen die ervoor zorgt dat de motoren in de juiste richting gaan draaien.
Renate 37:ea621fdf306a 298 error_M2 = Motor_2_position + theta_h_2_rad;
Renate 37:ea621fdf306a 299
Renate 37:ea621fdf306a 300 PI_controller();
Renate 37:ea621fdf306a 301
Renate 37:ea621fdf306a 302 abs_theta_t_1 = abs(theta_t_1);
Renate 37:ea621fdf306a 303 abs_theta_t_2 = abs(theta_t_2);
Renate 37:ea621fdf306a 304
Renate 37:ea621fdf306a 305 motor1.write(abs_theta_t_1);
Renate 37:ea621fdf306a 306 motor2.write(abs_theta_t_2);
Renate 37:ea621fdf306a 307 Define_motor_dir();
Renate 31:967b455bc328 308 }
Renate 37:ea621fdf306a 309
Renate 37:ea621fdf306a 310 // Aanmaken van een bool om te testen of de berekeningen in the ProcessStatemachine
Renate 37:ea621fdf306a 311 // meer tijd kosten dan wordt gegeven door de ticker. Dit zou mogelijk het encoder
Renate 37:ea621fdf306a 312 // probleem kunnen verklaren. Indien er te weinig tijd is, zou de loop zichzelf in moeten
Renate 37:ea621fdf306a 313 // halen. Start met een bool die true is, stel deze gelijk aan false in het begin van de loop
Renate 37:ea621fdf306a 314 // en verander deze weer in true wanneer de hele loop voltooid is. In het geval dat de loop zichzelf
Renate 37:ea621fdf306a 315 // inhaalt, blijft de bool false en wordt een string (There is a timing problem) geprint.
Renate 37:ea621fdf306a 316 // RESULTAAT: de string wordt niet geprint, er zouden geen timing issues moeten zijn.
Renate 37:ea621fdf306a 317 // Het script spreekt zichzelf dus tegen, experts komen ook niet uit dit probleem.
Renate 37:ea621fdf306a 318
Renate 37:ea621fdf306a 319 volatile bool loop_done = true;
Renate 37:ea621fdf306a 320
Renate 6:64146e16e10c 321 // Finite state machine programming (calibration servo motor?)
Renate 28:7c7508bdb21f 322 void ProcessStateMachine(void)
Renate 28:7c7508bdb21f 323 {
Renate 37:ea621fdf306a 324 if (!loop_done) {
Renate 37:ea621fdf306a 325 pc.printf("There is a timing problem\r\n");
Renate 37:ea621fdf306a 326
Renate 37:ea621fdf306a 327 return;
Renate 37:ea621fdf306a 328 }
Renate 37:ea621fdf306a 329
Renate 37:ea621fdf306a 330 loop_done = false;
Renate 37:ea621fdf306a 331
Renate 23:4572750a5c59 332 // Berekenen van de motorhoeken (in radialen)
Renate 37:ea621fdf306a 333 counts1 = Encoder1.getPulses();
Renate 37:ea621fdf306a 334 counts2 = Encoder2.getPulses();
Renate 37:ea621fdf306a 335 theta_h_1_rad = conversion_factor*(counts1-offset1);
Renate 37:ea621fdf306a 336 theta_h_2_rad = conversion_factor*(counts2-offset2);
Renate 28:7c7508bdb21f 337
Renate 29:8e0a7c33e4e7 338 // Calculating joint angles based on motor angles (current encoder values)
Renate 29:8e0a7c33e4e7 339 q1 = theta_h_1_rad; // Relative angle joint 1 (rad)
Renate 29:8e0a7c33e4e7 340 q2 = theta_h_2_rad - theta_h_1_rad; // Relative angle joint 2 (rad)
Renate 29:8e0a7c33e4e7 341
Renate 23:4572750a5c59 342 // Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal
Renate 28:7c7508bdb21f 343 // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled'
Renate 23:4572750a5c59 344 filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
Renate 23:4572750a5c59 345 filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
Renate 23:4572750a5c59 346 filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
Renate 28:7c7508bdb21f 347
Renate 23:4572750a5c59 348 // Vervolgens wordt de absolute waarde hiervan genomen
Renate 23:4572750a5c59 349 filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
Renate 23:4572750a5c59 350 filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
Renate 23:4572750a5c59 351 filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
Renate 28:7c7508bdb21f 352
Renate 23:4572750a5c59 353 // Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter)
Renate 23:4572750a5c59 354 // over het signaal gedaan
Renate 23:4572750a5c59 355 filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
Renate 23:4572750a5c59 356 filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
Renate 23:4572750a5c59 357 filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
Renate 28:7c7508bdb21f 358
Renate 28:7c7508bdb21f 359 // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope
Renate 28:7c7508bdb21f 360 scope.set(0, normalized_EMG_biceps_right);
Renate 28:7c7508bdb21f 361 scope.set(1, normalized_EMG_biceps_left);
Renate 28:7c7508bdb21f 362 scope.set(2, normalized_EMG_calf);
Renate 23:4572750a5c59 363 scope.send();
Renate 28:7c7508bdb21f 364
Renate 28:7c7508bdb21f 365 // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die
Renate 28:7c7508bdb21f 366 // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende
Renate 23:4572750a5c59 367 // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
Renate 28:7c7508bdb21f 368 // waarna het gemiddelde wordt bepaald.
Renate 28:7c7508bdb21f 369 if (calib) {
Renate 37:ea621fdf306a 370 EMG_calibration();
Renate 28:7c7508bdb21f 371 }
Renate 28:7c7508bdb21f 372
Renate 23:4572750a5c59 373 // Genormaliseerde EMG's berekenen
Renate 23:4572750a5c59 374 normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
Renate 23:4572750a5c59 375 normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
Renate 23:4572750a5c59 376 normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
Renate 28:7c7508bdb21f 377
Renate 28:7c7508bdb21f 378 // Finite state machine
Renate 28:7c7508bdb21f 379 switch (currentState) {
Renate 6:64146e16e10c 380 case Motors_off:
Renate 28:7c7508bdb21f 381
Renate 28:7c7508bdb21f 382 if (stateChanged) {
Renate 38:fb163733c147 383 motors_off(); // Functie waarbij motoren uitgaan.
Renate 11:4bc0304978e2 384 stateChanged = false;
Renate 9:4de589636f50 385 pc.printf("Motors off state\r\n");
Renate 28:7c7508bdb21f 386 }
Renate 38:fb163733c147 387 if (Emergency_button_pressed.read() == false) { // Normaal krijgt de button een waarde 1 bij indrukken, nu nul -> false.
Renate 38:fb163733c147 388 emergency(); // Bij het indrukken van de emergency button, wordt overgegaan op de emergency functie.
Renate 29:8e0a7c33e4e7 389 }
Renate 38:fb163733c147 390 if (Power_button_pressed.read() == false) { // Normaal krijgt de button een waarde 1 bij indrukken, nu nul -> false.
Renate 38:fb163733c147 391 currentState = Calib_motor; // Er wordt doorgegaan naar de volgende state, wanneer de power button wordt ingedrukt.
Renate 11:4bc0304978e2 392 stateChanged = true;
Renate 11:4bc0304978e2 393 pc.printf("Moving to Calib_motor state\r\n");
Renate 6:64146e16e10c 394 }
Renate 6:64146e16e10c 395 break;
Renate 28:7c7508bdb21f 396
Renate 39:37744ca57a58 397 case Calib_motor: // In deze state wordt de robot arm met de hand in de juiste referentie (home) positie geplaatst.
Renate 39:37744ca57a58 398 // Wanneer dit is gebeurd, wordt de motor kalibratie knop ingedrukt. De offset wordt dan ingesteld als
Renate 39:37744ca57a58 399 if (stateChanged) { // het huidige aantal counts. Door deze offset in de komende metingen af te trekken van het aantal counts
Renate 39:37744ca57a58 400 pc.printf("Zet motoren handmatig in home positie\r\n"); // op dat moment, wordt ervoor gezorgd dat de motorhoeken in de referentiepositie gelijk zijn aan nul.
Renate 29:8e0a7c33e4e7 401 stateChanged = false;
Renate 29:8e0a7c33e4e7 402 }
Renate 29:8e0a7c33e4e7 403
Renate 29:8e0a7c33e4e7 404 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 405 emergency();
Renate 29:8e0a7c33e4e7 406 }
Renate 29:8e0a7c33e4e7 407 if (Motor_calib_button_pressed.read() == false) {
Renate 37:ea621fdf306a 408 offset1 = counts1;
Renate 37:ea621fdf306a 409 offset2 = counts2;
Renate 11:4bc0304978e2 410 currentState = Calib_EMG;
Renate 11:4bc0304978e2 411 stateChanged = true;
Renate 9:4de589636f50 412 pc.printf("Moving to Calib_EMG state\r\n");
Renate 28:7c7508bdb21f 413 }
Renate 11:4bc0304978e2 414 break;
Renate 28:7c7508bdb21f 415
Renate 39:37744ca57a58 416 case Calib_EMG: // In deze state wordt een kalibratie uitgevoerd van de EMG-signalen. Hiervoor wordt een bool (calib)
Renate 39:37744ca57a58 417 // samen met een teller (i_calib) in werking gezet, die ervoor zorgt dat de eerder beschreven EMG-kalibratie
Renate 39:37744ca57a58 418 if (stateChanged) { // wordt doorlopen. De spieren worden tijdens de kalibratie gedurende 5 seconden maximaal aangespannen.
Renate 39:37744ca57a58 419 i_calib = 0; // Na deze 5 seconden wordt doorgegaan naar de volgende state. De output van deze kalibratie is een gemiddelde
Renate 39:37744ca57a58 420 calib = true; // waarde van het EMG-signaal tijdens het maximaal aanspannen.
Renate 28:7c7508bdb21f 421 pc.printf("Span spieren aan\r\n");
Renate 28:7c7508bdb21f 422 stateChanged = false;
Renate 28:7c7508bdb21f 423 }
Renate 28:7c7508bdb21f 424
Renate 29:8e0a7c33e4e7 425 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 426 emergency();
Renate 29:8e0a7c33e4e7 427 }
Renate 29:8e0a7c33e4e7 428
Renate 28:7c7508bdb21f 429 if (i_calib > 2500) {
Renate 28:7c7508bdb21f 430 calib = false;
Renate 28:7c7508bdb21f 431 currentState = Homing;
Renate 28:7c7508bdb21f 432 stateChanged = true;
Renate 28:7c7508bdb21f 433 pc.printf("Moving to Homing state\r\n");
Renate 28:7c7508bdb21f 434 }
Renate 28:7c7508bdb21f 435 break;
Renate 28:7c7508bdb21f 436
Renate 39:37744ca57a58 437 case Homing: // In deze state wordt de homing uitgevoerd. De motoren draaien hier op een langzame snelheid, todat de gewenste referentiepositie
Renate 39:37744ca57a58 438 // wordt bereikt. In dit geval worden de motorsnelheden op 0 gezet. Indien beide motoren de juiste posities hebben bereikt, wordt
Renate 39:37744ca57a58 439 if (stateChanged) { // overgegaan naar de volgende state. De eerste keer dat de ProcessStateMachine in de homing state belandt, is dit al het geval,
Renate 39:37744ca57a58 440 stateChanged = false; // waardoor gelijk doorgegaan kan worden naar de operation mode.
Renate 11:4bc0304978e2 441 }
Renate 28:7c7508bdb21f 442 if (Emergency_button_pressed.read() == false) {
Renate 10:83f3cec8dd1c 443 emergency();
Renate 28:7c7508bdb21f 444 }
Renate 29:8e0a7c33e4e7 445
Renate 37:ea621fdf306a 446 Homing_function();
Renate 29:8e0a7c33e4e7 447
Renate 37:ea621fdf306a 448 if (theta_h_1_rad == 0.0 && theta_h_2_rad == 0.0) {
Renate 28:7c7508bdb21f 449 currentState = Operation_mode;
Renate 28:7c7508bdb21f 450 stateChanged = true;
Renate 29:8e0a7c33e4e7 451 pc.printf("Moving to operation mode \r\n");
Renate 28:7c7508bdb21f 452 }
Renate 28:7c7508bdb21f 453 break;
Renate 28:7c7508bdb21f 454
Renate 37:ea621fdf306a 455 case Operation_mode:
Renate 28:7c7508bdb21f 456
Renate 39:37744ca57a58 457 if (stateChanged) { // Aan het begin van de operation mode, wordt aan allerlei variabelen een beginwaarde van 0 toegekend. Dit wordt een keer gedaan
Renate 39:37744ca57a58 458 motors_off(); // (stateChanged), waarna deze waarden verderop in deze state worden overschreven. In het geval dat er tussendoor teruggegaan is
Renate 39:37744ca57a58 459 Joint_1_position = 0; // naar een andere state
Renate 37:ea621fdf306a 460 Joint_2_position = 0;
Renate 37:ea621fdf306a 461 Joint_1_position_prev = Joint_1_position;
Renate 37:ea621fdf306a 462 Joint_2_position_prev = Joint_2_position;
Renate 37:ea621fdf306a 463 Joint_velocity[0][0] = 0;
Renate 37:ea621fdf306a 464 Joint_velocity[1][0] = 0;
Renate 37:ea621fdf306a 465 Motor_1_position = 0;
Renate 37:ea621fdf306a 466 Motor_2_position = 0;
Renate 37:ea621fdf306a 467 theta_k_1 = 0.0;
Renate 37:ea621fdf306a 468 theta_k_2 = 0.0;
Renate 37:ea621fdf306a 469 error_integral_1 = 0.0;
Renate 37:ea621fdf306a 470 error_integral_2 = 0.0;
Renate 29:8e0a7c33e4e7 471 stateChanged = false;
Renate 37:ea621fdf306a 472 pc.printf("einde operation mode init");
Renate 29:8e0a7c33e4e7 473 }
Renate 28:7c7508bdb21f 474
Renate 37:ea621fdf306a 475 if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 37:ea621fdf306a 476 motors_off();
Renate 37:ea621fdf306a 477 currentState = Motors_off;
Renate 37:ea621fdf306a 478 stateChanged = true;
Renate 37:ea621fdf306a 479 pc.printf("Terug naar de state Motors_off\r\n");
Renate 37:ea621fdf306a 480 }
Renate 29:8e0a7c33e4e7 481 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 482 emergency();
Renate 29:8e0a7c33e4e7 483 }
Renate 29:8e0a7c33e4e7 484 if (Motor_calib_button_pressed.read() == false) { // Is nu voor de homing
Renate 29:8e0a7c33e4e7 485 motors_off();
Renate 29:8e0a7c33e4e7 486 currentState = Homing;
Renate 29:8e0a7c33e4e7 487 stateChanged = true;
Renate 29:8e0a7c33e4e7 488 pc.printf("Terug naar de state Homing\r\n");
Renate 29:8e0a7c33e4e7 489 }
Renate 29:8e0a7c33e4e7 490 if (normalized_EMG_biceps_right >= 0.3) {
Renate 31:967b455bc328 491
Renate 32:d651c23bbb77 492 if (normalized_EMG_calf < 0.3) {
Renate 31:967b455bc328 493 vx = 0.0;
Renate 37:ea621fdf306a 494 vy = 0.02;
Renate 31:967b455bc328 495 }
Renate 32:d651c23bbb77 496 if (normalized_EMG_calf >= 0.3) {
Renate 31:967b455bc328 497 vx = 0.0;
Renate 37:ea621fdf306a 498 vy = -0.02;
Renate 31:967b455bc328 499 }
Renate 32:d651c23bbb77 500
Renate 37:ea621fdf306a 501 Controlling_system();
Renate 28:7c7508bdb21f 502
Renate 29:8e0a7c33e4e7 503 } else if (normalized_EMG_biceps_left >= 0.3) {
Renate 32:d651c23bbb77 504 if (normalized_EMG_calf < 0.3) {
Renate 32:d651c23bbb77 505 vx = 0.05;
Renate 32:d651c23bbb77 506 vy = 0.0;
Renate 32:d651c23bbb77 507 }
Renate 32:d651c23bbb77 508 if (normalized_EMG_calf >= 0.3) {
Renate 32:d651c23bbb77 509 vx = -0.05;
Renate 32:d651c23bbb77 510 vy = 0.0;
Renate 32:d651c23bbb77 511 }
Renate 31:967b455bc328 512
Renate 37:ea621fdf306a 513 Controlling_system();
Renate 32:d651c23bbb77 514
Renate 29:8e0a7c33e4e7 515 } else {
Renate 37:ea621fdf306a 516 vx = 0.0;
Renate 37:ea621fdf306a 517 vy = 0.0;
Renate 37:ea621fdf306a 518
Renate 37:ea621fdf306a 519 Controlling_system();
Renate 37:ea621fdf306a 520
Renate 28:7c7508bdb21f 521 }
Renate 21:456acc79726c 522 break;
Renate 28:7c7508bdb21f 523
Renate 7:1d57463393c6 524 default:
Renate 7:1d57463393c6 525 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 14:54343b9fd708 526 motors_off();
Renate 9:4de589636f50 527 pc.printf("Unknown or uninplemented state reached!\r\n");
Renate 28:7c7508bdb21f 528
WiesjeRoskamp 2:aee655d11b6d 529 }
Renate 37:ea621fdf306a 530 loop_done = true;
Renate 11:4bc0304978e2 531 }
WiesjeRoskamp 2:aee655d11b6d 532
Renate 8:c7d3b67346db 533 int main(void)
Renate 28:7c7508bdb21f 534 {
Renate 37:ea621fdf306a 535 pc.baud(115200);
Renate 37:ea621fdf306a 536
Renate 37:ea621fdf306a 537 motor1.period_us(56);
Renate 37:ea621fdf306a 538 motor2.period_us(56);
Renate 37:ea621fdf306a 539
Renate 28:7c7508bdb21f 540 pc.printf("Opstarten\r\n");
Renate 23:4572750a5c59 541
Renate 28:7c7508bdb21f 542 // Chain voor rechter biceps
Renate 28:7c7508bdb21f 543 bqcbr.add(&bqbr1).add(&bqbr2);
Renate 28:7c7508bdb21f 544 bqcbr2.add(&bqbr3).add(&bqbr4);
Renate 28:7c7508bdb21f 545 // Chain voor linker biceps
Renate 28:7c7508bdb21f 546 bqcbl.add(&bqbl1).add(&bqbl2);
Renate 28:7c7508bdb21f 547 bqcbl2.add(&bqbl3).add(&bqbl4);
Renate 28:7c7508bdb21f 548 // Chain voor kuit
Renate 28:7c7508bdb21f 549 bqck.add(&bqk1).add(&bqk2);
Renate 28:7c7508bdb21f 550 bqck2.add(&bqk3).add(&bqk4);
Renate 28:7c7508bdb21f 551
Renate 28:7c7508bdb21f 552 loop_ticker.attach(&ProcessStateMachine, 0.002f);
Renate 28:7c7508bdb21f 553
Renate 28:7c7508bdb21f 554 while(true) {
Renate 28:7c7508bdb21f 555 /* do nothing */
Renate 28:7c7508bdb21f 556 }
Renate 28:7c7508bdb21f 557 }