Renate de Boer / Mbed 2 deprecated script_voor_project_copy

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@30:0a328a9a4788, 2019-10-30 (annotated)

Committer:
Renate
Date:
Wed Oct 30 08:28:37 2019 +0000
Revision:
30:0a328a9a4788
Parent:
29:8e0a7c33e4e7
Child:
31:967b455bc328
Versie aan einde dag 30-10

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 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 double error_q1_y;
Renate 30:0a328a9a4788 158 double error_q2_y;
Renate 30:0a328a9a4788 159
Renate 30:0a328a9a4788 160 // VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
Renate 30:0a328a9a4788 161 double error_y_integral_1 = 0.0;
Renate 30:0a328a9a4788 162 double error_y_integral_2 = 0.0;
Renate 29:8e0a7c33e4e7 163
Renate 30:0a328a9a4788 164 const double Kp = 17.5;
Renate 30:0a328a9a4788 165 const double Ki = 1.02;
Renate 30:0a328a9a4788 166
Renate 30:0a328a9a4788 167 double theta_k_1 = 0.0;
Renate 30:0a328a9a4788 168 double theta_k_2 = 0.0;
Renate 29:8e0a7c33e4e7 169
Renate 30:0a328a9a4788 170 double error_integral_1 = 0.0;
Renate 30:0a328a9a4788 171 double error_integral_2 = 0.0;
Renate 30:0a328a9a4788 172
Renate 30:0a328a9a4788 173 double u_i_1;
Renate 30:0a328a9a4788 174 double u_i_2;
Renate 30:0a328a9a4788 175
Renate 30:0a328a9a4788 176 double theta_t_1;
Renate 30:0a328a9a4788 177 double theta_t_2;
Renate 29:8e0a7c33e4e7 178
Renate 23:4572750a5c59 179 // VOIDS
Renate 23:4572750a5c59 180
Renate 23:4572750a5c59 181 // Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden).
Renate 28:7c7508bdb21f 182 // Enige optie is resetten, dan wordt het script opnieuw opgestart.
Renate 8:c7d3b67346db 183 void emergency()
Renate 28:7c7508bdb21f 184 {
Renate 28:7c7508bdb21f 185 loop_ticker.detach();
Renate 28:7c7508bdb21f 186 motor1.write(0);
Renate 28:7c7508bdb21f 187 motor2.write(0);
Renate 28:7c7508bdb21f 188 pc.printf("Ik ga exploderen!!!\r\n");
Renate 28:7c7508bdb21f 189 }
Renate 11:4bc0304978e2 190
Renate 28:7c7508bdb21f 191 // Motoren uitzetten
Renate 8:c7d3b67346db 192 void motors_off()
Renate 28:7c7508bdb21f 193 {
Renate 28:7c7508bdb21f 194 motor1.write(0);
Renate 29:8e0a7c33e4e7 195 motor1_dir.write(1);
Renate 28:7c7508bdb21f 196 motor2.write(0);
Renate 29:8e0a7c33e4e7 197 motor2_dir.write(1);
Renate 28:7c7508bdb21f 198 pc.printf("Motoren uit functie\r\n");
Renate 28:7c7508bdb21f 199 }
Renate 28:7c7508bdb21f 200
Renate 14:54343b9fd708 201 // Motoren aanzetten
Renate 15:ad065ab92d11 202 void motors_on()
Renate 28:7c7508bdb21f 203 {
Renate 28:7c7508bdb21f 204 motor1.write(0.9);
Renate 28:7c7508bdb21f 205 motor1_dir.write(1);
Renate 28:7c7508bdb21f 206 motor2.write(0.1);
Renate 28:7c7508bdb21f 207 motor2_dir.write(1);
Renate 28:7c7508bdb21f 208 pc.printf("Motoren aan functie\r\n");
Renate 28:7c7508bdb21f 209 }
Rosalie 3:6ee0b20c23b0 210
Renate 29:8e0a7c33e4e7 211 void Inverse_Kinematics()
Renate 29:8e0a7c33e4e7 212 {
Renate 30:0a328a9a4788 213 // Defining joint velocities based on calculations of matlab
Renate 29:8e0a7c33e4e7 214
Renate 29:8e0a7c33e4e7 215 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 216 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 217
Renate 29:8e0a7c33e4e7 218 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 219 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 220
Renate 30:0a328a9a4788 221 q1_dot_x = Joint_velocity_x[0][0];
Renate 30:0a328a9a4788 222 q2_dot_x = Joint_velocity_x[1][0];
Renate 29:8e0a7c33e4e7 223
Renate 30:0a328a9a4788 224 q1_dot_y = Joint_velocity_y[0][0];
Renate 30:0a328a9a4788 225 q2_dot_y = Joint_velocity_y[1][0];
Renate 29:8e0a7c33e4e7 226
Renate 29:8e0a7c33e4e7 227 // Integratie
Renate 30:0a328a9a4788 228 Joint_1_position_x = Joint_1_position_x_prev + Joint_velocity_x[0][0]*delta_t;
Renate 30:0a328a9a4788 229 Joint_2_position_x = Joint_2_position_x_prev + Joint_velocity_x[1][0]*delta_t;
Renate 30:0a328a9a4788 230
Renate 30:0a328a9a4788 231 Joint_1_position_y = Joint_1_position_y_prev + Joint_velocity_y[0][0]*delta_t;
Renate 30:0a328a9a4788 232 Joint_2_position_y = Joint_2_position_y_prev + Joint_velocity_y[1][0]*delta_t;
Renate 29:8e0a7c33e4e7 233
Renate 30:0a328a9a4788 234 Joint_1_position_x_prev = Joint_1_position_x;
Renate 30:0a328a9a4788 235 Joint_2_position_x_prev = Joint_2_position_x;
Renate 30:0a328a9a4788 236
Renate 30:0a328a9a4788 237 Joint_1_position_y_prev = Joint_1_position_y;
Renate 30:0a328a9a4788 238 Joint_2_position_y_prev = Joint_2_position_y;
Renate 30:0a328a9a4788 239
Renate 30:0a328a9a4788 240 Motor_1_position_x = Joint_1_position_x;
Renate 30:0a328a9a4788 241 Motor_2_position_x = Joint_1_position_x + Joint_2_position_x;
Renate 29:8e0a7c33e4e7 242
Renate 30:0a328a9a4788 243 Motor_1_position_y = Joint_1_position_y;
Renate 30:0a328a9a4788 244 Motor_2_position_y = Joint_1_position_y + Joint_2_position_y;
Renate 30:0a328a9a4788 245 }
Renate 29:8e0a7c33e4e7 246
Renate 30:0a328a9a4788 247 // PI-controller y-richting
Renate 30:0a328a9a4788 248 void PI_controller_y()
Renate 30:0a328a9a4788 249 {
Renate 30:0a328a9a4788 250 // Proportional part:
Renate 30:0a328a9a4788 251 theta_k_1= Kp * error_q1_y;
Renate 30:0a328a9a4788 252 theta_k_2= Kp * error_q2_y;
Renate 30:0a328a9a4788 253
Renate 30:0a328a9a4788 254 // Integral part
Renate 30:0a328a9a4788 255 error_integral_1 = error_integral_1 + error_q1_y *delta_t;
Renate 30:0a328a9a4788 256 error_integral_2 = error_integral_2 + error_q2_y *delta_t;
Renate 30:0a328a9a4788 257 u_i_1= Ki * error_integral_1;
Renate 30:0a328a9a4788 258 u_i_2= Ki * error_integral_2;
Renate 30:0a328a9a4788 259
Renate 30:0a328a9a4788 260 // sum all parts and return it
Renate 30:0a328a9a4788 261 theta_t_1 = theta_k_1 + u_i_1;
Renate 30:0a328a9a4788 262 theta_t_2 = theta_k_2 + u_i_2;
Renate 29:8e0a7c33e4e7 263 }
Renate 29:8e0a7c33e4e7 264
Renate 6:64146e16e10c 265 // Finite state machine programming (calibration servo motor?)
Renate 28:7c7508bdb21f 266 void ProcessStateMachine(void)
Renate 28:7c7508bdb21f 267 {
Renate 23:4572750a5c59 268 // Berekenen van de motorhoeken (in radialen)
Renate 28:7c7508bdb21f 269 counts1 = Encoder1.getPulses();
Renate 28:7c7508bdb21f 270 counts2 = Encoder2.getPulses();
Renate 23:4572750a5c59 271 theta_h_1_deg=(counts1/(double)CPR)*(double)full_degrees;
Renate 23:4572750a5c59 272 theta_h_2_deg=(counts2/(double)CPR)*(double)full_degrees;
Renate 23:4572750a5c59 273 theta_h_1_rad=(theta_h_1_deg/half_degrees)*M_PI;
Renate 23:4572750a5c59 274 theta_h_2_rad=(theta_h_2_deg/half_degrees)*M_PI;
Renate 28:7c7508bdb21f 275
Renate 29:8e0a7c33e4e7 276 // Calculating joint angles based on motor angles (current encoder values)
Renate 29:8e0a7c33e4e7 277 q1 = theta_h_1_rad; // Relative angle joint 1 (rad)
Renate 29:8e0a7c33e4e7 278 q2 = theta_h_2_rad - theta_h_1_rad; // Relative angle joint 2 (rad)
Renate 29:8e0a7c33e4e7 279
Renate 23:4572750a5c59 280 // Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal
Renate 28:7c7508bdb21f 281 // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled'
Renate 23:4572750a5c59 282 filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
Renate 23:4572750a5c59 283 filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
Renate 23:4572750a5c59 284 filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
Renate 28:7c7508bdb21f 285
Renate 23:4572750a5c59 286 // Vervolgens wordt de absolute waarde hiervan genomen
Renate 23:4572750a5c59 287 filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
Renate 23:4572750a5c59 288 filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
Renate 23:4572750a5c59 289 filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
Renate 28:7c7508bdb21f 290
Renate 23:4572750a5c59 291 // Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter)
Renate 23:4572750a5c59 292 // over het signaal gedaan
Renate 23:4572750a5c59 293 filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
Renate 23:4572750a5c59 294 filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
Renate 23:4572750a5c59 295 filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
Renate 28:7c7508bdb21f 296
Renate 28:7c7508bdb21f 297 // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope
Renate 28:7c7508bdb21f 298 scope.set(0, normalized_EMG_biceps_right);
Renate 28:7c7508bdb21f 299 scope.set(1, normalized_EMG_biceps_left);
Renate 28:7c7508bdb21f 300 scope.set(2, normalized_EMG_calf);
Renate 23:4572750a5c59 301 scope.send();
Renate 28:7c7508bdb21f 302
Renate 28:7c7508bdb21f 303 // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die
Renate 28:7c7508bdb21f 304 // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende
Renate 23:4572750a5c59 305 // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
Renate 28:7c7508bdb21f 306 // waarna het gemiddelde wordt bepaald.
Renate 28:7c7508bdb21f 307 if (calib) {
Renate 28:7c7508bdb21f 308 if (i_calib == 0) {
Renate 28:7c7508bdb21f 309 filtered_EMG_biceps_right_total=0;
Renate 28:7c7508bdb21f 310 filtered_EMG_biceps_left_total=0;
Renate 28:7c7508bdb21f 311 filtered_EMG_calf_total=0;
Renate 28:7c7508bdb21f 312 }
Renate 28:7c7508bdb21f 313 if (i_calib <= 2500) {
Renate 28:7c7508bdb21f 314 filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
Renate 28:7c7508bdb21f 315 filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
Renate 28:7c7508bdb21f 316 filtered_EMG_calf_total+=filtered_EMG_calf;
Renate 28:7c7508bdb21f 317 i_calib++;
Renate 28:7c7508bdb21f 318 }
Renate 28:7c7508bdb21f 319 if (i_calib > 2500) {
Renate 28:7c7508bdb21f 320 mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
Renate 28:7c7508bdb21f 321 mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
Renate 28:7c7508bdb21f 322 mean_EMG_calf=filtered_EMG_calf_total/2500.0;
Renate 28:7c7508bdb21f 323 pc.printf("Ontspan spieren\r\n");
Renate 28:7c7508bdb21f 324 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 325 calib = false;
Renate 28:7c7508bdb21f 326 }
Renate 28:7c7508bdb21f 327 }
Renate 28:7c7508bdb21f 328
Renate 23:4572750a5c59 329 // Genormaliseerde EMG's berekenen
Renate 23:4572750a5c59 330 normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
Renate 23:4572750a5c59 331 normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
Renate 23:4572750a5c59 332 normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
Renate 28:7c7508bdb21f 333
Renate 28:7c7508bdb21f 334 // Finite state machine
Renate 28:7c7508bdb21f 335 switch (currentState) {
Renate 6:64146e16e10c 336 case Motors_off:
Renate 28:7c7508bdb21f 337
Renate 28:7c7508bdb21f 338 if (stateChanged) {
Renate 8:c7d3b67346db 339 motors_off(); // functie waarbij motoren uitgaan
Renate 11:4bc0304978e2 340 stateChanged = false;
Renate 9:4de589636f50 341 pc.printf("Motors off state\r\n");
Renate 28:7c7508bdb21f 342 }
Renate 29:8e0a7c33e4e7 343 if (Emergency_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 29:8e0a7c33e4e7 344 emergency();
Renate 29:8e0a7c33e4e7 345 }
Renate 28:7c7508bdb21f 346 if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 9:4de589636f50 347 currentState = Calib_motor;
Renate 11:4bc0304978e2 348 stateChanged = true;
Renate 11:4bc0304978e2 349 pc.printf("Moving to Calib_motor state\r\n");
Renate 6:64146e16e10c 350 }
Renate 6:64146e16e10c 351 break;
Renate 28:7c7508bdb21f 352
Renate 9:4de589636f50 353 case Calib_motor:
Renate 28:7c7508bdb21f 354
Renate 29:8e0a7c33e4e7 355 if (stateChanged) {
Renate 29:8e0a7c33e4e7 356 pc.printf("Zet motoren handmatig in home positie\r\n");
Renate 29:8e0a7c33e4e7 357 stateChanged = false;
Renate 29:8e0a7c33e4e7 358 }
Renate 29:8e0a7c33e4e7 359
Renate 29:8e0a7c33e4e7 360 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 361 emergency();
Renate 29:8e0a7c33e4e7 362 }
Renate 29:8e0a7c33e4e7 363 if (Motor_calib_button_pressed.read() == false) {
Renate 21:456acc79726c 364 theta_h_1_rad = 0;
Renate 21:456acc79726c 365 theta_h_2_rad = 0;
Renate 21:456acc79726c 366 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 367 currentState = Calib_EMG;
Renate 11:4bc0304978e2 368 stateChanged = true;
Renate 9:4de589636f50 369 pc.printf("Moving to Calib_EMG state\r\n");
Renate 28:7c7508bdb21f 370 }
Renate 11:4bc0304978e2 371 break;
Renate 28:7c7508bdb21f 372
Renate 28:7c7508bdb21f 373 case Calib_EMG:
Renate 28:7c7508bdb21f 374
Renate 28:7c7508bdb21f 375 if (stateChanged) {
Renate 28:7c7508bdb21f 376 i_calib = 0;
Renate 28:7c7508bdb21f 377 calib = true;
Renate 28:7c7508bdb21f 378 pc.printf("Span spieren aan\r\n");
Renate 28:7c7508bdb21f 379 stateChanged = false;
Renate 28:7c7508bdb21f 380 }
Renate 28:7c7508bdb21f 381
Renate 29:8e0a7c33e4e7 382 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 383 emergency();
Renate 29:8e0a7c33e4e7 384 }
Renate 29:8e0a7c33e4e7 385
Renate 28:7c7508bdb21f 386 if (i_calib > 2500) {
Renate 28:7c7508bdb21f 387 calib = false;
Renate 28:7c7508bdb21f 388 currentState = Homing;
Renate 28:7c7508bdb21f 389 stateChanged = true;
Renate 28:7c7508bdb21f 390 pc.printf("Moving to Homing state\r\n");
Renate 28:7c7508bdb21f 391 }
Renate 28:7c7508bdb21f 392 break;
Renate 28:7c7508bdb21f 393
Renate 28:7c7508bdb21f 394 case Homing: // NOG NAAR KIJKEN
Renate 28:7c7508bdb21f 395
Renate 28:7c7508bdb21f 396 if (stateChanged) {
Renate 28:7c7508bdb21f 397 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
Renate 11:4bc0304978e2 398 // (inclusief de end-effector) in de juiste home positie wordt gezet
Renate 21:456acc79726c 399 motors_on();
Renate 28:7c7508bdb21f 400 stateChanged = false;
Renate 11:4bc0304978e2 401 }
Renate 28:7c7508bdb21f 402 if (Emergency_button_pressed.read() == false) {
Renate 10:83f3cec8dd1c 403 emergency();
Renate 28:7c7508bdb21f 404 }
Renate 29:8e0a7c33e4e7 405
Renate 29:8e0a7c33e4e7 406 // Hier moet iets van encoder counts waarde maal -1 worden gedaan om
Renate 29:8e0a7c33e4e7 407 // naar positie 0 te bewegen
Renate 29:8e0a7c33e4e7 408
Renate 29:8e0a7c33e4e7 409 if (Power_button_pressed.read() == false)
Renate 29:8e0a7c33e4e7 410 //if (theta_h_1_rad == 0.0 && theta_h_2_rad == 0.0)
Renate 29:8e0a7c33e4e7 411 // Veranderen wanneer encoders werken en motor in elkaar zit
Renate 28:7c7508bdb21f 412 {
Renate 28:7c7508bdb21f 413 currentState = Operation_mode;
Renate 28:7c7508bdb21f 414 stateChanged = true;
Renate 29:8e0a7c33e4e7 415 pc.printf("Moving to operation mode \r\n");
Renate 28:7c7508bdb21f 416 }
Renate 29:8e0a7c33e4e7 417
Renate 28:7c7508bdb21f 418 break;
Renate 28:7c7508bdb21f 419
Renate 28:7c7508bdb21f 420 case Operation_mode: // Overgaan tot emergency wanneer referentie niet
Renate 28:7c7508bdb21f 421 // overeenkomt met werkelijkheid
Renate 28:7c7508bdb21f 422
Renate 23:4572750a5c59 423 // 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 424 if (stateChanged) {
Renate 29:8e0a7c33e4e7 425 motors_off();
Renate 30:0a328a9a4788 426 Joint_1_position_x = 0;
Renate 30:0a328a9a4788 427 Joint_2_position_x = 0;
Renate 30:0a328a9a4788 428 Joint_1_position_y = 0;
Renate 30:0a328a9a4788 429 Joint_2_position_y = 0;
Renate 30:0a328a9a4788 430 Joint_1_position_x_prev = Joint_1_position_x;
Renate 30:0a328a9a4788 431 Joint_2_position_x_prev = Joint_2_position_x;
Renate 30:0a328a9a4788 432 Joint_1_position_y_prev = Joint_1_position_y;
Renate 30:0a328a9a4788 433 Joint_2_position_y_prev = Joint_2_position_y;
Renate 29:8e0a7c33e4e7 434 Joint_velocity_x[0][0] = 0;
Renate 29:8e0a7c33e4e7 435 Joint_velocity_x[1][0] = 0;
Renate 29:8e0a7c33e4e7 436 Joint_velocity_y[0][0] = 0;
Renate 29:8e0a7c33e4e7 437 Joint_velocity_y[1][0] = 0;
Renate 30:0a328a9a4788 438 Motor_1_position_x = 0;
Renate 30:0a328a9a4788 439 Motor_2_position_x = 0;
Renate 30:0a328a9a4788 440 Motor_1_position_y = 0;
Renate 30:0a328a9a4788 441 Motor_2_position_y = 0;
Renate 30:0a328a9a4788 442 error_y_integral_1 = 0;
Renate 30:0a328a9a4788 443 error_y_integral_2 = 0;
Renate 30:0a328a9a4788 444 theta_k_1 = 0.0;
Renate 30:0a328a9a4788 445 theta_k_2 = 0.0;
Renate 30:0a328a9a4788 446 error_integral_1 = 0.0;
Renate 30:0a328a9a4788 447 error_integral_2 = 0.0;
Renate 29:8e0a7c33e4e7 448 stateChanged = false;
Renate 29:8e0a7c33e4e7 449 }
Renate 29:8e0a7c33e4e7 450 // Hier moet een functie worden aangeroepen die ervoor zorgt dat
Renate 29:8e0a7c33e4e7 451 // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
Renate 29:8e0a7c33e4e7 452 // zodat de robotarm kan bewegen
Renate 28:7c7508bdb21f 453
Renate 29:8e0a7c33e4e7 454 // if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
Renate 29:8e0a7c33e4e7 455 // motors_off();
Renate 29:8e0a7c33e4e7 456 // currentState = Motors_off;
Renate 29:8e0a7c33e4e7 457 // stateChanged = true;
Renate 29:8e0a7c33e4e7 458 // pc.printf("Terug naar de state Motors_off\r\n");
Renate 29:8e0a7c33e4e7 459 // }
Renate 29:8e0a7c33e4e7 460 if (Emergency_button_pressed.read() == false) {
Renate 29:8e0a7c33e4e7 461 emergency();
Renate 29:8e0a7c33e4e7 462 }
Renate 29:8e0a7c33e4e7 463 if (Motor_calib_button_pressed.read() == false) { // Is nu voor de homing
Renate 29:8e0a7c33e4e7 464 motors_off();
Renate 29:8e0a7c33e4e7 465 currentState = Homing;
Renate 29:8e0a7c33e4e7 466 stateChanged = true;
Renate 29:8e0a7c33e4e7 467 pc.printf("Terug naar de state Homing\r\n");
Renate 29:8e0a7c33e4e7 468 }
Renate 29:8e0a7c33e4e7 469 if (normalized_EMG_biceps_right >= 0.3) {
Renate 30:0a328a9a4788 470 vx = 0.0;
Renate 30:0a328a9a4788 471 vy = 0.2;
Renate 30:0a328a9a4788 472 Inverse_Kinematics();
Renate 30:0a328a9a4788 473 error_q1_y = Motor_1_position_y - theta_h_1_rad;
Renate 30:0a328a9a4788 474 error_q2_y = Motor_2_position_y - theta_h_1_rad;
Renate 30:0a328a9a4788 475 PI_controller_y();
Renate 30:0a328a9a4788 476
Renate 30:0a328a9a4788 477 // return theta_t_1;
Renate 30:0a328a9a4788 478 // return theta_t_2;
Renate 30:0a328a9a4788 479
Renate 30:0a328a9a4788 480
Renate 29:8e0a7c33e4e7 481 motor1.write(0.3);
Renate 29:8e0a7c33e4e7 482 motor2.write(0.3);
Renate 29:8e0a7c33e4e7 483 motor1_dir.write(0);
Renate 29:8e0a7c33e4e7 484 motor2_dir.write(0);
Renate 29:8e0a7c33e4e7 485 //if (normalized_EMG_calf >= 0.3)
Renate 29:8e0a7c33e4e7 486 //{
Renate 29:8e0a7c33e4e7 487 //motor1.write(0.1);
Renate 29:8e0a7c33e4e7 488 //motor1_dir = !motor1_dir;
Renate 29:8e0a7c33e4e7 489 //}
Renate 28:7c7508bdb21f 490
Renate 29:8e0a7c33e4e7 491 } else if (normalized_EMG_biceps_left >= 0.3) {
Renate 29:8e0a7c33e4e7 492 motor2.write(0.9);
Renate 29:8e0a7c33e4e7 493 motor1.write(0.9);
Renate 29:8e0a7c33e4e7 494 motor1_dir.write(1);
Renate 29:8e0a7c33e4e7 495 motor2_dir.write(1);
Renate 28:7c7508bdb21f 496 //if (normalized_EMG_calf >= 0.3)
Renate 28:7c7508bdb21f 497 //{
Renate 28:7c7508bdb21f 498 // motor1_dir = !motor1_dir;
Renate 28:7c7508bdb21f 499 // pc.printf("Richting zou om moeten draaien");
Renate 28:7c7508bdb21f 500 // motor2_dir = !motor2_dir;
Renate 28:7c7508bdb21f 501 //}
Renate 29:8e0a7c33e4e7 502 } else {
Renate 29:8e0a7c33e4e7 503 motor1.write(0);
Renate 29:8e0a7c33e4e7 504 motor2.write(0);
Renate 28:7c7508bdb21f 505 }
Renate 28:7c7508bdb21f 506
Renate 21:456acc79726c 507 break;
Renate 28:7c7508bdb21f 508
Renate 7:1d57463393c6 509 default:
Renate 7:1d57463393c6 510 // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
Renate 14:54343b9fd708 511 motors_off();
Renate 9:4de589636f50 512 pc.printf("Unknown or uninplemented state reached!\r\n");
Renate 28:7c7508bdb21f 513
WiesjeRoskamp 2:aee655d11b6d 514 }
Renate 11:4bc0304978e2 515 }
WiesjeRoskamp 2:aee655d11b6d 516
Renate 8:c7d3b67346db 517 int main(void)
Renate 28:7c7508bdb21f 518 {
Renate 28:7c7508bdb21f 519 pc.printf("Opstarten\r\n");
Renate 23:4572750a5c59 520
Renate 28:7c7508bdb21f 521 // Chain voor rechter biceps
Renate 28:7c7508bdb21f 522 bqcbr.add(&bqbr1).add(&bqbr2);
Renate 28:7c7508bdb21f 523 bqcbr2.add(&bqbr3).add(&bqbr4);
Renate 28:7c7508bdb21f 524 // Chain voor linker biceps
Renate 28:7c7508bdb21f 525 bqcbl.add(&bqbl1).add(&bqbl2);
Renate 28:7c7508bdb21f 526 bqcbl2.add(&bqbl3).add(&bqbl4);
Renate 28:7c7508bdb21f 527 // Chain voor kuit
Renate 28:7c7508bdb21f 528 bqck.add(&bqk1).add(&bqk2);
Renate 28:7c7508bdb21f 529 bqck2.add(&bqk3).add(&bqk4);
Renate 28:7c7508bdb21f 530
Renate 28:7c7508bdb21f 531 loop_ticker.attach(&ProcessStateMachine, 0.002f);
Renate 28:7c7508bdb21f 532
Renate 28:7c7508bdb21f 533 while(true) {
Renate 28:7c7508bdb21f 534 // wait(0.2);
Renate 28:7c7508bdb21f 535 /* do nothing */
Renate 28:7c7508bdb21f 536 }
Renate 28:7c7508bdb21f 537 }