Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@33:f81f68a912b3, 2019-10-31 (annotated)

Committer:
Renate
Date:
Thu Oct 31 10:51:30 2019 +0000
Revision:
33:f81f68a912b3
Parent:
32:d651c23bbb77
Child:
34:1244984873ba
Revision voordat PI-controller in x-richting en y-richting wordt samengevoegd

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"
Rosalie 3:6ee0b20c23b0 6 #include "FastPWM.h"
Renate 32:d651c23bbb77 7 #define M_PI 3.14159265358979323846 /* pi */
WiesjeRoskamp 2:aee655d11b6d 8 #include <math.h>
Rosalie 5:9f1260408ef2 9 #include "Servo.h"
Renate 21:456acc79726c 10 #include <cmath>
Renate 29:8e0a7c33e4e7 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 23:4572750a5c59 16 Ticker loop_ticker;
Renate 33:f81f68a912b3 17 Ticker loop_prints;
Renate 15:ad065ab92d11 18
Renate 23:4572750a5c59 19 // BENODIGD VOOR PROCESS STATE MACHINE
Renate 23:4572750a5c59 20 enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
Renate 28:7c7508bdb21f 21 states currentState = Motors_off;
Renate 23:4572750a5c59 22 bool stateChanged = true; // Make sure the initialization of first state is executed
Renate 23:4572750a5c59 23
Renate 23:4572750a5c59 24 // INPUTS
Renate 14:54343b9fd708 25 DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken!
Renate 9:4de589636f50 26 DigitalIn Emergency_button_pressed(D2);
Renate 22:8585d41a670b 27 DigitalIn Motor_calib_button_pressed(SW2);
Renate 33:f81f68a912b3 28 DigitalIn Homing_button_pressed(SW3);
WiesjeRoskamp 2:aee655d11b6d 29
Renate 15:ad065ab92d11 30 AnalogIn EMG_biceps_right_raw (A0);
Renate 15:ad065ab92d11 31 AnalogIn EMG_biceps_left_raw (A1);
Renate 19:1fd39a2afc30 32 AnalogIn EMG_calf_raw (A2);
Renate 15:ad065ab92d11 33
Renate 32:d651c23bbb77 34 QEI Encoder1(D12, D13, NC, 8400, QEI::X2_ENCODING); //Checken of die D12, D9 etc wel kloppen, 8400= gear ratio x 64
Renate 32:d651c23bbb77 35 QEI Encoder2(D9, D10, NC, 8400, QEI::X2_ENCODING);
Renate 21:456acc79726c 36
Renate 23:4572750a5c59 37 // OUTPUTS
Renate 28:7c7508bdb21f 38 PwmOut motor1(D6); // Misschien moeten we hiervoor DigitalOut gebruiken, moet
Renate 23:4572750a5c59 39 PwmOut motor2(D5); // samen kunnen gaan met de servo motor
Renate 21:456acc79726c 40
Renate 23:4572750a5c59 41 DigitalOut motor1_dir(D7);
Renate 23:4572750a5c59 42 DigitalOut motor2_dir(D4);
Renate 23:4572750a5c59 43
Renate 23:4572750a5c59 44 // VARIABELEN VOOR ENCODER, MOTORHOEK ETC.
Renate 32:d651c23bbb77 45 int pulses_M1;
Renate 32:d651c23bbb77 46 int pulses_M2;
Renate 21:456acc79726c 47 int counts1;
Renate 21:456acc79726c 48 int counts2;
Renate 32:d651c23bbb77 49 const double conversion_factor = (2.0*M_PI)/(64.0*131.25*2.0);
Renate 21:456acc79726c 50 double theta_h_1_rad;
Renate 21:456acc79726c 51 double theta_h_2_rad;
Renate 21:456acc79726c 52
Renate 23:4572750a5c59 53 // DEFINITIES VOOR FILTERS
Renate 20:a6a5bdd7d118 54
Renate 21:456acc79726c 55 // BICEPS-RECHTS
Renate 28:7c7508bdb21f 56 // Definities voor eerste BiQuadChain (High-pass en Notch)
Renate 28:7c7508bdb21f 57 BiQuadChain bqcbr;
Renate 28:7c7508bdb21f 58 BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
Renate 21:456acc79726c 59 BiQuad bqbr2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
Renate 21:456acc79726c 60 // Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
Renate 28:7c7508bdb21f 61 // Definieer (twee Low-pass -> vierde orde verkrijgen):
Renate 21:456acc79726c 62 BiQuadChain bqcbr2;
Renate 21:456acc79726c 63 BiQuad bqbr3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
Renate 21:456acc79726c 64 BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
Renate 20:a6a5bdd7d118 65
Renate 21:456acc79726c 66 // BICEPS-LINKS
Renate 28:7c7508bdb21f 67 // Definities voor eerste BiQuadChain (High-pass en Notch)
Renate 28:7c7508bdb21f 68 BiQuadChain bqcbl;
Renate 28:7c7508bdb21f 69 BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
Renate 21:456acc79726c 70 BiQuad bqbl2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
Renate 20:a6a5bdd7d118 71 // Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
Renate 28:7c7508bdb21f 72 // Definieer (twee Low-pass -> vierde orde verkrijgen):
Renate 21:456acc79726c 73 BiQuadChain bqcbl2;
Renate 21:456acc79726c 74 BiQuad bqbl3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
Renate 21:456acc79726c 75 BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
Renate 21:456acc79726c 76
Renate 21:456acc79726c 77 // KUIT
Renate 28:7c7508bdb21f 78 // Definities voor eerste BiQuadChain (High-pass en Notch)
Renate 28:7c7508bdb21f 79 BiQuadChain bqck;
Renate 28:7c7508bdb21f 80 BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
Renate 21:456acc79726c 81 BiQuad bqk2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
Renate 21:456acc79726c 82 // Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
Renate 28:7c7508bdb21f 83 // Definieer (twee Low-pass -> vierde orde verkrijgen):
Renate 21:456acc79726c 84 BiQuadChain bqck2;
Renate 21:456acc79726c 85 BiQuad bqk3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
Renate 21:456acc79726c 86 BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
Renate 20:a6a5bdd7d118 87
Renate 28:7c7508bdb21f 88 // VARIABELEN VOOR EMG + FILTEREN
Renate 23:4572750a5c59 89 double filtered_EMG_biceps_right;
Renate 23:4572750a5c59 90 double filtered_EMG_biceps_left;
Renate 23:4572750a5c59 91 double filtered_EMG_calf;
Renate 23:4572750a5c59 92
Renate 23:4572750a5c59 93 double filtered_EMG_biceps_right_1;
Renate 23:4572750a5c59 94 double filtered_EMG_biceps_left_1;
Renate 23:4572750a5c59 95 double filtered_EMG_calf_1;
Renate 23:4572750a5c59 96
Renate 23:4572750a5c59 97 double filtered_EMG_biceps_right_abs;
Renate 23:4572750a5c59 98 double filtered_EMG_biceps_left_abs;
Renate 23:4572750a5c59 99 double filtered_EMG_calf_abs;
Renate 23:4572750a5c59 100
Renate 28:7c7508bdb21f 101 double filtered_EMG_biceps_right_total;
Renate 23:4572750a5c59 102 double filtered_EMG_biceps_left_total;
Renate 23:4572750a5c59 103 double filtered_EMG_calf_total;
Renate 19:1fd39a2afc30 104
Renate 23:4572750a5c59 105 // Variabelen voor HIDScope
Renate 23:4572750a5c59 106 HIDScope scope(3);
Renate 23:4572750a5c59 107
Renate 23:4572750a5c59 108 // VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP
Renate 28:7c7508bdb21f 109 bool calib = false;
Renate 23:4572750a5c59 110 static int i_calib = 0;
Renate 21:456acc79726c 111
Renate 23:4572750a5c59 112 double mean_EMG_biceps_right;
Renate 23:4572750a5c59 113 double mean_EMG_biceps_left;
Renate 23:4572750a5c59 114 double mean_EMG_calf;
Renate 23:4572750a5c59 115
Renate 29:8e0a7c33e4e7 116 // VARIABELEN VOOR OPERATION MODE (EMG)
Renate 23:4572750a5c59 117 double normalized_EMG_biceps_right;
Renate 23:4572750a5c59 118 double normalized_EMG_biceps_left;
Renate 23:4572750a5c59 119 double normalized_EMG_calf;
Renate 23:4572750a5c59 120
Renate 29:8e0a7c33e4e7 121 // VARIABELEN VOOR OPERATION MODE (RKI)
Renate 29:8e0a7c33e4e7 122 double vx; // Gelijk aan variabele die snelheid aangeeft in EMG (in x-richting)
Renate 29:8e0a7c33e4e7 123 double vy; // Gelijk aan variabele die snelheid aangeeft in EMG (in y-richting)
Renate 29:8e0a7c33e4e7 124
Renate 29:8e0a7c33e4e7 125 const double delta_t = 0.002;
Renate 29:8e0a7c33e4e7 126
Renate 30:0a328a9a4788 127 double Joint_1_position_x = 0.0;
Renate 30:0a328a9a4788 128 double Joint_2_position_x = 0.0;
Renate 30:0a328a9a4788 129
Renate 30:0a328a9a4788 130 double Joint_1_position_y = 0.0;
Renate 30:0a328a9a4788 131 double Joint_2_position_y = 0.0;
Renate 29:8e0a7c33e4e7 132
Renate 30:0a328a9a4788 133 double Joint_1_position_x_prev = 0.0;
Renate 30:0a328a9a4788 134 double Joint_2_position_x_prev = 0.0;
Renate 30:0a328a9a4788 135
Renate 30:0a328a9a4788 136 double Joint_1_position_y_prev;
Renate 30:0a328a9a4788 137 double Joint_2_position_y_prev;
Renate 30:0a328a9a4788 138
Renate 30:0a328a9a4788 139 double Joint_velocity_x[2][1] = {{0.0}, {0.0}};
Renate 30:0a328a9a4788 140 double Joint_velocity_y[2][1] = {{0.0}, {0.0}};
Renate 29:8e0a7c33e4e7 141
Renate 29:8e0a7c33e4e7 142 double q1_dot_x;
Renate 29:8e0a7c33e4e7 143 double q2_dot_x;
Renate 29:8e0a7c33e4e7 144
Renate 29:8e0a7c33e4e7 145 double q1_dot_y;
Renate 29:8e0a7c33e4e7 146 double q2_dot_y;
Renate 29:8e0a7c33e4e7 147
Renate 29:8e0a7c33e4e7 148 double q1;
Renate 29:8e0a7c33e4e7 149 double q2;
Renate 29:8e0a7c33e4e7 150
Renate 30:0a328a9a4788 151 double Motor_1_position_x = 0.0;
Renate 30:0a328a9a4788 152 double Motor_2_position_x = 0.0;
Renate 30:0a328a9a4788 153
Renate 30:0a328a9a4788 154 double Motor_1_position_y = 0.0;
Renate 30:0a328a9a4788 155 double Motor_2_position_y = 0.0;
Renate 30:0a328a9a4788 156
Renate 30:0a328a9a4788 157 // VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
Renate 29:8e0a7c33e4e7 158
Renate 33:f81f68a912b3 159 const double Kp = 12.5;
Renate 32:d651c23bbb77 160 const double Ki = 0.03;
Renate 30:0a328a9a4788 161
Renate 31:967b455bc328 162 // Voor x-richting
Renate 31:967b455bc328 163 double theta_k_1_x = 0.0;
Renate 31:967b455bc328 164 double theta_k_2_x = 0.0;
Renate 31:967b455bc328 165
Renate 31:967b455bc328 166 double error_integral_1_x = 0.0;
Renate 31:967b455bc328 167 double error_integral_2_x = 0.0;
Renate 29:8e0a7c33e4e7 168
Renate 31:967b455bc328 169 double u_i_1_x;
Renate 31:967b455bc328 170 double u_i_2_x;
Renate 31:967b455bc328 171
Renate 31:967b455bc328 172 double theta_t_1_x;
Renate 31:967b455bc328 173 double theta_t_2_x;
Renate 31:967b455bc328 174
Renate 31:967b455bc328 175 double error_q1_x;
Renate 31:967b455bc328 176 double error_q2_x;
Renate 30:0a328a9a4788 177
Renate 32:d651c23bbb77 178 double abs_theta_t_1_x;
Renate 32:d651c23bbb77 179 double abs_theta_t_2_x;
Renate 32:d651c23bbb77 180
Renate 31:967b455bc328 181 // Voor y-richting
Renate 31:967b455bc328 182 double theta_k_1_y = 0.0;
Renate 31:967b455bc328 183 double theta_k_2_y = 0.0;
Renate 31:967b455bc328 184
Renate 31:967b455bc328 185 double error_integral_1_y = 0.0;
Renate 31:967b455bc328 186 double error_integral_2_y = 0.0;
Renate 30:0a328a9a4788 187
Renate 31:967b455bc328 188 double u_i_1_y;
Renate 31:967b455bc328 189 double u_i_2_y;
Renate 31:967b455bc328 190
Renate 31:967b455bc328 191 double theta_t_1_y;
Renate 31:967b455bc328 192 double theta_t_2_y;
Renate 31:967b455bc328 193
Renate 31:967b455bc328 194 double error_q1_y;
Renate 31:967b455bc328 195 double error_q2_y;
Renate 31:967b455bc328 196
Renate 31:967b455bc328 197 double abs_theta_t_1_y;
Renate 31:967b455bc328 198 double abs_theta_t_2_y;
Renate 29:8e0a7c33e4e7 199
Renate 23:4572750a5c59 200 // VOIDS
Renate 23:4572750a5c59 201
Renate 23:4572750a5c59 202 // Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden).
Renate 28:7c7508bdb21f 203 // Enige optie is resetten, dan wordt het script opnieuw opgestart.
Renate 8:c7d3b67346db 204 void emergency()
Renate 28:7c7508bdb21f 205 {
Renate 28:7c7508bdb21f 206 loop_ticker.detach();
Renate 28:7c7508bdb21f 207 motor1.write(0);
Renate 28:7c7508bdb21f 208 motor2.write(0);
Renate 28:7c7508bdb21f 209 pc.printf("Ik ga exploderen!!!\r\n");
Renate 28:7c7508bdb21f 210 }
Renate 11:4bc0304978e2 211
Renate 28:7c7508bdb21f 212 // Motoren uitzetten
Renate 8:c7d3b67346db 213 void motors_off()
Renate 28:7c7508bdb21f 214 {
Renate 28:7c7508bdb21f 215 motor1.write(0);
Renate 28:7c7508bdb21f 216 motor2.write(0);
Renate 28:7c7508bdb21f 217 pc.printf("Motoren uit functie\r\n");
Renate 28:7c7508bdb21f 218 }
Renate 28:7c7508bdb21f 219
Renate 14:54343b9fd708 220 // Motoren aanzetten
Renate 15:ad065ab92d11 221 void motors_on()
Renate 28:7c7508bdb21f 222 {
Renate 32:d651c23bbb77 223 motor1.write(0.3);
Renate 31:967b455bc328 224 motor1_dir.write(0);
Renate 31:967b455bc328 225 motor2.write(0.3);
Renate 28:7c7508bdb21f 226 motor2_dir.write(1);
Renate 31:967b455bc328 227 // Door motor 1 een richting van 1 te geven en motor 2 een van 0, draaien
Renate 31:967b455bc328 228 // beide motoren rechtsom
Renate 28:7c7508bdb21f 229 pc.printf("Motoren aan functie\r\n");
Renate 28:7c7508bdb21f 230 }
Rosalie 3:6ee0b20c23b0 231
Renate 29:8e0a7c33e4e7 232 void Inverse_Kinematics()
Renate 29:8e0a7c33e4e7 233 {
Renate 30:0a328a9a4788 234 // Defining joint velocities based on calculations of matlab
Renate 32:d651c23bbb77 235
Renate 32:d651c23bbb77 236 Joint_velocity_x[0][0] = (vx*(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 32:d651c23bbb77 237 Joint_velocity_x[1][0] = (vx*(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 29:8e0a7c33e4e7 238
Renate 32:d651c23bbb77 239 Joint_velocity_y[0][0] = (vy*(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 32:d651c23bbb77 240 Joint_velocity_y[1][0] = (vy*(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 241
Renate 29:8e0a7c33e4e7 242 // Integratie
Renate 30:0a328a9a4788 243 Joint_1_position_x = Joint_1_position_x_prev + Joint_velocity_x[0][0]*delta_t;
Renate 30:0a328a9a4788 244 Joint_2_position_x = Joint_2_position_x_prev + Joint_velocity_x[1][0]*delta_t;
Renate 30:0a328a9a4788 245
Renate 30:0a328a9a4788 246 Joint_1_position_y = Joint_1_position_y_prev + Joint_velocity_y[0][0]*delta_t;
Renate 30:0a328a9a4788 247 Joint_2_position_y = Joint_2_position_y_prev + Joint_velocity_y[1][0]*delta_t;
Renate 29:8e0a7c33e4e7 248
Renate 30:0a328a9a4788 249 Joint_1_position_x_prev = Joint_1_position_x;
Renate 30:0a328a9a4788 250 Joint_2_position_x_prev = Joint_2_position_x;
Renate 30:0a328a9a4788 251
Renate 30:0a328a9a4788 252 Joint_1_position_y_prev = Joint_1_position_y;
Renate 30:0a328a9a4788 253 Joint_2_position_y_prev = Joint_2_position_y;
Renate 30:0a328a9a4788 254
Renate 30:0a328a9a4788 255 Motor_1_position_x = Joint_1_position_x;
Renate 30:0a328a9a4788 256 Motor_2_position_x = Joint_1_position_x + Joint_2_position_x;
Renate 29:8e0a7c33e4e7 257
Renate 30:0a328a9a4788 258 Motor_1_position_y = Joint_1_position_y;
Renate 30:0a328a9a4788 259 Motor_2_position_y = Joint_1_position_y + Joint_2_position_y;
Renate 30:0a328a9a4788 260 }
Renate 29:8e0a7c33e4e7 261
Renate 31:967b455bc328 262 // PI-CONTROLLER X-RICHTING
Renate 31:967b455bc328 263 void PI_controller_x()
Renate 31:967b455bc328 264 {
Renate 31:967b455bc328 265 // Proportional part:
Renate 31:967b455bc328 266 theta_k_1_x= Kp * error_q1_x;
Renate 31:967b455bc328 267 theta_k_2_x= Kp * error_q2_x;
Renate 31:967b455bc328 268
Renate 31:967b455bc328 269 // Integral part
Renate 31:967b455bc328 270 error_integral_1_x = error_integral_1_x + error_q1_x *delta_t;
Renate 31:967b455bc328 271 error_integral_2_x = error_integral_2_x + error_q2_x *delta_t;
Renate 31:967b455bc328 272 u_i_1_x= Ki * error_integral_1_x;
Renate 31:967b455bc328 273 u_i_2_x= Ki * error_integral_2_x;
Renate 31:967b455bc328 274
Renate 31:967b455bc328 275 // sum all parts and return it
Renate 31:967b455bc328 276 theta_t_1_x = theta_k_1_x + u_i_1_x;
Renate 31:967b455bc328 277 theta_t_2_x = theta_k_2_x + u_i_2_x;
Renate 31:967b455bc328 278 }
Renate 31:967b455bc328 279
Renate 31:967b455bc328 280 // PI-CONTROLLER Y-RICHTING
Renate 30:0a328a9a4788 281 void PI_controller_y()
Renate 30:0a328a9a4788 282 {
Renate 30:0a328a9a4788 283 // Proportional part:
Renate 31:967b455bc328 284 theta_k_1_y= Kp * error_q1_y;
Renate 31:967b455bc328 285 theta_k_2_y= Kp * error_q2_y;
Renate 30:0a328a9a4788 286
Renate 30:0a328a9a4788 287 // Integral part
Renate 31:967b455bc328 288 error_integral_1_y = error_integral_1_y + error_q1_y *delta_t;
Renate 31:967b455bc328 289 error_integral_2_y = error_integral_2_y + error_q2_y *delta_t;
Renate 31:967b455bc328 290 u_i_1_y= Ki * error_integral_1_y;
Renate 31:967b455bc328 291 u_i_2_y= Ki * error_integral_2_y;
Renate 30:0a328a9a4788 292
Renate 30:0a328a9a4788 293 // sum all parts and return it
Renate 31:967b455bc328 294 theta_t_1_y = theta_k_1_y + u_i_1_y;
Renate 31:967b455bc328 295 theta_t_2_y = theta_k_2_y + u_i_2_y;
Renate 29:8e0a7c33e4e7 296 }
Renate 29:8e0a7c33e4e7 297
Renate 32:d651c23bbb77 298 void Define_motor_dir_x()
Renate 32:d651c23bbb77 299 {
Renate 32:d651c23bbb77 300 if (theta_t_1_x >= 0 && theta_t_2_x >= 0) {
Renate 32:d651c23bbb77 301 motor1_dir.write(0);
Renate 32:d651c23bbb77 302 motor2_dir.write(1);
Renate 32:d651c23bbb77 303 }
Renate 32:d651c23bbb77 304 if (theta_t_1_x < 0 && theta_t_2_x >= 0) {
Renate 32:d651c23bbb77 305 motor1_dir.write(1);
Renate 32:d651c23bbb77 306 motor1_dir.write(1);
Renate 32:d651c23bbb77 307 }
Renate 32:d651c23bbb77 308 if (theta_t_1_x >= 0 && theta_t_2_x < 0) {
Renate 32:d651c23bbb77 309 motor1_dir.write(0);
Renate 32:d651c23bbb77 310 motor2_dir.write(0);
Renate 32:d651c23bbb77 311 } else {
Renate 32:d651c23bbb77 312 motor1_dir.write(1);
Renate 32:d651c23bbb77 313 motor2_dir.write(0);
Renate 32:d651c23bbb77 314 }
Renate 32:d651c23bbb77 315 }
Renate 32:d651c23bbb77 316
Renate 32:d651c23bbb77 317 void Define_motor_dir_y()
Renate 31:967b455bc328 318 {
Renate 31:967b455bc328 319 if (theta_t_1_y >= 0 && theta_t_2_y >= 0) {
Renate 31:967b455bc328 320 motor1_dir.write(0);
Renate 31:967b455bc328 321 motor2_dir.write(1);
Renate 31:967b455bc328 322 }
Renate 31:967b455bc328 323 if (theta_t_1_y < 0 && theta_t_2_y >= 0) {
Renate 31:967b455bc328 324 motor1_dir.write(1);
Renate 31:967b455bc328 325 motor1_dir.write(1);
Renate 31:967b455bc328 326 }
Renate 31:967b455bc328 327 if (theta_t_1_y >= 0 && theta_t_2_y < 0) {
Renate 31:967b455bc328 328 motor1_dir.write(0);
Renate 31:967b455bc328 329 motor2_dir.write(0);
Renate 31:967b455bc328 330 } else {
Renate 31:967b455bc328 331 motor1_dir.write(1);
Renate 31:967b455bc328 332 motor2_dir.write(0);
Renate 31:967b455bc328 333 }
Renate 31:967b455bc328 334 }
Renate 6:64146e16e10c 335 // Finite state machine programming (calibration servo motor?)
Renate 28:7c7508bdb21f 336 void ProcessStateMachine(void)
Renate 28:7c7508bdb21f 337 {
Renate 23:4572750a5c59 338 // Berekenen van de motorhoeken (in radialen)
Renate 32:d651c23bbb77 339 pulses_M1 = Encoder1.getPulses();
Renate 32:d651c23bbb77 340 pulses_M2 = Encoder2.getPulses();
Renate 32:d651c23bbb77 341 counts1 = pulses_M1*2;
Renate 32:d651c23bbb77 342 counts2 = pulses_M2*2;
Renate 32:d651c23bbb77 343 theta_h_1_rad = conversion_factor*counts1;
Renate 32:d651c23bbb77 344 theta_h_2_rad = conversion_factor*counts2;
Renate 28:7c7508bdb21f 345
Renate 29:8e0a7c33e4e7 346 // Calculating joint angles based on motor angles (current encoder values)
Renate 29:8e0a7c33e4e7 347 q1 = theta_h_1_rad; // Relative angle joint 1 (rad)
Renate 29:8e0a7c33e4e7 348 q2 = theta_h_2_rad - theta_h_1_rad; // Relative angle joint 2 (rad)
Renate 29:8e0a7c33e4e7 349
Renate 23:4572750a5c59 350 // Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal
Renate 28:7c7508bdb21f 351 // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled'
Renate 23:4572750a5c59 352 filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
Renate 23:4572750a5c59 353 filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
Renate 23:4572750a5c59 354 filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
Renate 28:7c7508bdb21f 355
Renate 23:4572750a5c59 356 // Vervolgens wordt de absolute waarde hiervan genomen
Renate 23:4572750a5c59 357 filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
Renate 23:4572750a5c59 358 filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
Renate 23:4572750a5c59 359 filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
Renate 28:7c7508bdb21f 360
Renate 23:4572750a5c59 361 // Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter)
Renate 23:4572750a5c59 362 // over het signaal gedaan
Renate 23:4572750a5c59 363 filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
Renate 23:4572750a5c59 364 filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
Renate 23:4572750a5c59 365 filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
Renate 28:7c7508bdb21f 366
Renate 28:7c7508bdb21f 367 // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope
Renate 33:f81f68a912b3 368 //scope.set(0, normalized_EMG_biceps_right);
Renate 33:f81f68a912b3 369 //scope.set(1, normalized_EMG_biceps_left);
Renate 33:f81f68a912b3 370 //scope.set(2, normalized_EMG_calf);
Renate 33:f81f68a912b3 371 //scope.send();
Renate 28:7c7508bdb21f 372
Renate 28:7c7508bdb21f 373 // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die
Renate 28:7c7508bdb21f 374 // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende
Renate 23:4572750a5c59 375 // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
Renate 28:7c7508bdb21f 376 // waarna het gemiddelde wordt bepaald.
Renate 28:7c7508bdb21f 377 if (calib) {
Renate 28:7c7508bdb21f 378 if (i_calib == 0) {
Renate 28:7c7508bdb21f 379 filtered_EMG_biceps_right_total=0;
Renate 28:7c7508bdb21f 380 filtered_EMG_biceps_left_total=0;
Renate 28:7c7508bdb21f 381 filtered_EMG_calf_total=0;
Renate 28:7c7508bdb21f 382 }
Renate 28:7c7508bdb21f 383 if (i_calib <= 2500) {
Renate 28:7c7508bdb21f 384 filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
Renate 28:7c7508bdb21f 385 filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
Renate 28:7c7508bdb21f 386 filtered_EMG_calf_total+=filtered_EMG_calf;
Renate 28:7c7508bdb21f 387 i_calib++;
Renate 28:7c7508bdb21f 388 }
Renate 28:7c7508bdb21f 389 if (i_calib > 2500) {
Renate 28:7c7508bdb21f 390 mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
Renate 28:7c7508bdb21f 391 mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
Renate 28:7c7508bdb21f 392 mean_EMG_calf=filtered_EMG_calf_total/2500.0;
Renate 28:7c7508bdb21f 393 pc.printf("Ontspan spieren\r\n");
Renate 28:7c7508bdb21f 394 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 28:7c7508bdb21f 395 calib = false;
Renate 28:7c7508bdb21f 396 }
Renate 28:7c7508bdb21f 397 }
Renate 28:7c7508bdb21f 398
Renate 23:4572750a5c59 399 // Genormaliseerde EMG's berekenen
Renate 23:4572750a5c59 400 normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
Renate 23:4572750a5c59 401 normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
Renate 23:4572750a5c59 402 normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
Renate 28:7c7508bdb21f 403
Renate 28:7c7508bdb21f 404 // Finite state machine
Renate 28:7c7508bdb21f 405 switch (currentState) {
Renate 6:64146e16e10c 406 case Motors_off:
Renate 28:7c7508bdb21f 407
Renate 28:7c7508bdb21f 408 if (stateChanged) {
Renate 8:c7d3b67346db 409 motors_off(); // functie waarbij motoren uitgaan
Renate 11:4bc0304978e2 410 stateChanged = false;
Renate 9:4de589636f50 411 pc.printf("Motors off state\r\n");
Renate 28:7c7508bdb21f 412 }
Renate 29:8e0a7c33e4e7 413 if (Emergency_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 29:8e0a7c33e4e7 414 emergency();
Renate 29:8e0a7c33e4e7 415 }
Renate 28:7c7508bdb21f 416 if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 9:4de589636f50 417 currentState = Calib_motor;
Renate 11:4bc0304978e2 418 stateChanged = true;
Renate 11:4bc0304978e2 419 pc.printf("Moving to Calib_motor state\r\n");
Renate 6:64146e16e10c 420 }
Renate 6:64146e16e10c 421 break;
Renate 28:7c7508bdb21f 422
Renate 9:4de589636f50 423 case Calib_motor:
Renate 28:7c7508bdb21f 424
Renate 29:8e0a7c33e4e7 425 if (stateChanged) {
Renate 29:8e0a7c33e4e7 426 pc.printf("Zet motoren handmatig in home positie\r\n");
Renate 29:8e0a7c33e4e7 427 stateChanged = false;
Renate 29:8e0a7c33e4e7 428 }
Renate 29:8e0a7c33e4e7 429
Renate 29:8e0a7c33e4e7 430 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 431 emergency();
Renate 29:8e0a7c33e4e7 432 }
Renate 29:8e0a7c33e4e7 433 if (Motor_calib_button_pressed.read() == false) {
Renate 21:456acc79726c 434 theta_h_1_rad = 0;
Renate 21:456acc79726c 435 theta_h_2_rad = 0;
Renate 21:456acc79726c 436 pc.printf("Huidige hoek in radialen motor 1:%f en motor 2: %f (moet 0 zijn) \r\n", theta_h_1_rad, theta_h_2_rad);
Renate 11:4bc0304978e2 437 currentState = Calib_EMG;
Renate 11:4bc0304978e2 438 stateChanged = true;
Renate 9:4de589636f50 439 pc.printf("Moving to Calib_EMG state\r\n");
Renate 28:7c7508bdb21f 440 }
Renate 11:4bc0304978e2 441 break;
Renate 28:7c7508bdb21f 442
Renate 28:7c7508bdb21f 443 case Calib_EMG:
Renate 28:7c7508bdb21f 444
Renate 28:7c7508bdb21f 445 if (stateChanged) {
Renate 28:7c7508bdb21f 446 i_calib = 0;
Renate 28:7c7508bdb21f 447 calib = true;
Renate 28:7c7508bdb21f 448 pc.printf("Span spieren aan\r\n");
Renate 28:7c7508bdb21f 449 stateChanged = false;
Renate 28:7c7508bdb21f 450 }
Renate 28:7c7508bdb21f 451
Renate 29:8e0a7c33e4e7 452 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 453 emergency();
Renate 29:8e0a7c33e4e7 454 }
Renate 29:8e0a7c33e4e7 455
Renate 28:7c7508bdb21f 456 if (i_calib > 2500) {
Renate 28:7c7508bdb21f 457 calib = false;
Renate 28:7c7508bdb21f 458 currentState = Homing;
Renate 28:7c7508bdb21f 459 stateChanged = true;
Renate 28:7c7508bdb21f 460 pc.printf("Moving to Homing state\r\n");
Renate 28:7c7508bdb21f 461 }
Renate 28:7c7508bdb21f 462 break;
Renate 28:7c7508bdb21f 463
Renate 32:d651c23bbb77 464 case Homing:
Renate 28:7c7508bdb21f 465
Renate 28:7c7508bdb21f 466 if (stateChanged) {
Renate 28:7c7508bdb21f 467 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
Renate 11:4bc0304978e2 468 // (inclusief de end-effector) in de juiste home positie wordt gezet
Renate 28:7c7508bdb21f 469 stateChanged = false;
Renate 11:4bc0304978e2 470 }
Renate 28:7c7508bdb21f 471 if (Emergency_button_pressed.read() == false) {
Renate 10:83f3cec8dd1c 472 emergency();
Renate 28:7c7508bdb21f 473 }
Renate 29:8e0a7c33e4e7 474
Renate 33:f81f68a912b3 475 if (Homing_button_pressed.read() == false)
Renate 28:7c7508bdb21f 476 {
Renate 28:7c7508bdb21f 477 currentState = Operation_mode;
Renate 28:7c7508bdb21f 478 stateChanged = true;
Renate 29:8e0a7c33e4e7 479 pc.printf("Moving to operation mode \r\n");
Renate 28:7c7508bdb21f 480 }
Renate 29:8e0a7c33e4e7 481
Renate 28:7c7508bdb21f 482 break;
Renate 28:7c7508bdb21f 483
Renate 28:7c7508bdb21f 484 case Operation_mode: // Overgaan tot emergency wanneer referentie niet
Renate 28:7c7508bdb21f 485 // overeenkomt met werkelijkheid
Renate 28:7c7508bdb21f 486
Renate 23:4572750a5c59 487 // pc.printf("normalized_EMG_biceps_right= %f, mean_EMG_biceps_right = %f, filtered_EMG_biceps_right = %f\r\n", normalized_EMG_biceps_right, mean_EMG_biceps_right, filtered_EMG_biceps_right);
Renate 29:8e0a7c33e4e7 488 if (stateChanged) {
Renate 29:8e0a7c33e4e7 489 motors_off();
Renate 30:0a328a9a4788 490 Joint_1_position_x = 0;
Renate 30:0a328a9a4788 491 Joint_2_position_x = 0;
Renate 30:0a328a9a4788 492 Joint_1_position_y = 0;
Renate 30:0a328a9a4788 493 Joint_2_position_y = 0;
Renate 30:0a328a9a4788 494 Joint_1_position_x_prev = Joint_1_position_x;
Renate 30:0a328a9a4788 495 Joint_2_position_x_prev = Joint_2_position_x;
Renate 30:0a328a9a4788 496 Joint_1_position_y_prev = Joint_1_position_y;
Renate 30:0a328a9a4788 497 Joint_2_position_y_prev = Joint_2_position_y;
Renate 29:8e0a7c33e4e7 498 Joint_velocity_x[0][0] = 0;
Renate 29:8e0a7c33e4e7 499 Joint_velocity_x[1][0] = 0;
Renate 29:8e0a7c33e4e7 500 Joint_velocity_y[0][0] = 0;
Renate 29:8e0a7c33e4e7 501 Joint_velocity_y[1][0] = 0;
Renate 30:0a328a9a4788 502 Motor_1_position_x = 0;
Renate 30:0a328a9a4788 503 Motor_2_position_x = 0;
Renate 30:0a328a9a4788 504 Motor_1_position_y = 0;
Renate 30:0a328a9a4788 505 Motor_2_position_y = 0;
Renate 31:967b455bc328 506 theta_k_1_x = 0.0;
Renate 31:967b455bc328 507 theta_k_2_x = 0.0;
Renate 31:967b455bc328 508 error_integral_1_x = 0.0;
Renate 31:967b455bc328 509 error_integral_2_x = 0.0;
Renate 31:967b455bc328 510 theta_k_1_y = 0.0;
Renate 31:967b455bc328 511 theta_k_2_y = 0.0;
Renate 31:967b455bc328 512 error_integral_1_y = 0.0;
Renate 31:967b455bc328 513 error_integral_2_y = 0.0;
Renate 29:8e0a7c33e4e7 514 stateChanged = false;
Renate 29:8e0a7c33e4e7 515 }
Renate 29:8e0a7c33e4e7 516 // Hier moet een functie worden aangeroepen die ervoor zorgt dat
Renate 29:8e0a7c33e4e7 517 // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
Renate 29:8e0a7c33e4e7 518 // zodat de robotarm kan bewegen
Renate 28:7c7508bdb21f 519
Renate 29:8e0a7c33e4e7 520 // if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 29:8e0a7c33e4e7 521 // motors_off();
Renate 29:8e0a7c33e4e7 522 // currentState = Motors_off;
Renate 29:8e0a7c33e4e7 523 // stateChanged = true;
Renate 29:8e0a7c33e4e7 524 // pc.printf("Terug naar de state Motors_off\r\n");
Renate 29:8e0a7c33e4e7 525 // }
Renate 29:8e0a7c33e4e7 526 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 527 emergency();
Renate 29:8e0a7c33e4e7 528 }
Renate 29:8e0a7c33e4e7 529 if (Motor_calib_button_pressed.read() == false) { // Is nu voor de homing
Renate 29:8e0a7c33e4e7 530 motors_off();
Renate 29:8e0a7c33e4e7 531 currentState = Homing;
Renate 29:8e0a7c33e4e7 532 stateChanged = true;
Renate 29:8e0a7c33e4e7 533 pc.printf("Terug naar de state Homing\r\n");
Renate 29:8e0a7c33e4e7 534 }
Renate 29:8e0a7c33e4e7 535 if (normalized_EMG_biceps_right >= 0.3) {
Renate 31:967b455bc328 536
Renate 32:d651c23bbb77 537 if (normalized_EMG_calf < 0.3) {
Renate 31:967b455bc328 538 vx = 0.0;
Renate 31:967b455bc328 539 vy = 0.05;
Renate 31:967b455bc328 540 }
Renate 32:d651c23bbb77 541 if (normalized_EMG_calf >= 0.3) {
Renate 31:967b455bc328 542 vx = 0.0;
Renate 31:967b455bc328 543 vy = -0.05;
Renate 31:967b455bc328 544 }
Renate 30:0a328a9a4788 545 Inverse_Kinematics();
Renate 30:0a328a9a4788 546 error_q1_y = Motor_1_position_y - theta_h_1_rad;
Renate 31:967b455bc328 547 error_q2_y = Motor_2_position_y - theta_h_2_rad;
Renate 30:0a328a9a4788 548 PI_controller_y();
Renate 33:f81f68a912b3 549
Renate 31:967b455bc328 550 abs_theta_t_1_y = abs(theta_t_1_y);
Renate 31:967b455bc328 551 abs_theta_t_2_y = abs(theta_t_2_y);
Renate 30:0a328a9a4788 552
Renate 32:d651c23bbb77 553 motor1.write(abs_theta_t_1_y);
Renate 32:d651c23bbb77 554 motor2.write(abs_theta_t_2_y);
Renate 32:d651c23bbb77 555 Define_motor_dir_y();
Renate 28:7c7508bdb21f 556
Renate 29:8e0a7c33e4e7 557 } else if (normalized_EMG_biceps_left >= 0.3) {
Renate 32:d651c23bbb77 558 if (normalized_EMG_calf < 0.3) {
Renate 32:d651c23bbb77 559 vx = 0.05;
Renate 32:d651c23bbb77 560 vy = 0.0;
Renate 32:d651c23bbb77 561 }
Renate 32:d651c23bbb77 562 if (normalized_EMG_calf >= 0.3) {
Renate 32:d651c23bbb77 563 vx = -0.05;
Renate 32:d651c23bbb77 564 vy = 0.0;
Renate 32:d651c23bbb77 565 }
Renate 31:967b455bc328 566 Inverse_Kinematics();
Renate 31:967b455bc328 567 error_q1_x = Motor_1_position_x - theta_h_1_rad;
Renate 31:967b455bc328 568 error_q2_x = Motor_2_position_x - theta_h_2_rad;
Renate 31:967b455bc328 569 PI_controller_x();
Renate 31:967b455bc328 570
Renate 32:d651c23bbb77 571 abs_theta_t_1_x = abs(theta_t_1_x);
Renate 32:d651c23bbb77 572 abs_theta_t_2_x = abs(theta_t_2_x);
Renate 31:967b455bc328 573
Renate 32:d651c23bbb77 574 motor1.write(abs_theta_t_1_x);
Renate 32:d651c23bbb77 575 motor2.write(abs_theta_t_2_x);
Renate 32:d651c23bbb77 576 Define_motor_dir_x();
Renate 32:d651c23bbb77 577
Renate 29:8e0a7c33e4e7 578 } else {
Renate 33:f81f68a912b3 579 vx = 0.0;
Renate 33:f81f68a912b3 580 vy = 0.0;
Renate 33:f81f68a912b3 581
Renate 33:f81f68a912b3 582 Inverse_Kinematics();
Renate 33:f81f68a912b3 583 error_q1_x = Motor_1_position_x - theta_h_1_rad;
Renate 33:f81f68a912b3 584 error_q2_x = Motor_2_position_x - theta_h_2_rad;
Renate 33:f81f68a912b3 585 error_q1_y = Motor_1_position_y - theta_h_1_rad;
Renate 33:f81f68a912b3 586 error_q2_y = Motor_2_position_y - theta_h_2_rad;
Renate 33:f81f68a912b3 587 PI_controller_x();
Renate 33:f81f68a912b3 588 PI_controller_y();
Renate 33:f81f68a912b3 589
Renate 33:f81f68a912b3 590 abs_theta_t_1_x = abs(theta_t_1_x);
Renate 33:f81f68a912b3 591 abs_theta_t_2_x = abs(theta_t_2_x);
Renate 33:f81f68a912b3 592 abs_theta_t_1_y = abs(theta_t_1_y);
Renate 33:f81f68a912b3 593 abs_theta_t_2_y = abs(theta_t_2_y);
Renate 33:f81f68a912b3 594
Renate 33:f81f68a912b3 595 motor1.write(abs_theta_t_1_x);
Renate 33:f81f68a912b3 596 motor2.write(abs_theta_t_2_x);
Renate 33:f81f68a912b3 597 Define_motor_dir_x();
Renate 33:f81f68a912b3 598 motor1.write(abs_theta_t_1_y);
Renate 33:f81f68a912b3 599 motor2.write(abs_theta_t_2_y);
Renate 33:f81f68a912b3 600 Define_motor_dir_y();
Renate 33:f81f68a912b3 601
Renate 28:7c7508bdb21f 602 }
Renate 28:7c7508bdb21f 603
Renate 21:456acc79726c 604 break;
Renate 28:7c7508bdb21f 605
Renate 7:1d57463393c6 606 default:
Renate 7:1d57463393c6 607 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 14:54343b9fd708 608 motors_off();
Renate 9:4de589636f50 609 pc.printf("Unknown or uninplemented state reached!\r\n");
Renate 28:7c7508bdb21f 610
Renate 33:f81f68a912b3 611 }
Renate 33:f81f68a912b3 612 }
Renate 33:f81f68a912b3 613
Renate 33:f81f68a912b3 614 void Print()
Renate 33:f81f68a912b3 615 {
Renate 33:f81f68a912b3 616 pc.printf("Joint_velocity_x[0][0] = %f, Joint_velocity_x[1][0] =%f\r\n", Joint_velocity_x[0][0],Joint_velocity_x[1][0]);
Renate 33:f81f68a912b3 617 pc.printf("Joint_1_position_x =%f, Joint_2_position_x = %f\r\n", Joint_1_position_x , Joint_2_position_x);
Renate 11:4bc0304978e2 618 }
WiesjeRoskamp 2:aee655d11b6d 619
Renate 8:c7d3b67346db 620 int main(void)
Renate 28:7c7508bdb21f 621 {
Renate 28:7c7508bdb21f 622 pc.printf("Opstarten\r\n");
Renate 23:4572750a5c59 623
Renate 33:f81f68a912b3 624 motor1.period_us(56);
Renate 33:f81f68a912b3 625 motor2.period_us(56);
Renate 33:f81f68a912b3 626
Renate 28:7c7508bdb21f 627 // Chain voor rechter biceps
Renate 28:7c7508bdb21f 628 bqcbr.add(&bqbr1).add(&bqbr2);
Renate 28:7c7508bdb21f 629 bqcbr2.add(&bqbr3).add(&bqbr4);
Renate 28:7c7508bdb21f 630 // Chain voor linker biceps
Renate 28:7c7508bdb21f 631 bqcbl.add(&bqbl1).add(&bqbl2);
Renate 28:7c7508bdb21f 632 bqcbl2.add(&bqbl3).add(&bqbl4);
Renate 28:7c7508bdb21f 633 // Chain voor kuit
Renate 28:7c7508bdb21f 634 bqck.add(&bqk1).add(&bqk2);
Renate 28:7c7508bdb21f 635 bqck2.add(&bqk3).add(&bqk4);
Renate 28:7c7508bdb21f 636
Renate 28:7c7508bdb21f 637 loop_ticker.attach(&ProcessStateMachine, 0.002f);
Renate 33:f81f68a912b3 638 loop_prints.attach(&Print, 0.5f);
Renate 28:7c7508bdb21f 639
Renate 28:7c7508bdb21f 640 while(true) {
Renate 28:7c7508bdb21f 641 // wait(0.2);
Renate 28:7c7508bdb21f 642 /* do nothing */
Renate 28:7c7508bdb21f 643 }
Renate 28:7c7508bdb21f 644 }