![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp@41:dcfe6c86e3f5, 2019-11-05 (annotated)
- Committer:
- Renate
- Date:
- Tue Nov 05 16:09:52 2019 +0000
- Revision:
- 41:dcfe6c86e3f5
- Parent:
- 40:b26d19d52d40
- Child:
- 42:ca4bbc3e0239
Deels al servo toegevoegd
Who changed what in which revision?
User | Revision | Line number | New 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 | 41:dcfe6c86e3f5 | 6 | #include "FastPWM.h" |
Renate | 32:d651c23bbb77 | 7 | #define M_PI 3.14159265358979323846 /* pi */ |
WiesjeRoskamp | 2:aee655d11b6d | 8 | #include <math.h> |
Renate | 41:dcfe6c86e3f5 | 9 | #include "Servo.h" |
Renate | 21:456acc79726c | 10 | #include <cmath> |
Renate | 41:dcfe6c86e3f5 | 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 | 41:dcfe6c86e3f5 | 27 | DigitalIn servo_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 | 41:dcfe6c86e3f5 | 43 | Servo mijnServo(D7); // Definitie voor de output naar de servo motor, benodigd voor het controleren van de spatelpositie. |
Renate | 41:dcfe6c86e3f5 | 44 | |
Renate | 23:4572750a5c59 | 45 | // VARIABELEN VOOR ENCODER, MOTORHOEK ETC. |
Renate | 38:fb163733c147 | 46 | double counts1; // Global variables definiëren voor het aantal counts dat uit de encoder komt en een begindefinitie voor |
Renate | 37:ea621fdf306a | 47 | double counts2; // de offset opstellen. |
Renate | 37:ea621fdf306a | 48 | double offset1 = 0.0; |
Renate | 37:ea621fdf306a | 49 | double offset2 = 0.0; |
Renate | 37:ea621fdf306a | 50 | 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 | 51 | double theta_h_1_rad; // Actuele motorhoek in radialen (motor 1). |
Renate | 37:ea621fdf306a | 52 | double theta_h_2_rad; // Actuele motorhoek in radialen (motor 2). |
Renate | 38:fb163733c147 | 53 | double q1; // Hoek joint 1 die volgt uit de actuele motorhoeken. |
Renate | 38:fb163733c147 | 54 | double q2; // Hoek joint 2 die volgt uit de actuele motorhoeken. |
Renate | 21:456acc79726c | 55 | |
Renate | 23:4572750a5c59 | 56 | // DEFINITIES VOOR FILTERS |
Renate | 20:a6a5bdd7d118 | 57 | |
Renate | 21:456acc79726c | 58 | // BICEPS-RECHTS |
Renate | 37:ea621fdf306a | 59 | // Definities voor eerste BiQuadChain (High-pass en Notch) opstellen |
Renate | 28:7c7508bdb21f | 60 | BiQuadChain bqcbr; |
Renate | 37:ea621fdf306a | 61 | 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 | 62 | 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 | 63 | // Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden |
Renate | 37:ea621fdf306a | 64 | // toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen): |
Renate | 21:456acc79726c | 65 | BiQuadChain bqcbr2; |
Renate | 37:ea621fdf306a | 66 | 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 | 67 | BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); |
Renate | 20:a6a5bdd7d118 | 68 | |
Renate | 21:456acc79726c | 69 | // BICEPS-LINKS |
Renate | 37:ea621fdf306a | 70 | // Definities voor eerste BiQuadChain (High-pass en Notch) opstellen |
Renate | 28:7c7508bdb21f | 71 | BiQuadChain bqcbl; |
Renate | 37:ea621fdf306a | 72 | 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 | 73 | 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 | 74 | // Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden |
Renate | 37:ea621fdf306a | 75 | // toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen): |
Renate | 21:456acc79726c | 76 | BiQuadChain bqcbl2; |
Renate | 37:ea621fdf306a | 77 | 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 | 78 | BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); |
Renate | 21:456acc79726c | 79 | |
Renate | 21:456acc79726c | 80 | // KUIT |
Renate | 37:ea621fdf306a | 81 | // Definities voor eerste BiQuadChain (High-pass en Notch) opstellen |
Renate | 28:7c7508bdb21f | 82 | BiQuadChain bqck; |
Renate | 37:ea621fdf306a | 83 | 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 | 84 | 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 | 85 | // Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden |
Renate | 37:ea621fdf306a | 86 | // toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen): |
Renate | 21:456acc79726c | 87 | BiQuadChain bqck2; |
Renate | 37:ea621fdf306a | 88 | 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 | 89 | BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); |
Renate | 20:a6a5bdd7d118 | 90 | |
Renate | 28:7c7508bdb21f | 91 | // VARIABELEN VOOR EMG + FILTEREN |
Renate | 37:ea621fdf306a | 92 | double filtered_EMG_biceps_right_1; // Definities voor ruwe EMG-signalen, gefilterd met de high-pass en notch filter. |
Renate | 37:ea621fdf306a | 93 | double filtered_EMG_biceps_left_1; |
Renate | 37:ea621fdf306a | 94 | double filtered_EMG_calf_1; |
Renate | 37:ea621fdf306a | 95 | |
Renate | 37:ea621fdf306a | 96 | double filtered_EMG_biceps_right_abs; // Definities voor de signalen, waarbij de absolute waarden genomen zijn van de eerste filterketen. |
Renate | 37:ea621fdf306a | 97 | double filtered_EMG_biceps_left_abs; |
Renate | 37:ea621fdf306a | 98 | double filtered_EMG_calf_abs; |
Renate | 37:ea621fdf306a | 99 | |
Renate | 37:ea621fdf306a | 100 | double filtered_EMG_biceps_right; // Definities voor de gefilterde EMG-signalen, na de tweede filter keten. |
Renate | 23:4572750a5c59 | 101 | double filtered_EMG_biceps_left; |
Renate | 23:4572750a5c59 | 102 | double filtered_EMG_calf; |
Renate | 23:4572750a5c59 | 103 | |
Renate | 23:4572750a5c59 | 104 | // Variabelen voor HIDScope |
Renate | 38:fb163733c147 | 105 | HIDScope scope(3); // Op deze manier kunnen drie signalen worden weergegeven in HIDScope. |
Renate | 23:4572750a5c59 | 106 | |
Renate | 23:4572750a5c59 | 107 | // VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP |
Renate | 38:fb163733c147 | 108 | bool calib = false; // Benodigde bool en 'runner' (i_calib) voor het uitvoeren van de EMG-kalbratie. |
Renate | 23:4572750a5c59 | 109 | static int i_calib = 0; |
Renate | 21:456acc79726c | 110 | |
Renate | 37:ea621fdf306a | 111 | double filtered_EMG_biceps_right_total; // Benodigde variabelen voor het berekenen van een gemiddelde maximale EMG-waarde tijdens de EMG-kalibratie. |
Renate | 38:fb163733c147 | 112 | double filtered_EMG_biceps_left_total; // Dit totaal is een sommatie van de signalen over 5 seconden. |
Renate | 37:ea621fdf306a | 113 | double filtered_EMG_calf_total; |
Renate | 37:ea621fdf306a | 114 | |
Renate | 38:fb163733c147 | 115 | 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 | 116 | double mean_EMG_biceps_left; |
Renate | 23:4572750a5c59 | 117 | double mean_EMG_calf; |
Renate | 23:4572750a5c59 | 118 | |
Renate | 29:8e0a7c33e4e7 | 119 | // VARIABELEN VOOR OPERATION MODE (EMG) |
Renate | 38:fb163733c147 | 120 | 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 | 121 | double normalized_EMG_biceps_left; |
Renate | 23:4572750a5c59 | 122 | double normalized_EMG_calf; |
Renate | 23:4572750a5c59 | 123 | |
Renate | 29:8e0a7c33e4e7 | 124 | // VARIABELEN VOOR OPERATION MODE (RKI) |
Renate | 38:fb163733c147 | 125 | double vx; // Geeft de definitie voor de 'desired velocity' in x-richting. |
Renate | 38:fb163733c147 | 126 | double vy; // Geeft de definitie voor de 'desired velocity' in y-richting. |
Renate | 37:ea621fdf306a | 127 | |
Renate | 38:fb163733c147 | 128 | double Inverse_jacobian[2][2]; // Matrixen opstellen voor de inverse jacobian en de gewenste snelheden (vx en vy). |
Renate | 37:ea621fdf306a | 129 | double desired_velocity[2][1]; |
Renate | 29:8e0a7c33e4e7 | 130 | |
Renate | 38:fb163733c147 | 131 | double Joint_velocity[2][1] = {{0.0}, {0.0}}; // Nulmatrix opstellen voor de joint snelheden, beginwaarden. |
Renate | 29:8e0a7c33e4e7 | 132 | |
Renate | 38:fb163733c147 | 133 | 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 | 134 | |
Renate | 38:fb163733c147 | 135 | double Joint_1_position = 0.0; // Begindefinities opstellen voor de joint en motor posities. |
Renate | 37:ea621fdf306a | 136 | double Joint_2_position = 0.0; |
Renate | 30:0a328a9a4788 | 137 | |
Renate | 37:ea621fdf306a | 138 | double Joint_1_position_prev = 0.0; |
Renate | 37:ea621fdf306a | 139 | double Joint_2_position_prev = 0.0; |
Renate | 29:8e0a7c33e4e7 | 140 | |
Renate | 37:ea621fdf306a | 141 | double Motor_1_position = 0.0; |
Renate | 37:ea621fdf306a | 142 | double Motor_2_position = 0.0; |
Renate | 30:0a328a9a4788 | 143 | |
Renate | 38:fb163733c147 | 144 | double error_M1; // Definiëren van de motorerrors, het verschil tussen de daadwerkelijke hoek en de gewenste hoek. |
Renate | 38:fb163733c147 | 145 | double error_M2; |
Renate | 29:8e0a7c33e4e7 | 146 | |
Renate | 38:fb163733c147 | 147 | // VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER) |
Renate | 38:fb163733c147 | 148 | const double Kp = 17.5; // Kp en Ki waarden voor de PI-controller definiëren. |
Renate | 38:fb163733c147 | 149 | 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 | 150 | // robot oncontroleerbaar gedrag vertoonde. |
Renate | 38:fb163733c147 | 151 | double theta_k_1 = 0.0; // Begin definities opstellen voor de motorhoeken na de proportional part. |
Renate | 37:ea621fdf306a | 152 | double theta_k_2 = 0.0; |
Renate | 29:8e0a7c33e4e7 | 153 | |
Renate | 38:fb163733c147 | 154 | double error_integral_1 = 0.0; // Begin definities opstellen voor de integralen van de errors. |
Renate | 37:ea621fdf306a | 155 | double error_integral_2 = 0.0; |
Renate | 31:967b455bc328 | 156 | |
Renate | 38:fb163733c147 | 157 | double u_i_1; // Uitkomst variabelen definiëren voor de vermenigvuldiging van de error integraal met Ki. |
Renate | 37:ea621fdf306a | 158 | double u_i_2; |
Renate | 32:d651c23bbb77 | 159 | |
Renate | 38:fb163733c147 | 160 | double theta_t_1; // Variabelen opstellen voor de sommatie van theta_k en u_i. |
Renate | 37:ea621fdf306a | 161 | double theta_t_2; |
Renate | 30:0a328a9a4788 | 162 | |
Renate | 38:fb163733c147 | 163 | double abs_theta_t_1; // Variabele opstellen voor de absolute waarde van theta_t. |
Renate | 37:ea621fdf306a | 164 | double abs_theta_t_2; |
Renate | 29:8e0a7c33e4e7 | 165 | |
Renate | 41:dcfe6c86e3f5 | 166 | // VARIABELEN VOOR OPERATION MODE (SERVO) |
Renate | 41:dcfe6c86e3f5 | 167 | double theta_f= 580; // Dit zijn allemaal waarden tussen de 500 en 2400. |
Renate | 41:dcfe6c86e3f5 | 168 | double theta_sout; // Dit zijn allemaal waarden tussen de 500 en 2400. |
Renate | 41:dcfe6c86e3f5 | 169 | double q1_ser; // Dit zijn allemaal waarden tussen de 500 en 2400. |
Renate | 41:dcfe6c86e3f5 | 170 | double q2_ser; // Dit zijn allemaal waarden tussen de 500 en 2400. |
Renate | 41:dcfe6c86e3f5 | 171 | |
Renate | 23:4572750a5c59 | 172 | // VOIDS |
Renate | 23:4572750a5c59 | 173 | |
Renate | 38:fb163733c147 | 174 | void emergency() // Noodfunctie waarbij de motoren uit worden gezet en de ProcessStateMachine wordt losgekoppeld |
Renate | 38:fb163733c147 | 175 | { // van de ticker. De robot doet dan niks meer. De enige optie is om de mbed te resetten, waarna |
Renate | 38:fb163733c147 | 176 | loop_ticker.detach(); // het script opnieuw moet worden opgestart. |
Renate | 28:7c7508bdb21f | 177 | motor1.write(0); |
Renate | 28:7c7508bdb21f | 178 | motor2.write(0); |
Renate | 28:7c7508bdb21f | 179 | pc.printf("Ik ga exploderen!!!\r\n"); |
Renate | 28:7c7508bdb21f | 180 | } |
Renate | 38:fb163733c147 | 181 | |
Renate | 38:fb163733c147 | 182 | void motors_off() // Functie waarbij beide motoren worden uitgezet. |
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 | 38:fb163733c147 | 189 | void EMG_calibration() // Functie die wordt uitgevoerd tijdens de EMG kalibratie. Wordt geinitialiseerd door de bool calib, |
Renate | 38:fb163733c147 | 190 | { // die alleen waar is tijdens de EMG kalibratie. Er wordt gebruikgemaakt van een runner i_calib, zodat |
Renate | 38:fb163733c147 | 191 | if (i_calib == 0) { // EMG-waarden tijdens maximale spierspanning gedurende 5 seconden bij elkaar op worden geteld. Het gemiddelde |
Renate | 38:fb163733c147 | 192 | 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 | 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 | 38:fb163733c147 | 212 | 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 | 213 | { // weer in de referentie positie staan (0 in ons geval, doordat we compenseren met een offset voor encoder counts). Wanneer |
Renate | 38:fb163733c147 | 214 | 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 | 215 | if (theta_h_1_rad < 0) { // wanneer dit voor beide motoren is gebeurd. |
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 | 38:fb163733c147 | 240 | void Inverse_Kinematics() // Functie die de inverse kinematica uitvoert. De inverse Jacobian is afkosmtig uit eerdere berekeningen in Matlab. |
Renate | 38:fb163733c147 | 241 | { // The desired velocity komt voort uit de operation mode, waar hier een waarde aan wordt toegekend. |
Renate | 38:fb163733c147 | 242 | // 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 | 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 | 38:fb163733c147 | 254 | // Integration |
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 | 38:fb163733c147 | 265 | void PI_controller() // De PI-controller zorgt ervoor dat het proportionele en het integrale aandeel worden bepaald. |
Renate | 38:fb163733c147 | 266 | { // Na sommatie van beide aandelen, wordt theta_t als output verkregen. Deze wordt later als input |
Renate | 38:fb163733c147 | 267 | // Proportional part: // voor de motorsnelheden gebruikt. |
Renate | 37:ea621fdf306a | 268 | theta_k_1= Kp * error_M1; |
Renate | 37:ea621fdf306a | 269 | theta_k_2= Kp * error_M2; |
Renate | 31:967b455bc328 | 270 | |
Renate | 31:967b455bc328 | 271 | // Integral part |
Renate | 37:ea621fdf306a | 272 | error_integral_1 = error_integral_1+ error_M1*delta_t; |
Renate | 37:ea621fdf306a | 273 | error_integral_2 = error_integral_2+ error_M2*delta_t; |
Renate | 37:ea621fdf306a | 274 | u_i_1= Ki * error_integral_1; |
Renate | 37:ea621fdf306a | 275 | u_i_2= Ki * error_integral_2; |
Renate | 31:967b455bc328 | 276 | |
Renate | 37:ea621fdf306a | 277 | // Sum all parts and return it |
Renate | 37:ea621fdf306a | 278 | theta_t_1= theta_k_1 + u_i_1; |
Renate | 37:ea621fdf306a | 279 | theta_t_2= theta_k_2 + u_i_2; |
Renate | 31:967b455bc328 | 280 | } |
Renate | 31:967b455bc328 | 281 | |
Renate | 38:fb163733c147 | 282 | void Define_motor_dir() // Deze functie bepaalt de output richtingen die aan de motoren worden gegeven tijdens de operation mode. |
Renate | 38:fb163733c147 | 283 | { // Wanneer de input (wordt ook als snelheid gegeven aan de motoren) positief is, wordt de draairichting van |
Renate | 38:fb163733c147 | 284 | 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 | 285 | motor1_dir.write(0); |
Renate | 37:ea621fdf306a | 286 | motor2_dir.write(0); |
Renate | 37:ea621fdf306a | 287 | } |
Renate | 37:ea621fdf306a | 288 | if (theta_t_1 < 0 && theta_t_2 >= 0) { |
Renate | 37:ea621fdf306a | 289 | motor1_dir.write(1); |
Renate | 37:ea621fdf306a | 290 | motor1_dir.write(0); |
Renate | 37:ea621fdf306a | 291 | } |
Renate | 37:ea621fdf306a | 292 | if (theta_t_1 >= 0 && theta_t_2 < 0) { |
Renate | 32:d651c23bbb77 | 293 | motor1_dir.write(0); |
Renate | 32:d651c23bbb77 | 294 | motor2_dir.write(1); |
Renate | 32:d651c23bbb77 | 295 | } else { |
Renate | 32:d651c23bbb77 | 296 | motor1_dir.write(1); |
Renate | 37:ea621fdf306a | 297 | motor2_dir.write(1); |
Renate | 32:d651c23bbb77 | 298 | } |
Renate | 32:d651c23bbb77 | 299 | } |
Renate | 32:d651c23bbb77 | 300 | |
Renate | 38:fb163733c147 | 301 | void Controlling_system() // Hoofdfunctie die wordt toegepast tijdens de operation mode. Een gewenste snelheid in x- en y-richting wordt |
Renate | 38:fb163733c147 | 302 | { // hiervoor opgesteld en is vervolgens de input voor de inverse kinematica functie. Door middel van een gewenste |
Renate | 38:fb163733c147 | 303 | Inverse_Kinematics(); // motorhoek en de actuele motorhoek wordt een error bepaald. Deze errors worden als input gebruikt voor de |
Renate | 38:fb163733c147 | 304 | // PI-controller, wiens absolute output wordt gebruikt om de motorsnelheid aan te sturen. Tevens wordt een functie |
Renate | 38:fb163733c147 | 305 | 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 | 306 | error_M2 = Motor_2_position + theta_h_2_rad; |
Renate | 37:ea621fdf306a | 307 | |
Renate | 37:ea621fdf306a | 308 | PI_controller(); |
Renate | 37:ea621fdf306a | 309 | |
Renate | 37:ea621fdf306a | 310 | abs_theta_t_1 = abs(theta_t_1); |
Renate | 37:ea621fdf306a | 311 | abs_theta_t_2 = abs(theta_t_2); |
Renate | 37:ea621fdf306a | 312 | |
Renate | 37:ea621fdf306a | 313 | motor1.write(abs_theta_t_1); |
Renate | 37:ea621fdf306a | 314 | motor2.write(abs_theta_t_2); |
Renate | 37:ea621fdf306a | 315 | Define_motor_dir(); |
Renate | 31:967b455bc328 | 316 | } |
Renate | 37:ea621fdf306a | 317 | |
Renate | 41:dcfe6c86e3f5 | 318 | void servo() |
Renate | 41:dcfe6c86e3f5 | 319 | { |
Renate | 41:dcfe6c86e3f5 | 320 | q1_ser= q1*604.7887837; |
Renate | 41:dcfe6c86e3f5 | 321 | q2_ser= q2*604.7887837; |
Renate | 41:dcfe6c86e3f5 | 322 | |
Renate | 41:dcfe6c86e3f5 | 323 | if (servo_button_pressed.read()== false) { |
Renate | 41:dcfe6c86e3f5 | 324 | theta_sout=theta_f+q1_ser+q2_ser+400; |
Renate | 41:dcfe6c86e3f5 | 325 | if (theta_sout>=2400) { |
Renate | 41:dcfe6c86e3f5 | 326 | theta_sout=2400; |
Renate | 41:dcfe6c86e3f5 | 327 | } |
Renate | 41:dcfe6c86e3f5 | 328 | if (theta_sout<=500) { |
Renate | 41:dcfe6c86e3f5 | 329 | theta_sout=500; |
Renate | 41:dcfe6c86e3f5 | 330 | } else { |
Renate | 41:dcfe6c86e3f5 | 331 | theta_sout; |
Renate | 41:dcfe6c86e3f5 | 332 | } |
Renate | 41:dcfe6c86e3f5 | 333 | mijnServo.SetPosition(theta_sout); |
Renate | 41:dcfe6c86e3f5 | 334 | pc.printf("De servo staat op %f graden. in de klapstand\n\r", theta_sout); |
Renate | 41:dcfe6c86e3f5 | 335 | } |
Renate | 41:dcfe6c86e3f5 | 336 | |
Renate | 41:dcfe6c86e3f5 | 337 | else { |
Renate | 41:dcfe6c86e3f5 | 338 | theta_sout=theta_f+q1_ser+q2_ser; |
Renate | 41:dcfe6c86e3f5 | 339 | if (theta_sout>=2400) { |
Renate | 41:dcfe6c86e3f5 | 340 | theta_sout=2400; |
Renate | 41:dcfe6c86e3f5 | 341 | } |
Renate | 41:dcfe6c86e3f5 | 342 | if (theta_sout<=500) { |
Renate | 41:dcfe6c86e3f5 | 343 | theta_sout=500; |
Renate | 41:dcfe6c86e3f5 | 344 | } else { |
Renate | 41:dcfe6c86e3f5 | 345 | theta_sout; |
Renate | 41:dcfe6c86e3f5 | 346 | } |
Renate | 41:dcfe6c86e3f5 | 347 | mijnServo.SetPosition(theta_sout); |
Renate | 41:dcfe6c86e3f5 | 348 | pc.printf("De servo staat op %f graden.\n\r", theta_sout); |
Renate | 41:dcfe6c86e3f5 | 349 | } |
Renate | 41:dcfe6c86e3f5 | 350 | } |
Renate | 41:dcfe6c86e3f5 | 351 | |
Renate | 37:ea621fdf306a | 352 | // Aanmaken van een bool om te testen of de berekeningen in the ProcessStatemachine |
Renate | 37:ea621fdf306a | 353 | // meer tijd kosten dan wordt gegeven door de ticker. Dit zou mogelijk het encoder |
Renate | 37:ea621fdf306a | 354 | // probleem kunnen verklaren. Indien er te weinig tijd is, zou de loop zichzelf in moeten |
Renate | 37:ea621fdf306a | 355 | // halen. Start met een bool die true is, stel deze gelijk aan false in het begin van de loop |
Renate | 37:ea621fdf306a | 356 | // en verander deze weer in true wanneer de hele loop voltooid is. In het geval dat de loop zichzelf |
Renate | 37:ea621fdf306a | 357 | // inhaalt, blijft de bool false en wordt een string (There is a timing problem) geprint. |
Renate | 37:ea621fdf306a | 358 | // RESULTAAT: de string wordt niet geprint, er zouden geen timing issues moeten zijn. |
Renate | 37:ea621fdf306a | 359 | // Het script spreekt zichzelf dus tegen, experts komen ook niet uit dit probleem. |
Renate | 37:ea621fdf306a | 360 | |
Renate | 37:ea621fdf306a | 361 | volatile bool loop_done = true; |
Renate | 37:ea621fdf306a | 362 | |
Renate | 6:64146e16e10c | 363 | // Finite state machine programming (calibration servo motor?) |
Renate | 28:7c7508bdb21f | 364 | void ProcessStateMachine(void) |
Renate | 28:7c7508bdb21f | 365 | { |
Renate | 37:ea621fdf306a | 366 | if (!loop_done) { |
Renate | 37:ea621fdf306a | 367 | pc.printf("There is a timing problem\r\n"); |
Renate | 37:ea621fdf306a | 368 | |
Renate | 37:ea621fdf306a | 369 | return; |
Renate | 37:ea621fdf306a | 370 | } |
Renate | 37:ea621fdf306a | 371 | |
Renate | 37:ea621fdf306a | 372 | loop_done = false; |
Renate | 37:ea621fdf306a | 373 | |
Renate | 23:4572750a5c59 | 374 | // Berekenen van de motorhoeken (in radialen) |
Renate | 40:b26d19d52d40 | 375 | counts1 = Encoder1.getPulses(); // Verkrijgen van het aantal counts uit de encoders |
Renate | 37:ea621fdf306a | 376 | counts2 = Encoder2.getPulses(); |
Renate | 40:b26d19d52d40 | 377 | theta_h_1_rad = conversion_factor*(counts1-offset1); // Van het gemeten aantal counts wordt de offset afgetrokken, zodat het aantal counts in de referentie positie gelijk is aan 0. |
Renate | 40:b26d19d52d40 | 378 | theta_h_2_rad = conversion_factor*(counts2-offset2); // Vermenigvuldiging met de conversion factor zorgt ervoor dat het aantal counts wordt omgezet naar de huidige motorhoeken. |
Renate | 28:7c7508bdb21f | 379 | |
Renate | 29:8e0a7c33e4e7 | 380 | // Calculating joint angles based on motor angles (current encoder values) |
Renate | 40:b26d19d52d40 | 381 | q1 = theta_h_1_rad; // Relatieve hoek joint 1 in radialen. |
Renate | 40:b26d19d52d40 | 382 | q2 = theta_h_2_rad - theta_h_1_rad; // Relatieve hoek joint 2 in radialen. |
Renate | 29:8e0a7c33e4e7 | 383 | |
Renate | 23:4572750a5c59 | 384 | // Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal |
Renate | 28:7c7508bdb21f | 385 | // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled' |
Renate | 23:4572750a5c59 | 386 | filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read()); |
Renate | 23:4572750a5c59 | 387 | filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read()); |
Renate | 23:4572750a5c59 | 388 | filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read()); |
Renate | 28:7c7508bdb21f | 389 | |
Renate | 23:4572750a5c59 | 390 | // Vervolgens wordt de absolute waarde hiervan genomen |
Renate | 23:4572750a5c59 | 391 | filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1); |
Renate | 23:4572750a5c59 | 392 | filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1); |
Renate | 23:4572750a5c59 | 393 | filtered_EMG_calf_abs=abs(filtered_EMG_calf_1); |
Renate | 28:7c7508bdb21f | 394 | |
Renate | 23:4572750a5c59 | 395 | // Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter) |
Renate | 23:4572750a5c59 | 396 | // over het signaal gedaan |
Renate | 23:4572750a5c59 | 397 | filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs); |
Renate | 23:4572750a5c59 | 398 | filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs); |
Renate | 23:4572750a5c59 | 399 | filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs); |
Renate | 28:7c7508bdb21f | 400 | |
Renate | 28:7c7508bdb21f | 401 | // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope |
Renate | 28:7c7508bdb21f | 402 | scope.set(0, normalized_EMG_biceps_right); |
Renate | 28:7c7508bdb21f | 403 | scope.set(1, normalized_EMG_biceps_left); |
Renate | 28:7c7508bdb21f | 404 | scope.set(2, normalized_EMG_calf); |
Renate | 23:4572750a5c59 | 405 | scope.send(); |
Renate | 28:7c7508bdb21f | 406 | |
Renate | 28:7c7508bdb21f | 407 | // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die |
Renate | 28:7c7508bdb21f | 408 | // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende |
Renate | 23:4572750a5c59 | 409 | // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld, |
Renate | 28:7c7508bdb21f | 410 | // waarna het gemiddelde wordt bepaald. |
Renate | 28:7c7508bdb21f | 411 | if (calib) { |
Renate | 37:ea621fdf306a | 412 | EMG_calibration(); |
Renate | 28:7c7508bdb21f | 413 | } |
Renate | 28:7c7508bdb21f | 414 | |
Renate | 23:4572750a5c59 | 415 | // Genormaliseerde EMG's berekenen |
Renate | 23:4572750a5c59 | 416 | normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right; |
Renate | 23:4572750a5c59 | 417 | normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left; |
Renate | 23:4572750a5c59 | 418 | normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf; |
Renate | 28:7c7508bdb21f | 419 | |
Renate | 28:7c7508bdb21f | 420 | // Finite state machine |
Renate | 28:7c7508bdb21f | 421 | switch (currentState) { |
Renate | 6:64146e16e10c | 422 | case Motors_off: |
Renate | 28:7c7508bdb21f | 423 | |
Renate | 28:7c7508bdb21f | 424 | if (stateChanged) { |
Renate | 38:fb163733c147 | 425 | motors_off(); // Functie waarbij motoren uitgaan. |
Renate | 11:4bc0304978e2 | 426 | stateChanged = false; |
Renate | 9:4de589636f50 | 427 | pc.printf("Motors off state\r\n"); |
Renate | 28:7c7508bdb21f | 428 | } |
Renate | 38:fb163733c147 | 429 | if (Emergency_button_pressed.read() == false) { // Normaal krijgt de button een waarde 1 bij indrukken, nu nul -> false. |
Renate | 38:fb163733c147 | 430 | emergency(); // Bij het indrukken van de emergency button, wordt overgegaan op de emergency functie. |
Renate | 29:8e0a7c33e4e7 | 431 | } |
Renate | 38:fb163733c147 | 432 | if (Power_button_pressed.read() == false) { // Normaal krijgt de button een waarde 1 bij indrukken, nu nul -> false. |
Renate | 38:fb163733c147 | 433 | currentState = Calib_motor; // Er wordt doorgegaan naar de volgende state, wanneer de power button wordt ingedrukt. |
Renate | 11:4bc0304978e2 | 434 | stateChanged = true; |
Renate | 11:4bc0304978e2 | 435 | pc.printf("Moving to Calib_motor state\r\n"); |
Renate | 6:64146e16e10c | 436 | } |
Renate | 6:64146e16e10c | 437 | break; |
Renate | 28:7c7508bdb21f | 438 | |
Renate | 39:37744ca57a58 | 439 | case Calib_motor: // In deze state wordt de robot arm met de hand in de juiste referentie (home) positie geplaatst. |
Renate | 39:37744ca57a58 | 440 | // Wanneer dit is gebeurd, wordt de motor kalibratie knop ingedrukt. De offset wordt dan ingesteld als |
Renate | 39:37744ca57a58 | 441 | if (stateChanged) { // het huidige aantal counts. Door deze offset in de komende metingen af te trekken van het aantal counts |
Renate | 39:37744ca57a58 | 442 | 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 | 443 | stateChanged = false; |
Renate | 29:8e0a7c33e4e7 | 444 | } |
Renate | 29:8e0a7c33e4e7 | 445 | |
Renate | 29:8e0a7c33e4e7 | 446 | if (Emergency_button_pressed.read() == false) { |
Renate | 29:8e0a7c33e4e7 | 447 | emergency(); |
Renate | 29:8e0a7c33e4e7 | 448 | } |
Renate | 29:8e0a7c33e4e7 | 449 | if (Motor_calib_button_pressed.read() == false) { |
Renate | 37:ea621fdf306a | 450 | offset1 = counts1; |
Renate | 37:ea621fdf306a | 451 | offset2 = counts2; |
Renate | 11:4bc0304978e2 | 452 | currentState = Calib_EMG; |
Renate | 11:4bc0304978e2 | 453 | stateChanged = true; |
Renate | 9:4de589636f50 | 454 | pc.printf("Moving to Calib_EMG state\r\n"); |
Renate | 28:7c7508bdb21f | 455 | } |
Renate | 11:4bc0304978e2 | 456 | break; |
Renate | 28:7c7508bdb21f | 457 | |
Renate | 39:37744ca57a58 | 458 | case Calib_EMG: // In deze state wordt een kalibratie uitgevoerd van de EMG-signalen. Hiervoor wordt een bool (calib) |
Renate | 39:37744ca57a58 | 459 | // samen met een teller (i_calib) in werking gezet, die ervoor zorgt dat de eerder beschreven EMG-kalibratie |
Renate | 39:37744ca57a58 | 460 | if (stateChanged) { // wordt doorlopen. De spieren worden tijdens de kalibratie gedurende 5 seconden maximaal aangespannen. |
Renate | 39:37744ca57a58 | 461 | i_calib = 0; // Na deze 5 seconden wordt doorgegaan naar de volgende state. De output van deze kalibratie is een gemiddelde |
Renate | 39:37744ca57a58 | 462 | calib = true; // waarde van het EMG-signaal tijdens het maximaal aanspannen. |
Renate | 28:7c7508bdb21f | 463 | pc.printf("Span spieren aan\r\n"); |
Renate | 28:7c7508bdb21f | 464 | stateChanged = false; |
Renate | 28:7c7508bdb21f | 465 | } |
Renate | 28:7c7508bdb21f | 466 | |
Renate | 29:8e0a7c33e4e7 | 467 | if (Emergency_button_pressed.read() == false) { |
Renate | 29:8e0a7c33e4e7 | 468 | emergency(); |
Renate | 29:8e0a7c33e4e7 | 469 | } |
Renate | 29:8e0a7c33e4e7 | 470 | |
Renate | 28:7c7508bdb21f | 471 | if (i_calib > 2500) { |
Renate | 28:7c7508bdb21f | 472 | calib = false; |
Renate | 28:7c7508bdb21f | 473 | currentState = Homing; |
Renate | 28:7c7508bdb21f | 474 | stateChanged = true; |
Renate | 28:7c7508bdb21f | 475 | pc.printf("Moving to Homing state\r\n"); |
Renate | 28:7c7508bdb21f | 476 | } |
Renate | 28:7c7508bdb21f | 477 | break; |
Renate | 28:7c7508bdb21f | 478 | |
Renate | 39:37744ca57a58 | 479 | case Homing: // In deze state wordt de homing uitgevoerd. De motoren draaien hier op een langzame snelheid, todat de gewenste referentiepositie |
Renate | 39:37744ca57a58 | 480 | // wordt bereikt. In dit geval worden de motorsnelheden op 0 gezet. Indien beide motoren de juiste posities hebben bereikt, wordt |
Renate | 39:37744ca57a58 | 481 | 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 | 482 | stateChanged = false; // waardoor gelijk doorgegaan kan worden naar de operation mode. |
Renate | 11:4bc0304978e2 | 483 | } |
Renate | 28:7c7508bdb21f | 484 | if (Emergency_button_pressed.read() == false) { |
Renate | 10:83f3cec8dd1c | 485 | emergency(); |
Renate | 28:7c7508bdb21f | 486 | } |
Renate | 29:8e0a7c33e4e7 | 487 | |
Renate | 37:ea621fdf306a | 488 | Homing_function(); |
Renate | 29:8e0a7c33e4e7 | 489 | |
Renate | 37:ea621fdf306a | 490 | if (theta_h_1_rad == 0.0 && theta_h_2_rad == 0.0) { |
Renate | 28:7c7508bdb21f | 491 | currentState = Operation_mode; |
Renate | 28:7c7508bdb21f | 492 | stateChanged = true; |
Renate | 29:8e0a7c33e4e7 | 493 | pc.printf("Moving to operation mode \r\n"); |
Renate | 28:7c7508bdb21f | 494 | } |
Renate | 28:7c7508bdb21f | 495 | break; |
Renate | 28:7c7508bdb21f | 496 | |
Renate | 37:ea621fdf306a | 497 | case Operation_mode: |
Renate | 28:7c7508bdb21f | 498 | |
Renate | 40:b26d19d52d40 | 499 | if (stateChanged) { // Aan het begin van de operation mode wordt aan allerlei variabelen een beginwaarde van 0 toegekend. Dit wordt één keer gedaan |
Renate | 39:37744ca57a58 | 500 | motors_off(); // (stateChanged), waarna deze waarden verderop in deze state worden overschreven. In het geval dat er tussendoor teruggegaan is |
Renate | 40:b26d19d52d40 | 501 | Joint_1_position = 0; // naar een andere state, wordt op deze manier elke keer weer begonnen met de juiste beginwaarden. |
Renate | 37:ea621fdf306a | 502 | Joint_2_position = 0; |
Renate | 37:ea621fdf306a | 503 | Joint_1_position_prev = Joint_1_position; |
Renate | 37:ea621fdf306a | 504 | Joint_2_position_prev = Joint_2_position; |
Renate | 37:ea621fdf306a | 505 | Joint_velocity[0][0] = 0; |
Renate | 37:ea621fdf306a | 506 | Joint_velocity[1][0] = 0; |
Renate | 37:ea621fdf306a | 507 | Motor_1_position = 0; |
Renate | 37:ea621fdf306a | 508 | Motor_2_position = 0; |
Renate | 37:ea621fdf306a | 509 | theta_k_1 = 0.0; |
Renate | 37:ea621fdf306a | 510 | theta_k_2 = 0.0; |
Renate | 37:ea621fdf306a | 511 | error_integral_1 = 0.0; |
Renate | 37:ea621fdf306a | 512 | error_integral_2 = 0.0; |
Renate | 29:8e0a7c33e4e7 | 513 | stateChanged = false; |
Renate | 29:8e0a7c33e4e7 | 514 | } |
Renate | 28:7c7508bdb21f | 515 | |
Renate | 40:b26d19d52d40 | 516 | if (Power_button_pressed.read() == false) { // Wanneer de power button wordt ingedrukt, worden de motoren uitgezet. |
Renate | 37:ea621fdf306a | 517 | motors_off(); |
Renate | 37:ea621fdf306a | 518 | currentState = Motors_off; |
Renate | 37:ea621fdf306a | 519 | stateChanged = true; |
Renate | 37:ea621fdf306a | 520 | pc.printf("Terug naar de state Motors_off\r\n"); |
Renate | 37:ea621fdf306a | 521 | } |
Renate | 40:b26d19d52d40 | 522 | if (Emergency_button_pressed.read() == false) { // Wanneer de emergency button wordt ingedrukt, wordt de emergency functie aangeroepen. |
Renate | 29:8e0a7c33e4e7 | 523 | emergency(); |
Renate | 29:8e0a7c33e4e7 | 524 | } |
Renate | 40:b26d19d52d40 | 525 | if (Motor_calib_button_pressed.read() == false) { // Wanneer tijdens de operation mode teruggegaan moet worden naar de referentie positie, kan de button voor de motor kalibratie |
Renate | 40:b26d19d52d40 | 526 | motors_off(); // worden ingedrukt. In dit geval is het niet nodig om de motoren opnieuw te kalibreren, dus kan direct teruggegaan worden naar |
Renate | 40:b26d19d52d40 | 527 | currentState = Homing; // de state Homing. De motoren bewegen dan totdat de referentie positie weer wordt bereikt. |
Renate | 29:8e0a7c33e4e7 | 528 | stateChanged = true; |
Renate | 29:8e0a7c33e4e7 | 529 | pc.printf("Terug naar de state Homing\r\n"); |
Renate | 29:8e0a7c33e4e7 | 530 | } |
Renate | 40:b26d19d52d40 | 531 | if (normalized_EMG_biceps_right >= 0.3) { // Vanaf hier begint de daadwerkelijke besturing van de robot arm door middel van EMG signalen. Indien de rechter biceps wordt |
Renate | 40:b26d19d52d40 | 532 | // aangespannen, zal het genormaliseerde EMG signaal boven 0.3 uitkomen. Wanneer tegelijkertijd de kuit wordt aangespannen is er |
Renate | 40:b26d19d52d40 | 533 | if (normalized_EMG_calf < 0.3) { // een gewenste negatieve snelheid in de y-richting. Zo niet, dan is er een gewenste positieve snelheid in de y-richting. |
Renate | 40:b26d19d52d40 | 534 | vx = 0.0; // Deze gewenste snelheid wordt doorgevoerd in de functie Controlling_system, die onder andere inverse kinematics en een |
Renate | 40:b26d19d52d40 | 535 | vy = 0.02; // PI-controller bevat. Uiteindelijk wordt aan beide motoren een snelheid en richting toegekend, zodat de robot met de |
Renate | 40:b26d19d52d40 | 536 | } // gewenste snelheid in een rechte lijn verticaal kan bewegen. |
Renate | 32:d651c23bbb77 | 537 | if (normalized_EMG_calf >= 0.3) { |
Renate | 31:967b455bc328 | 538 | vx = 0.0; |
Renate | 37:ea621fdf306a | 539 | vy = -0.02; |
Renate | 31:967b455bc328 | 540 | } |
Renate | 32:d651c23bbb77 | 541 | |
Renate | 37:ea621fdf306a | 542 | Controlling_system(); |
Renate | 28:7c7508bdb21f | 543 | |
Renate | 40:b26d19d52d40 | 544 | } else if (normalized_EMG_biceps_left >= 0.3) { // Indien de rechter biceps niet wordt aangespannen en de genormaliseerde EMG-waarde van de linker biceps groter is dan 0.3, geldt |
Renate | 40:b26d19d52d40 | 545 | if (normalized_EMG_calf < 0.3) { // hetzelfde als hierboven beschreven, maar dan voor de x-richting. Dit resulteert in een horizontale beweging in een rechte lijn. |
Renate | 40:b26d19d52d40 | 546 | vx = 0.02; |
Renate | 32:d651c23bbb77 | 547 | vy = 0.0; |
Renate | 32:d651c23bbb77 | 548 | } |
Renate | 32:d651c23bbb77 | 549 | if (normalized_EMG_calf >= 0.3) { |
Renate | 40:b26d19d52d40 | 550 | vx = -0.02; |
Renate | 32:d651c23bbb77 | 551 | vy = 0.0; |
Renate | 32:d651c23bbb77 | 552 | } |
Renate | 31:967b455bc328 | 553 | |
Renate | 37:ea621fdf306a | 554 | Controlling_system(); |
Renate | 32:d651c23bbb77 | 555 | |
Renate | 40:b26d19d52d40 | 556 | } else { // Indien beide biceps niet worden aangespannen, moet de robot arm niet bewegen. De snelheid in x- en y-richting moet dan gelijk zijn |
Renate | 40:b26d19d52d40 | 557 | vx = 0.0; // aan nul. Toch zal wel gecompenseerd moeten worden voor de zwaartekracht om de arm in dezelfde positie te houden. Deze snelheden |
Renate | 40:b26d19d52d40 | 558 | vy = 0.0; // worden daarom alsnog doorgevoerd in de functie Controlling_systeem. Dit resulteert in motorsnelheden en richtingen die ervoor zorgen |
Renate | 40:b26d19d52d40 | 559 | // dat de arm gebalanceerd wordt op de voorgaande positie. |
Renate | 37:ea621fdf306a | 560 | Controlling_system(); |
Renate | 37:ea621fdf306a | 561 | |
Renate | 28:7c7508bdb21f | 562 | } |
Renate | 21:456acc79726c | 563 | break; |
Renate | 28:7c7508bdb21f | 564 | |
Renate | 7:1d57463393c6 | 565 | default: |
Renate | 40:b26d19d52d40 | 566 | |
Renate | 40:b26d19d52d40 | 567 | motors_off(); // Hier wordt dezelfde functie als eerder toegepast om de motoren uit te schakelen -> safety! |
Renate | 9:4de589636f50 | 568 | pc.printf("Unknown or uninplemented state reached!\r\n"); |
Renate | 28:7c7508bdb21f | 569 | |
WiesjeRoskamp | 2:aee655d11b6d | 570 | } |
Renate | 40:b26d19d52d40 | 571 | loop_done = true; // Dit is nog een element voor het testen of de ProcessStateMachine zichzelf inhaalt. Indien de volledige |
Renate | 40:b26d19d52d40 | 572 | } // loop wel doorlopen is, verandert de bool hier in true. Hierdoor wordt de string aan het begin van de state machine |
Renate | 40:b26d19d52d40 | 573 | // NIET geprint. Zie een eerdere comment (vlak voor de definitie van de state machine) voor verdere toelichting. |
Renate | 8:c7d3b67346db | 574 | int main(void) |
Renate | 28:7c7508bdb21f | 575 | { |
Renate | 40:b26d19d52d40 | 576 | pc.baud(115200); // De baud rate wordt op 115200 bits/sec gezet. |
Renate | 37:ea621fdf306a | 577 | |
Renate | 40:b26d19d52d40 | 578 | motor1.period_us(56); // Idealiter ligt deze frequentie tussen de 15 en 18 kHz. We hebben gekozen voor de kleinst mogelijke motor periode, |
Renate | 40:b26d19d52d40 | 579 | motor2.period_us(56); // die gelijk is aan 1/18000 = ongeveer 56 us. |
Renate | 37:ea621fdf306a | 580 | |
Renate | 28:7c7508bdb21f | 581 | pc.printf("Opstarten\r\n"); |
Renate | 23:4572750a5c59 | 582 | |
Renate | 40:b26d19d52d40 | 583 | // Chain voor rechter biceps // De filterketens zijn apart voor elke spier gedefinieerd. Er wordt eerst een keten aangemaakt die bestaat uit een |
Renate | 40:b26d19d52d40 | 584 | bqcbr.add(&bqbr1).add(&bqbr2); // high-pass filter, gevolgd door een notch filter. Na het nemen van de absolute waarde van het bewerkte EMG-signaal |
Renate | 40:b26d19d52d40 | 585 | bqcbr2.add(&bqbr3).add(&bqbr4); // moet het tweede gedeelte van de filterketen worden toegepast. Hiervoor moeten twee low-pass filters aan elkaar |
Renate | 40:b26d19d52d40 | 586 | // Chain voor linker biceps // geketend worden. |
Renate | 28:7c7508bdb21f | 587 | bqcbl.add(&bqbl1).add(&bqbl2); |
Renate | 28:7c7508bdb21f | 588 | bqcbl2.add(&bqbl3).add(&bqbl4); |
Renate | 28:7c7508bdb21f | 589 | // Chain voor kuit |
Renate | 28:7c7508bdb21f | 590 | bqck.add(&bqk1).add(&bqk2); |
Renate | 28:7c7508bdb21f | 591 | bqck2.add(&bqk3).add(&bqk4); |
Renate | 28:7c7508bdb21f | 592 | |
Renate | 40:b26d19d52d40 | 593 | loop_ticker.attach(&ProcessStateMachine, 0.002f); // We laten de ProcessStateMachine runnen met een frequentie van 500 Hz. Dit is gedaan om te kunnen matchen met de gewenste |
Renate | 40:b26d19d52d40 | 594 | // sampling frequentie voor EMG-signalen. |
Renate | 28:7c7508bdb21f | 595 | while(true) { |
Renate | 28:7c7508bdb21f | 596 | /* do nothing */ |
Renate | 28:7c7508bdb21f | 597 | } |
Renate | 28:7c7508bdb21f | 598 | } |