Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Script 15-10-2019

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@36:3c8b13f43b78, 2019-11-05 (annotated)

Committer:
Renate
Date:
Tue Nov 05 08:14:08 2019 +0000
Revision:
36:3c8b13f43b78
Parent:
34:1244984873ba 
Script voor netjes maken op 5-11

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
Rosalie 3:6ee0b20c23b0 2 #include "HIDScope.h"
Rosalie 3:6ee0b20c23b0 3 #include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
Rosalie 3:6ee0b20c23b0 5 #include "BiQuad.h"
Renate 34:1244984873ba 6 //#include "FastPWM.h"
Renate 32:d651c23bbb77 7 #define M_PI 3.14159265358979323846 /* pi */
WiesjeRoskamp 2:aee655d11b6d 8 #include <math.h>
Renate 34:1244984873ba 9 //#include "Servo.h"
Renate 21:456acc79726c 10 #include <cmath>
Renate 34:1244984873ba 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 34:1244984873ba 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 34:1244984873ba 34 QEI Encoder1(D13, D12, NC, 64);
Renate 34:1244984873ba 35 QEI Encoder2(D10, D9, NC, 64);
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 34:1244984873ba 45 double pulses_M1;
Renate 34:1244984873ba 46 double pulses_M2;
Renate 34:1244984873ba 47 double counts1;
Renate 34:1244984873ba 48 double 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 34:1244984873ba 125 double Inverse_jacobian[2][2];
Renate 34:1244984873ba 126 double desired_velocity[2][1];
Renate 34:1244984873ba 127
Renate 29:8e0a7c33e4e7 128 const double delta_t = 0.002;
Renate 29:8e0a7c33e4e7 129
Renate 34:1244984873ba 130 double Joint_1_position = 0.0;
Renate 34:1244984873ba 131 double Joint_2_position = 0.0;
Renate 30:0a328a9a4788 132
Renate 34:1244984873ba 133 double Joint_1_position_prev = 0.0;
Renate 34:1244984873ba 134 double Joint_2_position_prev = 0.0;
Renate 29:8e0a7c33e4e7 135
Renate 34:1244984873ba 136 double Joint_velocity[2][1] = {{0.0}, {0.0}};
Renate 29:8e0a7c33e4e7 137
Renate 34:1244984873ba 138 double q1_dot;
Renate 34:1244984873ba 139 double q2_dot;
Renate 29:8e0a7c33e4e7 140
Renate 29:8e0a7c33e4e7 141 double q1;
Renate 29:8e0a7c33e4e7 142 double q2;
Renate 29:8e0a7c33e4e7 143
Renate 34:1244984873ba 144 double Motor_1_position = 0.0;
Renate 34:1244984873ba 145 double Motor_2_position = 0.0;
Renate 30:0a328a9a4788 146
Renate 30:0a328a9a4788 147 // VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
Renate 29:8e0a7c33e4e7 148
Renate 33:f81f68a912b3 149 const double Kp = 12.5;
Renate 32:d651c23bbb77 150 const double Ki = 0.03;
Renate 30:0a328a9a4788 151
Renate 34:1244984873ba 152 double theta_k_1 = 0.0;
Renate 34:1244984873ba 153 double theta_k_2 = 0.0;
Renate 29:8e0a7c33e4e7 154
Renate 34:1244984873ba 155 double error_integral_1 = 0.0;
Renate 34:1244984873ba 156 double error_integral_2 = 0.0;
Renate 31:967b455bc328 157
Renate 34:1244984873ba 158 double u_i_1;
Renate 34:1244984873ba 159 double u_i_2;
Renate 32:d651c23bbb77 160
Renate 34:1244984873ba 161 double theta_t_1;
Renate 34:1244984873ba 162 double theta_t_2;
Renate 30:0a328a9a4788 163
Renate 34:1244984873ba 164 double error_M1;
Renate 34:1244984873ba 165 double error_M2;
Renate 31:967b455bc328 166
Renate 34:1244984873ba 167 double abs_theta_t_1;
Renate 34:1244984873ba 168 double abs_theta_t_2;
Renate 29:8e0a7c33e4e7 169
Renate 23:4572750a5c59 170 // VOIDS
Renate 23:4572750a5c59 171
Renate 23:4572750a5c59 172 // Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden).
Renate 28:7c7508bdb21f 173 // Enige optie is resetten, dan wordt het script opnieuw opgestart.
Renate 8:c7d3b67346db 174 void emergency()
Renate 28:7c7508bdb21f 175 {
Renate 28:7c7508bdb21f 176 loop_ticker.detach();
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 11:4bc0304978e2 181
Renate 28:7c7508bdb21f 182 // Motoren uitzetten
Renate 8:c7d3b67346db 183 void motors_off()
Renate 28:7c7508bdb21f 184 {
Renate 28:7c7508bdb21f 185 motor1.write(0);
Renate 28:7c7508bdb21f 186 motor2.write(0);
Renate 28:7c7508bdb21f 187 pc.printf("Motoren uit functie\r\n");
Renate 28:7c7508bdb21f 188 }
Renate 28:7c7508bdb21f 189
Renate 14:54343b9fd708 190 // Motoren aanzetten
Renate 15:ad065ab92d11 191 void motors_on()
Renate 28:7c7508bdb21f 192 {
Renate 32:d651c23bbb77 193 motor1.write(0.3);
Renate 31:967b455bc328 194 motor1_dir.write(0);
Renate 31:967b455bc328 195 motor2.write(0.3);
Renate 28:7c7508bdb21f 196 motor2_dir.write(1);
Renate 31:967b455bc328 197 // Door motor 1 een richting van 1 te geven en motor 2 een van 0, draaien
Renate 31:967b455bc328 198 // beide motoren rechtsom
Renate 28:7c7508bdb21f 199 pc.printf("Motoren aan functie\r\n");
Renate 28:7c7508bdb21f 200 }
Rosalie 3:6ee0b20c23b0 201
Renate 29:8e0a7c33e4e7 202 void Inverse_Kinematics()
Renate 29:8e0a7c33e4e7 203 {
Renate 30:0a328a9a4788 204 // Defining joint velocities based on calculations of matlab
Renate 34:1244984873ba 205 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 34:1244984873ba 206 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 34:1244984873ba 207 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 34:1244984873ba 208 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 209
Renate 34:1244984873ba 210 desired_velocity[0][0] = vx;
Renate 34:1244984873ba 211 desired_velocity[1][0] = vy;
Renate 30:0a328a9a4788 212
Renate 34:1244984873ba 213 Joint_velocity[0][0] = Inverse_jacobian[0][0]*desired_velocity[0][0] + Inverse_jacobian[0][1]*desired_velocity[1][0];
Renate 34:1244984873ba 214 Joint_velocity[1][0] = Inverse_jacobian[1][0]*desired_velocity[0][0] + Inverse_jacobian[1][1]*desired_velocity[1][0];
Renate 30:0a328a9a4788 215
Renate 34:1244984873ba 216 /*// Integratie
Renate 34:1244984873ba 217 Joint_1_position = Joint_1_position_prev + Joint_velocity[0][0]*delta_t;
Renate 34:1244984873ba 218 Joint_2_position = Joint_2_position_prev + Joint_velocity[1][0]*delta_t;
Renate 30:0a328a9a4788 219
Renate 34:1244984873ba 220 Joint_1_position_prev = Joint_1_position;
Renate 34:1244984873ba 221 Joint_2_position_prev = Joint_2_position;
Renate 29:8e0a7c33e4e7 222
Renate 34:1244984873ba 223 Motor_1_position = Joint_1_position;
Renate 34:1244984873ba 224 Motor_2_position = Joint_1_position + Joint_2_position;*/
Renate 30:0a328a9a4788 225 }
Renate 29:8e0a7c33e4e7 226
Renate 34:1244984873ba 227 // PI-CONTROLLER
Renate 34:1244984873ba 228 void PI_controller()
Renate 31:967b455bc328 229 {
Renate 31:967b455bc328 230 // Proportional part:
Renate 34:1244984873ba 231 theta_k_1= Kp * error_M1;
Renate 34:1244984873ba 232 theta_k_2= Kp * error_M2;
Renate 31:967b455bc328 233
Renate 31:967b455bc328 234 // Integral part
Renate 34:1244984873ba 235 error_integral_1 = error_integral_1+ error_M1*delta_t;
Renate 34:1244984873ba 236 error_integral_2 = error_integral_2+ error_M2*delta_t;
Renate 34:1244984873ba 237 u_i_1= Ki * error_integral_1;
Renate 34:1244984873ba 238 u_i_2= Ki * error_integral_2;
Renate 31:967b455bc328 239
Renate 31:967b455bc328 240 // sum all parts and return it
Renate 34:1244984873ba 241 theta_t_1= theta_k_1 + u_i_1;
Renate 34:1244984873ba 242 theta_t_2= theta_k_2 + u_i_2;
Renate 31:967b455bc328 243 }
Renate 31:967b455bc328 244
Renate 34:1244984873ba 245 void Define_motor_dir()
Renate 30:0a328a9a4788 246 {
Renate 34:1244984873ba 247 if (theta_t_1 >= 0 && theta_t_2 >= 0) {
Renate 32:d651c23bbb77 248 motor1_dir.write(0);
Renate 32:d651c23bbb77 249 motor2_dir.write(1);
Renate 32:d651c23bbb77 250 }
Renate 34:1244984873ba 251 if (theta_t_1 < 0 && theta_t_2 >= 0) {
Renate 32:d651c23bbb77 252 motor1_dir.write(1);
Renate 32:d651c23bbb77 253 motor1_dir.write(1);
Renate 32:d651c23bbb77 254 }
Renate 34:1244984873ba 255 if (theta_t_1 >= 0 && theta_t_2 < 0) {
Renate 32:d651c23bbb77 256 motor1_dir.write(0);
Renate 32:d651c23bbb77 257 motor2_dir.write(0);
Renate 32:d651c23bbb77 258 } else {
Renate 32:d651c23bbb77 259 motor1_dir.write(1);
Renate 32:d651c23bbb77 260 motor2_dir.write(0);
Renate 32:d651c23bbb77 261 }
Renate 32:d651c23bbb77 262 }
Renate 32:d651c23bbb77 263
Renate 34:1244984873ba 264 volatile bool loop_done = true;
Renate 34:1244984873ba 265
Renate 6:64146e16e10c 266 // Finite state machine programming (calibration servo motor?)
Renate 28:7c7508bdb21f 267 void ProcessStateMachine(void)
Renate 28:7c7508bdb21f 268 {
Renate 34:1244984873ba 269 if (!loop_done) {
Renate 34:1244984873ba 270 pc.printf("Timing is f'ed up\r\n");
Renate 34:1244984873ba 271
Renate 34:1244984873ba 272 return;
Renate 34:1244984873ba 273 }
Renate 34:1244984873ba 274
Renate 34:1244984873ba 275 loop_done = false;
Renate 34:1244984873ba 276
Renate 23:4572750a5c59 277 // Berekenen van de motorhoeken (in radialen)
Renate 32:d651c23bbb77 278 pulses_M1 = Encoder1.getPulses();
Renate 32:d651c23bbb77 279 pulses_M2 = Encoder2.getPulses();
Renate 32:d651c23bbb77 280 counts1 = pulses_M1*2;
Renate 32:d651c23bbb77 281 counts2 = pulses_M2*2;
Renate 34:1244984873ba 282 static int offset1 = 0;
Renate 34:1244984873ba 283 static int offset2 = 0;
Renate 34:1244984873ba 284 theta_h_1_rad = conversion_factor*(counts1-offset1);
Renate 34:1244984873ba 285 theta_h_2_rad = conversion_factor*(counts2-offset2);
Renate 34:1244984873ba 286
Renate 34:1244984873ba 287 static bool returned;
Renate 34:1244984873ba 288
Renate 34:1244984873ba 289
Renate 34:1244984873ba 290
Renate 34:1244984873ba 291 scope.set(0, EMG_biceps_right_raw);
Renate 34:1244984873ba 292 scope.set(1, pulses_M2);
Renate 36:3c8b13f43b78 293 scope.set(2, pulses_M1);
Renate 34:1244984873ba 294 scope.send();
Renate 34:1244984873ba 295 returned = false;
Renate 28:7c7508bdb21f 296
Renate 29:8e0a7c33e4e7 297 // Calculating joint angles based on motor angles (current encoder values)
Renate 29:8e0a7c33e4e7 298 q1 = theta_h_1_rad; // Relative angle joint 1 (rad)
Renate 29:8e0a7c33e4e7 299 q2 = theta_h_2_rad - theta_h_1_rad; // Relative angle joint 2 (rad)
Renate 29:8e0a7c33e4e7 300
Renate 23:4572750a5c59 301 // Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal
Renate 28:7c7508bdb21f 302 // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled'
Renate 23:4572750a5c59 303 filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
Renate 23:4572750a5c59 304 filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
Renate 23:4572750a5c59 305 filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
Renate 28:7c7508bdb21f 306
Renate 23:4572750a5c59 307 // Vervolgens wordt de absolute waarde hiervan genomen
Renate 23:4572750a5c59 308 filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
Renate 23:4572750a5c59 309 filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
Renate 23:4572750a5c59 310 filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
Renate 28:7c7508bdb21f 311
Renate 23:4572750a5c59 312 // Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter)
Renate 23:4572750a5c59 313 // over het signaal gedaan
Renate 23:4572750a5c59 314 filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
Renate 23:4572750a5c59 315 filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
Renate 23:4572750a5c59 316 filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
Renate 28:7c7508bdb21f 317
Renate 28:7c7508bdb21f 318 // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope
Renate 33:f81f68a912b3 319 //scope.set(0, normalized_EMG_biceps_right);
Renate 33:f81f68a912b3 320 //scope.set(1, normalized_EMG_biceps_left);
Renate 33:f81f68a912b3 321 //scope.set(2, normalized_EMG_calf);
Renate 33:f81f68a912b3 322 //scope.send();
Renate 28:7c7508bdb21f 323
Renate 28:7c7508bdb21f 324 // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die
Renate 28:7c7508bdb21f 325 // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende
Renate 23:4572750a5c59 326 // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
Renate 28:7c7508bdb21f 327 // waarna het gemiddelde wordt bepaald.
Renate 28:7c7508bdb21f 328 if (calib) {
Renate 28:7c7508bdb21f 329 if (i_calib == 0) {
Renate 28:7c7508bdb21f 330 filtered_EMG_biceps_right_total=0;
Renate 28:7c7508bdb21f 331 filtered_EMG_biceps_left_total=0;
Renate 28:7c7508bdb21f 332 filtered_EMG_calf_total=0;
Renate 28:7c7508bdb21f 333 }
Renate 28:7c7508bdb21f 334 if (i_calib <= 2500) {
Renate 28:7c7508bdb21f 335 filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
Renate 28:7c7508bdb21f 336 filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
Renate 28:7c7508bdb21f 337 filtered_EMG_calf_total+=filtered_EMG_calf;
Renate 34:1244984873ba 338 i_calib+=100;
Renate 28:7c7508bdb21f 339 }
Renate 28:7c7508bdb21f 340 if (i_calib > 2500) {
Renate 28:7c7508bdb21f 341 mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
Renate 28:7c7508bdb21f 342 mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
Renate 28:7c7508bdb21f 343 mean_EMG_calf=filtered_EMG_calf_total/2500.0;
Renate 28:7c7508bdb21f 344 pc.printf("Ontspan spieren\r\n");
Renate 28:7c7508bdb21f 345 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 346 calib = false;
Renate 28:7c7508bdb21f 347 }
Renate 28:7c7508bdb21f 348 }
Renate 28:7c7508bdb21f 349
Renate 23:4572750a5c59 350 // Genormaliseerde EMG's berekenen
Renate 23:4572750a5c59 351 normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
Renate 23:4572750a5c59 352 normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
Renate 23:4572750a5c59 353 normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
Renate 28:7c7508bdb21f 354
Renate 28:7c7508bdb21f 355 // Finite state machine
Renate 28:7c7508bdb21f 356 switch (currentState) {
Renate 6:64146e16e10c 357 case Motors_off:
Renate 28:7c7508bdb21f 358
Renate 28:7c7508bdb21f 359 if (stateChanged) {
Renate 8:c7d3b67346db 360 motors_off(); // functie waarbij motoren uitgaan
Renate 11:4bc0304978e2 361 stateChanged = false;
Renate 9:4de589636f50 362 pc.printf("Motors off state\r\n");
Renate 28:7c7508bdb21f 363 }
Renate 29:8e0a7c33e4e7 364 if (Emergency_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 29:8e0a7c33e4e7 365 emergency();
Renate 29:8e0a7c33e4e7 366 }
Renate 28:7c7508bdb21f 367 if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 9:4de589636f50 368 currentState = Calib_motor;
Renate 11:4bc0304978e2 369 stateChanged = true;
Renate 11:4bc0304978e2 370 pc.printf("Moving to Calib_motor state\r\n");
Renate 6:64146e16e10c 371 }
Renate 6:64146e16e10c 372 break;
Renate 28:7c7508bdb21f 373
Renate 9:4de589636f50 374 case Calib_motor:
Renate 28:7c7508bdb21f 375
Renate 29:8e0a7c33e4e7 376 if (stateChanged) {
Renate 29:8e0a7c33e4e7 377 pc.printf("Zet motoren handmatig in home positie\r\n");
Renate 29:8e0a7c33e4e7 378 stateChanged = false;
Renate 29:8e0a7c33e4e7 379 }
Renate 29:8e0a7c33e4e7 380
Renate 29:8e0a7c33e4e7 381 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 382 emergency();
Renate 29:8e0a7c33e4e7 383 }
Renate 29:8e0a7c33e4e7 384 if (Motor_calib_button_pressed.read() == false) {
Renate 34:1244984873ba 385 offset1 = counts1;
Renate 34:1244984873ba 386 offset2 = counts2;
Renate 21:456acc79726c 387 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 388 currentState = Calib_EMG;
Renate 11:4bc0304978e2 389 stateChanged = true;
Renate 9:4de589636f50 390 pc.printf("Moving to Calib_EMG state\r\n");
Renate 28:7c7508bdb21f 391 }
Renate 11:4bc0304978e2 392 break;
Renate 28:7c7508bdb21f 393
Renate 28:7c7508bdb21f 394 case Calib_EMG:
Renate 28:7c7508bdb21f 395
Renate 28:7c7508bdb21f 396 if (stateChanged) {
Renate 28:7c7508bdb21f 397 i_calib = 0;
Renate 28:7c7508bdb21f 398 calib = true;
Renate 28:7c7508bdb21f 399 pc.printf("Span spieren aan\r\n");
Renate 28:7c7508bdb21f 400 stateChanged = false;
Renate 28:7c7508bdb21f 401 }
Renate 28:7c7508bdb21f 402
Renate 29:8e0a7c33e4e7 403 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 404 emergency();
Renate 29:8e0a7c33e4e7 405 }
Renate 29:8e0a7c33e4e7 406
Renate 28:7c7508bdb21f 407 if (i_calib > 2500) {
Renate 28:7c7508bdb21f 408 calib = false;
Renate 28:7c7508bdb21f 409 currentState = Homing;
Renate 28:7c7508bdb21f 410 stateChanged = true;
Renate 28:7c7508bdb21f 411 pc.printf("Moving to Homing state\r\n");
Renate 28:7c7508bdb21f 412 }
Renate 28:7c7508bdb21f 413 break;
Renate 28:7c7508bdb21f 414
Renate 34:1244984873ba 415 case Homing:
Renate 28:7c7508bdb21f 416
Renate 28:7c7508bdb21f 417 if (stateChanged) {
Renate 28:7c7508bdb21f 418 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
Renate 11:4bc0304978e2 419 // (inclusief de end-effector) in de juiste home positie wordt gezet
Renate 28:7c7508bdb21f 420 stateChanged = false;
Renate 11:4bc0304978e2 421 }
Renate 28:7c7508bdb21f 422 if (Emergency_button_pressed.read() == false) {
Renate 10:83f3cec8dd1c 423 emergency();
Renate 28:7c7508bdb21f 424 }
Renate 29:8e0a7c33e4e7 425
Renate 34:1244984873ba 426 if (Homing_button_pressed.read() == false) {
Renate 28:7c7508bdb21f 427 currentState = Operation_mode;
Renate 28:7c7508bdb21f 428 stateChanged = true;
Renate 29:8e0a7c33e4e7 429 pc.printf("Moving to operation mode \r\n");
Renate 28:7c7508bdb21f 430 }
Renate 29:8e0a7c33e4e7 431
Renate 28:7c7508bdb21f 432 break;
Renate 28:7c7508bdb21f 433
Renate 28:7c7508bdb21f 434 case Operation_mode: // Overgaan tot emergency wanneer referentie niet
Renate 28:7c7508bdb21f 435 // overeenkomt met werkelijkheid
Renate 28:7c7508bdb21f 436
Renate 23:4572750a5c59 437 // 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 438 if (stateChanged) {
Renate 29:8e0a7c33e4e7 439 motors_off();
Renate 34:1244984873ba 440 Joint_1_position = 0;
Renate 34:1244984873ba 441 Joint_2_position = 0;
Renate 34:1244984873ba 442 Joint_1_position_prev = Joint_1_position;
Renate 34:1244984873ba 443 Joint_2_position_prev = Joint_2_position;
Renate 34:1244984873ba 444 Joint_velocity[0][0] = 0;
Renate 34:1244984873ba 445 Joint_velocity[1][0] = 0;
Renate 34:1244984873ba 446 Motor_1_position = 0;
Renate 34:1244984873ba 447 Motor_2_position = 0;
Renate 34:1244984873ba 448 theta_k_1 = 0.0;
Renate 34:1244984873ba 449 theta_k_2 = 0.0;
Renate 34:1244984873ba 450 error_integral_1 = 0.0;
Renate 34:1244984873ba 451 error_integral_2 = 0.0;
Renate 29:8e0a7c33e4e7 452 stateChanged = false;
Renate 34:1244984873ba 453 pc.printf("einde operation mode init");
Renate 29:8e0a7c33e4e7 454 }
Renate 29:8e0a7c33e4e7 455 // Hier moet een functie worden aangeroepen die ervoor zorgt dat
Renate 29:8e0a7c33e4e7 456 // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
Renate 29:8e0a7c33e4e7 457 // zodat de robotarm kan bewegen
Renate 28:7c7508bdb21f 458
Renate 29:8e0a7c33e4e7 459 // if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 29:8e0a7c33e4e7 460 // motors_off();
Renate 29:8e0a7c33e4e7 461 // currentState = Motors_off;
Renate 29:8e0a7c33e4e7 462 // stateChanged = true;
Renate 29:8e0a7c33e4e7 463 // pc.printf("Terug naar de state Motors_off\r\n");
Renate 29:8e0a7c33e4e7 464 // }
Renate 29:8e0a7c33e4e7 465 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 466 emergency();
Renate 29:8e0a7c33e4e7 467 }
Renate 29:8e0a7c33e4e7 468 if (Motor_calib_button_pressed.read() == false) { // Is nu voor de homing
Renate 29:8e0a7c33e4e7 469 motors_off();
Renate 29:8e0a7c33e4e7 470 currentState = Homing;
Renate 29:8e0a7c33e4e7 471 stateChanged = true;
Renate 29:8e0a7c33e4e7 472 pc.printf("Terug naar de state Homing\r\n");
Renate 29:8e0a7c33e4e7 473 }
Renate 29:8e0a7c33e4e7 474 if (normalized_EMG_biceps_right >= 0.3) {
Renate 31:967b455bc328 475
Renate 32:d651c23bbb77 476 if (normalized_EMG_calf < 0.3) {
Renate 31:967b455bc328 477 vx = 0.0;
Renate 36:3c8b13f43b78 478 vy = 0.02;
Renate 31:967b455bc328 479 }
Renate 32:d651c23bbb77 480 if (normalized_EMG_calf >= 0.3) {
Renate 31:967b455bc328 481 vx = 0.0;
Renate 36:3c8b13f43b78 482 vy = -0.02;
Renate 31:967b455bc328 483 }
Renate 34:1244984873ba 484 //pc.printf("q1=%f, q2=%f\r\n", q1, q2);
Renate 30:0a328a9a4788 485 Inverse_Kinematics();
Renate 34:1244984873ba 486 int testval = Encoder1.getPulses();
Renate 34:1244984873ba 487 int testval2 = Encoder2.getPulses();
Renate 34:1244984873ba 488 pc.printf("testval %i testval2 %i \r\n", testval,testval2);
Renate 34:1244984873ba 489
Renate 34:1244984873ba 490 //pc.printf("done");
Renate 34:1244984873ba 491 }/*
Renate 34:1244984873ba 492 error_M1 = Motor_1_position - theta_h_1_rad;
Renate 34:1244984873ba 493 error_M2 = Motor_2_position - theta_h_2_rad;
Renate 30:0a328a9a4788 494
Renate 34:1244984873ba 495 PI_controller();
Renate 34:1244984873ba 496
Renate 34:1244984873ba 497 abs_theta_t_1 = abs(theta_t_1);
Renate 34:1244984873ba 498 abs_theta_t_2 = abs(theta_t_2);
Renate 34:1244984873ba 499
Renate 34:1244984873ba 500 motor1.write(abs_theta_t_1);
Renate 34:1244984873ba 501 motor2.write(abs_theta_t_2);
Renate 34:1244984873ba 502 Define_motor_dir();
Renate 28:7c7508bdb21f 503
Renate 29:8e0a7c33e4e7 504 } else if (normalized_EMG_biceps_left >= 0.3) {
Renate 32:d651c23bbb77 505 if (normalized_EMG_calf < 0.3) {
Renate 32:d651c23bbb77 506 vx = 0.05;
Renate 32:d651c23bbb77 507 vy = 0.0;
Renate 32:d651c23bbb77 508 }
Renate 32:d651c23bbb77 509 if (normalized_EMG_calf >= 0.3) {
Renate 32:d651c23bbb77 510 vx = -0.05;
Renate 32:d651c23bbb77 511 vy = 0.0;
Renate 32:d651c23bbb77 512 }
Renate 31:967b455bc328 513 Inverse_Kinematics();
Renate 34:1244984873ba 514
Renate 34:1244984873ba 515 error_M1 = Motor_1_position - theta_h_1_rad;
Renate 34:1244984873ba 516 error_M2 = Motor_2_position - theta_h_2_rad;
Renate 34:1244984873ba 517
Renate 34:1244984873ba 518 PI_controller();
Renate 31:967b455bc328 519
Renate 34:1244984873ba 520 abs_theta_t_1 = abs(theta_t_1);
Renate 34:1244984873ba 521 abs_theta_t_2 = abs(theta_t_2);
Renate 31:967b455bc328 522
Renate 34:1244984873ba 523 motor1.write(abs_theta_t_1);
Renate 34:1244984873ba 524 motor2.write(abs_theta_t_2);
Renate 34:1244984873ba 525 Define_motor_dir();
Renate 32:d651c23bbb77 526
Renate 29:8e0a7c33e4e7 527 } else {
Renate 33:f81f68a912b3 528 vx = 0.0;
Renate 33:f81f68a912b3 529 vy = 0.0;
Renate 34:1244984873ba 530
Renate 33:f81f68a912b3 531 Inverse_Kinematics();
Renate 34:1244984873ba 532
Renate 34:1244984873ba 533 error_M1 = Motor_1_position - theta_h_1_rad;
Renate 34:1244984873ba 534 error_M2 = Motor_2_position - theta_h_2_rad;
Renate 34:1244984873ba 535
Renate 34:1244984873ba 536 PI_controller();
Renate 33:f81f68a912b3 537
Renate 34:1244984873ba 538 abs_theta_t_1 = abs(theta_t_1);
Renate 34:1244984873ba 539 abs_theta_t_2 = abs(theta_t_2);
Renate 33:f81f68a912b3 540
Renate 34:1244984873ba 541 motor1.write(abs_theta_t_1);
Renate 34:1244984873ba 542 motor2.write(abs_theta_t_2);
Renate 34:1244984873ba 543 Define_motor_dir();
Renate 28:7c7508bdb21f 544 }
Renate 34:1244984873ba 545 */
Renate 21:456acc79726c 546 break;
Renate 28:7c7508bdb21f 547
Renate 7:1d57463393c6 548 default:
Renate 7:1d57463393c6 549 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 14:54343b9fd708 550 motors_off();
Renate 9:4de589636f50 551 pc.printf("Unknown or uninplemented state reached!\r\n");
Renate 28:7c7508bdb21f 552
Renate 34:1244984873ba 553 }
Renate 34:1244984873ba 554 returned = true;
Renate 34:1244984873ba 555
Renate 34:1244984873ba 556 loop_done = true;
Renate 33:f81f68a912b3 557 }
Renate 33:f81f68a912b3 558
Renate 34:1244984873ba 559
Renate 34:1244984873ba 560 //void Print()
Renate 34:1244984873ba 561 //{
Renate 34:1244984873ba 562 //pc.printf("error_M2 = %f, Motor_2_position= %f, theta_h_2_rad = %f, pulses_M2 = %i\r\n", error_M2, Motor_2_position,theta_h_2_rad, pulses_M2);
Renate 34:1244984873ba 563
Renate 34:1244984873ba 564 //}
WiesjeRoskamp 2:aee655d11b6d 565
Renate 8:c7d3b67346db 566 int main(void)
Renate 28:7c7508bdb21f 567 {
Renate 34:1244984873ba 568 pc.baud(115200);
Renate 34:1244984873ba 569
Renate 28:7c7508bdb21f 570 pc.printf("Opstarten\r\n");
Renate 23:4572750a5c59 571
Renate 33:f81f68a912b3 572 motor1.period_us(56);
Renate 33:f81f68a912b3 573 motor2.period_us(56);
Renate 33:f81f68a912b3 574
Renate 28:7c7508bdb21f 575 // Chain voor rechter biceps
Renate 28:7c7508bdb21f 576 bqcbr.add(&bqbr1).add(&bqbr2);
Renate 28:7c7508bdb21f 577 bqcbr2.add(&bqbr3).add(&bqbr4);
Renate 28:7c7508bdb21f 578 // Chain voor linker biceps
Renate 28:7c7508bdb21f 579 bqcbl.add(&bqbl1).add(&bqbl2);
Renate 28:7c7508bdb21f 580 bqcbl2.add(&bqbl3).add(&bqbl4);
Renate 28:7c7508bdb21f 581 // Chain voor kuit
Renate 28:7c7508bdb21f 582 bqck.add(&bqk1).add(&bqk2);
Renate 28:7c7508bdb21f 583 bqck2.add(&bqk3).add(&bqk4);
Renate 28:7c7508bdb21f 584
Renate 28:7c7508bdb21f 585 loop_ticker.attach(&ProcessStateMachine, 0.002f);
Renate 34:1244984873ba 586 //loop_prints.attach(&Print, 0.5f);
Renate 28:7c7508bdb21f 587
Renate 28:7c7508bdb21f 588 while(true) {
Renate 28:7c7508bdb21f 589 // wait(0.2);
Renate 28:7c7508bdb21f 590 /* do nothing */
Renate 28:7c7508bdb21f 591 }
Renate 28:7c7508bdb21f 592 }