Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@29:8e0a7c33e4e7, 2019-10-29 (annotated)

Committer:
Renate
Date:
Tue Oct 29 18:24:42 2019 +0000
Revision:
29:8e0a7c33e4e7
Parent:
28:7c7508bdb21f
Child:
30:0a328a9a4788
Toegevoegd stuk RKI;

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 21:456acc79726c 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 15:ad065ab92d11 17
Renate 23:4572750a5c59 18 // BENODIGD VOOR PROCESS STATE MACHINE
Renate 23:4572750a5c59 19 enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
Renate 28:7c7508bdb21f 20 states currentState = Motors_off;
Renate 23:4572750a5c59 21 bool stateChanged = true; // Make sure the initialization of first state is executed
Renate 23:4572750a5c59 22
Renate 23:4572750a5c59 23 // INPUTS
Renate 14:54343b9fd708 24 DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken!
Renate 9:4de589636f50 25 DigitalIn Emergency_button_pressed(D2);
Renate 22:8585d41a670b 26 DigitalIn Motor_calib_button_pressed(SW2);
WiesjeRoskamp 2:aee655d11b6d 27
Renate 15:ad065ab92d11 28 AnalogIn EMG_biceps_right_raw (A0);
Renate 15:ad065ab92d11 29 AnalogIn EMG_biceps_left_raw (A1);
Renate 19:1fd39a2afc30 30 AnalogIn EMG_calf_raw (A2);
Renate 15:ad065ab92d11 31
Renate 23:4572750a5c59 32 QEI Encoder1(D12, D13, NC, 8400, QEI::X4_ENCODING); //Checken of die D12, D9 etc wel kloppen, 8400= gear ratio x 64
Renate 28:7c7508bdb21f 33 QEI Encoder2(D9, D10, NC, 8400, QEI::X4_ENCODING);
Renate 21:456acc79726c 34
Renate 23:4572750a5c59 35 // OUTPUTS
Renate 28:7c7508bdb21f 36 PwmOut motor1(D6); // Misschien moeten we hiervoor DigitalOut gebruiken, moet
Renate 23:4572750a5c59 37 PwmOut motor2(D5); // samen kunnen gaan met de servo motor
Renate 21:456acc79726c 38
Renate 23:4572750a5c59 39 DigitalOut motor1_dir(D7);
Renate 23:4572750a5c59 40 DigitalOut motor2_dir(D4);
Renate 23:4572750a5c59 41
Renate 23:4572750a5c59 42 // VARIABELEN VOOR ENCODER, MOTORHOEK ETC.
Renate 21:456acc79726c 43 int counts1;
Renate 21:456acc79726c 44 int counts2;
Renate 21:456acc79726c 45 const int CPR = 64; // Counts per revolution
Renate 21:456acc79726c 46 const int full_degrees = 360;
Renate 21:456acc79726c 47 const int half_degrees = 180;
Renate 21:456acc79726c 48 double theta_h_1_deg;
Renate 21:456acc79726c 49 double theta_h_2_deg;
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 29:8e0a7c33e4e7 127 double Joint_1_position_x;
Renate 29:8e0a7c33e4e7 128 double Joint_2_position_x;
Renate 29:8e0a7c33e4e7 129
Renate 29:8e0a7c33e4e7 130 double Joint_velocity_x[2][1];
Renate 29:8e0a7c33e4e7 131 double Joint_velocity_y[2][1];
Renate 29:8e0a7c33e4e7 132
Renate 29:8e0a7c33e4e7 133 double q1_dot_x;
Renate 29:8e0a7c33e4e7 134 double q2_dot_x;
Renate 29:8e0a7c33e4e7 135
Renate 29:8e0a7c33e4e7 136 double q1_dot_y;
Renate 29:8e0a7c33e4e7 137 double q2_dot_y;
Renate 29:8e0a7c33e4e7 138
Renate 29:8e0a7c33e4e7 139 double q1;
Renate 29:8e0a7c33e4e7 140 double q2;
Renate 29:8e0a7c33e4e7 141
Renate 29:8e0a7c33e4e7 142 double Motor_1_velocity_x;
Renate 29:8e0a7c33e4e7 143 double Motor_2_velocity_x;
Renate 29:8e0a7c33e4e7 144
Renate 29:8e0a7c33e4e7 145 double Motor_1_velocity_y;
Renate 29:8e0a7c33e4e7 146 double Motor_2_velocity_y;
Renate 29:8e0a7c33e4e7 147
Renate 29:8e0a7c33e4e7 148
Renate 23:4572750a5c59 149 // VOIDS
Renate 23:4572750a5c59 150
Renate 23:4572750a5c59 151 // Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden).
Renate 28:7c7508bdb21f 152 // Enige optie is resetten, dan wordt het script opnieuw opgestart.
Renate 8:c7d3b67346db 153 void emergency()
Renate 28:7c7508bdb21f 154 {
Renate 28:7c7508bdb21f 155 loop_ticker.detach();
Renate 28:7c7508bdb21f 156 motor1.write(0);
Renate 28:7c7508bdb21f 157 motor2.write(0);
Renate 28:7c7508bdb21f 158 pc.printf("Ik ga exploderen!!!\r\n");
Renate 28:7c7508bdb21f 159 }
Renate 11:4bc0304978e2 160
Renate 28:7c7508bdb21f 161 // Motoren uitzetten
Renate 8:c7d3b67346db 162 void motors_off()
Renate 28:7c7508bdb21f 163 {
Renate 28:7c7508bdb21f 164 motor1.write(0);
Renate 29:8e0a7c33e4e7 165 motor1_dir.write(1);
Renate 28:7c7508bdb21f 166 motor2.write(0);
Renate 29:8e0a7c33e4e7 167 motor2_dir.write(1);
Renate 28:7c7508bdb21f 168 pc.printf("Motoren uit functie\r\n");
Renate 28:7c7508bdb21f 169 }
Renate 28:7c7508bdb21f 170
Renate 14:54343b9fd708 171 // Motoren aanzetten
Renate 15:ad065ab92d11 172 void motors_on()
Renate 28:7c7508bdb21f 173 {
Renate 28:7c7508bdb21f 174 motor1.write(0.9);
Renate 28:7c7508bdb21f 175 motor1_dir.write(1);
Renate 28:7c7508bdb21f 176 motor2.write(0.1);
Renate 28:7c7508bdb21f 177 motor2_dir.write(1);
Renate 28:7c7508bdb21f 178 pc.printf("Motoren aan functie\r\n");
Renate 28:7c7508bdb21f 179 }
Rosalie 3:6ee0b20c23b0 180
Renate 29:8e0a7c33e4e7 181 void Inverse_Kinematics()
Renate 29:8e0a7c33e4e7 182 {
Renate 29:8e0a7c33e4e7 183 // Defining joint velocities based om calculations of matlab
Renate 29:8e0a7c33e4e7 184
Renate 29:8e0a7c33e4e7 185 Joint_velocity_x[0][0] = (vx*(exp(q2*sqrt(-1.0))*(-1.798599718146044E18+6.546367607647411E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*(-1.798599718146044E18-6.546367607647411E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*(1.168095770314175E18+1.321169505615574E18*sqrt(-1.0))+exp(q1*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*3.828059683264922E18+1.168095770314175E18-1.321169505615574E18*sqrt(-1.0))*(2.0E1/1.7E1))/(cos(q1+3.141592653589793/9.0)*(6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17+6.605847528077872E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17-3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17+3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17-6.605847528077872E17*sqrt(-1.0)));
Renate 29:8e0a7c33e4e7 186 Joint_velocity_x[1][0] = (vx*(exp(q2*sqrt(-1.0))*(1.798599718146044E18-6.546367607647411E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*(1.798599718146044E18+6.546367607647411E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*(-1.168095770314175E18-1.321169505615574E18*sqrt(-1.0))+-1.168095770314175E18+1.321169505615574E18*sqrt(-1.0))*(2.0E1/1.7E1))/(cos(q1+3.141592653589793/9.0)*(6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17+6.605847528077872E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17-3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17+3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17-6.605847528077872E17*sqrt(-1.0)));
Renate 29:8e0a7c33e4e7 187
Renate 29:8e0a7c33e4e7 188 Joint_velocity_y[0][0] = (vy*(exp(q2*sqrt(-1.0))*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*1.914029841632461E18+6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))*(4.0E1/1.7E1))/(cos(q1+3.141592653589793/9.0)*(6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17+6.605847528077872E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17-3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17+3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17-6.605847528077872E17*sqrt(-1.0)));
Renate 29:8e0a7c33e4e7 189 Joint_velocity_y[1][0] = (vy*(exp(q2*sqrt(-1.0))*(3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*(3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*(-6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+-6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))*(4.0E1/1.7E1))/(cos(q1+3.141592653589793/9.0)*(6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17+6.605847528077872E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17-3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17+3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17-6.605847528077872E17*sqrt(-1.0)));
Renate 29:8e0a7c33e4e7 190
Renate 29:8e0a7c33e4e7 191 q1_dot_x = real(Joint_velocity_x[0][0]);
Renate 29:8e0a7c33e4e7 192 q2_dot_x = real(Joint_velocity_x[1][0]);
Renate 29:8e0a7c33e4e7 193
Renate 29:8e0a7c33e4e7 194 q1_dot_y = real(Joint_velocity_y[0][0]);
Renate 29:8e0a7c33e4e7 195 q2_dot_y = real(Joint_velocity_y[1][0]);
Renate 29:8e0a7c33e4e7 196
Renate 29:8e0a7c33e4e7 197 // Integratie
Renate 29:8e0a7c33e4e7 198 Joint_1_position_x = Joint_1_position_x + Joint_velocity_x[0][0]*delta_t;
Renate 29:8e0a7c33e4e7 199 Joint_2_position_x = Joint_2_position_x + Joint_velocity_x[1][0]*delta_t;
Renate 29:8e0a7c33e4e7 200
Renate 29:8e0a7c33e4e7 201 Joint1_position_y = Joint_1_position_y + Joint_velocity_y[0][0]*delta_t;
Renate 29:8e0a7c33e4e7 202 Joint2_position_y = Joint_2_position_y + Joint_velocity_y[1][0]*delta_t;
Renate 29:8e0a7c33e4e7 203
Renate 29:8e0a7c33e4e7 204 Motor_1_position_x = Joint_position_x[0][0];
Renate 29:8e0a7c33e4e7 205 Motor_2_position_x = Joint_position_x[0][0] + Joint_position_x[1][0];
Renate 29:8e0a7c33e4e7 206
Renate 29:8e0a7c33e4e7 207 Motor_1_position_y = Joint_position_y[0][0];
Renate 29:8e0a7c33e4e7 208 Motor_2_position_y = Joint_position_y[0][0] + Joint_position_y[1][0];
Renate 29:8e0a7c33e4e7 209 }
Renate 29:8e0a7c33e4e7 210
Renate 6:64146e16e10c 211 // Finite state machine programming (calibration servo motor?)
Renate 28:7c7508bdb21f 212 void ProcessStateMachine(void)
Renate 28:7c7508bdb21f 213 {
Renate 23:4572750a5c59 214 // Berekenen van de motorhoeken (in radialen)
Renate 28:7c7508bdb21f 215 counts1 = Encoder1.getPulses();
Renate 28:7c7508bdb21f 216 counts2 = Encoder2.getPulses();
Renate 23:4572750a5c59 217 theta_h_1_deg=(counts1/(double)CPR)*(double)full_degrees;
Renate 23:4572750a5c59 218 theta_h_2_deg=(counts2/(double)CPR)*(double)full_degrees;
Renate 23:4572750a5c59 219 theta_h_1_rad=(theta_h_1_deg/half_degrees)*M_PI;
Renate 23:4572750a5c59 220 theta_h_2_rad=(theta_h_2_deg/half_degrees)*M_PI;
Renate 28:7c7508bdb21f 221
Renate 29:8e0a7c33e4e7 222 // Calculating joint angles based on motor angles (current encoder values)
Renate 29:8e0a7c33e4e7 223 q1 = theta_h_1_rad; // Relative angle joint 1 (rad)
Renate 29:8e0a7c33e4e7 224 q2 = theta_h_2_rad - theta_h_1_rad; // Relative angle joint 2 (rad)
Renate 29:8e0a7c33e4e7 225
Renate 23:4572750a5c59 226 // Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal
Renate 28:7c7508bdb21f 227 // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled'
Renate 23:4572750a5c59 228 filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
Renate 23:4572750a5c59 229 filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
Renate 23:4572750a5c59 230 filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
Renate 28:7c7508bdb21f 231
Renate 23:4572750a5c59 232 // Vervolgens wordt de absolute waarde hiervan genomen
Renate 23:4572750a5c59 233 filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
Renate 23:4572750a5c59 234 filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
Renate 23:4572750a5c59 235 filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
Renate 28:7c7508bdb21f 236
Renate 23:4572750a5c59 237 // Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter)
Renate 23:4572750a5c59 238 // over het signaal gedaan
Renate 23:4572750a5c59 239 filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
Renate 23:4572750a5c59 240 filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
Renate 23:4572750a5c59 241 filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
Renate 28:7c7508bdb21f 242
Renate 28:7c7508bdb21f 243 // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope
Renate 28:7c7508bdb21f 244 scope.set(0, normalized_EMG_biceps_right);
Renate 28:7c7508bdb21f 245 scope.set(1, normalized_EMG_biceps_left);
Renate 28:7c7508bdb21f 246 scope.set(2, normalized_EMG_calf);
Renate 23:4572750a5c59 247 scope.send();
Renate 28:7c7508bdb21f 248
Renate 28:7c7508bdb21f 249 // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die
Renate 28:7c7508bdb21f 250 // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende
Renate 23:4572750a5c59 251 // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
Renate 28:7c7508bdb21f 252 // waarna het gemiddelde wordt bepaald.
Renate 28:7c7508bdb21f 253 if (calib) {
Renate 28:7c7508bdb21f 254 if (i_calib == 0) {
Renate 28:7c7508bdb21f 255 filtered_EMG_biceps_right_total=0;
Renate 28:7c7508bdb21f 256 filtered_EMG_biceps_left_total=0;
Renate 28:7c7508bdb21f 257 filtered_EMG_calf_total=0;
Renate 28:7c7508bdb21f 258 }
Renate 28:7c7508bdb21f 259 if (i_calib <= 2500) {
Renate 28:7c7508bdb21f 260 filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
Renate 28:7c7508bdb21f 261 filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
Renate 28:7c7508bdb21f 262 filtered_EMG_calf_total+=filtered_EMG_calf;
Renate 28:7c7508bdb21f 263 i_calib++;
Renate 28:7c7508bdb21f 264 }
Renate 28:7c7508bdb21f 265 if (i_calib > 2500) {
Renate 28:7c7508bdb21f 266 mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
Renate 28:7c7508bdb21f 267 mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
Renate 28:7c7508bdb21f 268 mean_EMG_calf=filtered_EMG_calf_total/2500.0;
Renate 28:7c7508bdb21f 269 pc.printf("Ontspan spieren\r\n");
Renate 28:7c7508bdb21f 270 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 271 calib = false;
Renate 28:7c7508bdb21f 272 }
Renate 28:7c7508bdb21f 273 }
Renate 28:7c7508bdb21f 274
Renate 23:4572750a5c59 275 // Genormaliseerde EMG's berekenen
Renate 23:4572750a5c59 276 normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
Renate 23:4572750a5c59 277 normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
Renate 23:4572750a5c59 278 normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
Renate 28:7c7508bdb21f 279
Renate 28:7c7508bdb21f 280 // Finite state machine
Renate 28:7c7508bdb21f 281 switch (currentState) {
Renate 6:64146e16e10c 282 case Motors_off:
Renate 28:7c7508bdb21f 283
Renate 28:7c7508bdb21f 284 if (stateChanged) {
Renate 8:c7d3b67346db 285 motors_off(); // functie waarbij motoren uitgaan
Renate 11:4bc0304978e2 286 stateChanged = false;
Renate 9:4de589636f50 287 pc.printf("Motors off state\r\n");
Renate 28:7c7508bdb21f 288 }
Renate 29:8e0a7c33e4e7 289 if (Emergency_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 29:8e0a7c33e4e7 290 emergency();
Renate 29:8e0a7c33e4e7 291 }
Renate 28:7c7508bdb21f 292 if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 9:4de589636f50 293 currentState = Calib_motor;
Renate 11:4bc0304978e2 294 stateChanged = true;
Renate 11:4bc0304978e2 295 pc.printf("Moving to Calib_motor state\r\n");
Renate 6:64146e16e10c 296 }
Renate 6:64146e16e10c 297 break;
Renate 28:7c7508bdb21f 298
Renate 9:4de589636f50 299 case Calib_motor:
Renate 28:7c7508bdb21f 300
Renate 29:8e0a7c33e4e7 301 if (stateChanged) {
Renate 29:8e0a7c33e4e7 302 pc.printf("Zet motoren handmatig in home positie\r\n");
Renate 29:8e0a7c33e4e7 303 stateChanged = false;
Renate 29:8e0a7c33e4e7 304 }
Renate 29:8e0a7c33e4e7 305
Renate 29:8e0a7c33e4e7 306 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 307 emergency();
Renate 29:8e0a7c33e4e7 308 }
Renate 29:8e0a7c33e4e7 309 if (Motor_calib_button_pressed.read() == false) {
Renate 21:456acc79726c 310 theta_h_1_rad = 0;
Renate 21:456acc79726c 311 theta_h_2_rad = 0;
Renate 21:456acc79726c 312 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 313 currentState = Calib_EMG;
Renate 11:4bc0304978e2 314 stateChanged = true;
Renate 9:4de589636f50 315 pc.printf("Moving to Calib_EMG state\r\n");
Renate 28:7c7508bdb21f 316 }
Renate 11:4bc0304978e2 317 break;
Renate 28:7c7508bdb21f 318
Renate 28:7c7508bdb21f 319 case Calib_EMG:
Renate 28:7c7508bdb21f 320
Renate 28:7c7508bdb21f 321 if (stateChanged) {
Renate 28:7c7508bdb21f 322 i_calib = 0;
Renate 28:7c7508bdb21f 323 calib = true;
Renate 28:7c7508bdb21f 324 pc.printf("Span spieren aan\r\n");
Renate 28:7c7508bdb21f 325 stateChanged = false;
Renate 28:7c7508bdb21f 326 }
Renate 28:7c7508bdb21f 327
Renate 29:8e0a7c33e4e7 328 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 329 emergency();
Renate 29:8e0a7c33e4e7 330 }
Renate 29:8e0a7c33e4e7 331
Renate 28:7c7508bdb21f 332 if (i_calib > 2500) {
Renate 28:7c7508bdb21f 333 calib = false;
Renate 28:7c7508bdb21f 334 currentState = Homing;
Renate 28:7c7508bdb21f 335 stateChanged = true;
Renate 28:7c7508bdb21f 336 pc.printf("Moving to Homing state\r\n");
Renate 28:7c7508bdb21f 337 }
Renate 28:7c7508bdb21f 338 break;
Renate 28:7c7508bdb21f 339
Renate 28:7c7508bdb21f 340 case Homing: // NOG NAAR KIJKEN
Renate 28:7c7508bdb21f 341
Renate 28:7c7508bdb21f 342 if (stateChanged) {
Renate 28:7c7508bdb21f 343 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
Renate 11:4bc0304978e2 344 // (inclusief de end-effector) in de juiste home positie wordt gezet
Renate 21:456acc79726c 345 motors_on();
Renate 28:7c7508bdb21f 346 stateChanged = false;
Renate 11:4bc0304978e2 347 }
Renate 28:7c7508bdb21f 348 if (Emergency_button_pressed.read() == false) {
Renate 10:83f3cec8dd1c 349 emergency();
Renate 28:7c7508bdb21f 350 }
Renate 29:8e0a7c33e4e7 351
Renate 29:8e0a7c33e4e7 352 // Hier moet iets van encoder counts waarde maal -1 worden gedaan om
Renate 29:8e0a7c33e4e7 353 // naar positie 0 te bewegen
Renate 29:8e0a7c33e4e7 354
Renate 29:8e0a7c33e4e7 355 if (Power_button_pressed.read() == false)
Renate 29:8e0a7c33e4e7 356 //if (theta_h_1_rad == 0.0 && theta_h_2_rad == 0.0)
Renate 29:8e0a7c33e4e7 357 // Veranderen wanneer encoders werken en motor in elkaar zit
Renate 28:7c7508bdb21f 358 {
Renate 28:7c7508bdb21f 359 currentState = Operation_mode;
Renate 28:7c7508bdb21f 360 stateChanged = true;
Renate 29:8e0a7c33e4e7 361 pc.printf("Moving to operation mode \r\n");
Renate 28:7c7508bdb21f 362 }
Renate 29:8e0a7c33e4e7 363
Renate 28:7c7508bdb21f 364 break;
Renate 28:7c7508bdb21f 365
Renate 28:7c7508bdb21f 366 case Operation_mode: // Overgaan tot emergency wanneer referentie niet
Renate 28:7c7508bdb21f 367 // overeenkomt met werkelijkheid
Renate 28:7c7508bdb21f 368
Renate 23:4572750a5c59 369 // 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 370 if (stateChanged) {
Renate 29:8e0a7c33e4e7 371 motors_off();
Renate 29:8e0a7c33e4e7 372 Joint_velocity_x[0][0] = 0;
Renate 29:8e0a7c33e4e7 373 Joint_velocity_x[1][0] = 0;
Renate 29:8e0a7c33e4e7 374 Joint_velocity_y[0][0] = 0;
Renate 29:8e0a7c33e4e7 375 Joint_velocity_y[1][0] = 0;
Renate 29:8e0a7c33e4e7 376 stateChanged = false;
Renate 29:8e0a7c33e4e7 377 }
Renate 29:8e0a7c33e4e7 378 // Hier moet een functie worden aangeroepen die ervoor zorgt dat
Renate 29:8e0a7c33e4e7 379 // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
Renate 29:8e0a7c33e4e7 380 // zodat de robotarm kan bewegen
Renate 28:7c7508bdb21f 381
Renate 29:8e0a7c33e4e7 382 // if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 29:8e0a7c33e4e7 383 // motors_off();
Renate 29:8e0a7c33e4e7 384 // currentState = Motors_off;
Renate 29:8e0a7c33e4e7 385 // stateChanged = true;
Renate 29:8e0a7c33e4e7 386 // pc.printf("Terug naar de state Motors_off\r\n");
Renate 29:8e0a7c33e4e7 387 // }
Renate 29:8e0a7c33e4e7 388 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 389 emergency();
Renate 29:8e0a7c33e4e7 390 }
Renate 29:8e0a7c33e4e7 391 if (Motor_calib_button_pressed.read() == false) { // Is nu voor de homing
Renate 29:8e0a7c33e4e7 392 motors_off();
Renate 29:8e0a7c33e4e7 393 currentState = Homing;
Renate 29:8e0a7c33e4e7 394 stateChanged = true;
Renate 29:8e0a7c33e4e7 395 pc.printf("Terug naar de state Homing\r\n");
Renate 29:8e0a7c33e4e7 396 }
Renate 29:8e0a7c33e4e7 397 if (normalized_EMG_biceps_right >= 0.3) {
Renate 29:8e0a7c33e4e7 398 motor1.write(0.3);
Renate 29:8e0a7c33e4e7 399 motor2.write(0.3);
Renate 29:8e0a7c33e4e7 400 motor1_dir.write(0);
Renate 29:8e0a7c33e4e7 401 motor2_dir.write(0);
Renate 29:8e0a7c33e4e7 402 //if (normalized_EMG_calf >= 0.3)
Renate 29:8e0a7c33e4e7 403 //{
Renate 29:8e0a7c33e4e7 404 //motor1.write(0.1);
Renate 29:8e0a7c33e4e7 405 //motor1_dir = !motor1_dir;
Renate 29:8e0a7c33e4e7 406 //}
Renate 28:7c7508bdb21f 407
Renate 29:8e0a7c33e4e7 408 } else if (normalized_EMG_biceps_left >= 0.3) {
Renate 29:8e0a7c33e4e7 409 motor2.write(0.9);
Renate 29:8e0a7c33e4e7 410 motor1.write(0.9);
Renate 29:8e0a7c33e4e7 411 motor1_dir.write(1);
Renate 29:8e0a7c33e4e7 412 motor2_dir.write(1);
Renate 28:7c7508bdb21f 413 //if (normalized_EMG_calf >= 0.3)
Renate 28:7c7508bdb21f 414 //{
Renate 28:7c7508bdb21f 415 // motor1_dir = !motor1_dir;
Renate 28:7c7508bdb21f 416 // pc.printf("Richting zou om moeten draaien");
Renate 28:7c7508bdb21f 417 // motor2_dir = !motor2_dir;
Renate 28:7c7508bdb21f 418 //}
Renate 29:8e0a7c33e4e7 419 } else {
Renate 29:8e0a7c33e4e7 420 motor1.write(0);
Renate 29:8e0a7c33e4e7 421 motor2.write(0);
Renate 28:7c7508bdb21f 422 }
Renate 28:7c7508bdb21f 423
Renate 21:456acc79726c 424 break;
Renate 28:7c7508bdb21f 425
Renate 7:1d57463393c6 426 default:
Renate 7:1d57463393c6 427 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 14:54343b9fd708 428 motors_off();
Renate 9:4de589636f50 429 pc.printf("Unknown or uninplemented state reached!\r\n");
Renate 28:7c7508bdb21f 430
WiesjeRoskamp 2:aee655d11b6d 431 }
Renate 11:4bc0304978e2 432 }
WiesjeRoskamp 2:aee655d11b6d 433
Renate 8:c7d3b67346db 434 int main(void)
Renate 28:7c7508bdb21f 435 {
Renate 28:7c7508bdb21f 436 pc.printf("Opstarten\r\n");
Renate 23:4572750a5c59 437
Renate 28:7c7508bdb21f 438 // Chain voor rechter biceps
Renate 28:7c7508bdb21f 439 bqcbr.add(&bqbr1).add(&bqbr2);
Renate 28:7c7508bdb21f 440 bqcbr2.add(&bqbr3).add(&bqbr4);
Renate 28:7c7508bdb21f 441 // Chain voor linker biceps
Renate 28:7c7508bdb21f 442 bqcbl.add(&bqbl1).add(&bqbl2);
Renate 28:7c7508bdb21f 443 bqcbl2.add(&bqbl3).add(&bqbl4);
Renate 28:7c7508bdb21f 444 // Chain voor kuit
Renate 28:7c7508bdb21f 445 bqck.add(&bqk1).add(&bqk2);
Renate 28:7c7508bdb21f 446 bqck2.add(&bqk3).add(&bqk4);
Renate 28:7c7508bdb21f 447
Renate 28:7c7508bdb21f 448 loop_ticker.attach(&ProcessStateMachine, 0.002f);
Renate 28:7c7508bdb21f 449
Renate 28:7c7508bdb21f 450 while(true) {
Renate 28:7c7508bdb21f 451 // wait(0.2);
Renate 28:7c7508bdb21f 452 /* do nothing */
Renate 28:7c7508bdb21f 453 }
Renate 28:7c7508bdb21f 454 }