Renate de Boer
Script 15-10-2019
Dependencies: Servoaansturing mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 35:113a4015d408
- Parent:
- 32:d651c23bbb77
--- a/main.cpp Wed Oct 30 14:33:50 2019 +0000
+++ b/main.cpp Mon Nov 04 09:59:52 2019 +0000
@@ -12,626 +12,53 @@
Serial pc(USBTX, USBRX);
-// TICKERS
-Ticker loop_ticker;
-
-// BENODIGD VOOR PROCESS STATE MACHINE
-enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
-states currentState = Motors_off;
-bool stateChanged = true; // Make sure the initialization of first state is executed
-
-// INPUTS
-DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken!
-DigitalIn Emergency_button_pressed(D2);
-DigitalIn Motor_calib_button_pressed(SW2);
-
-AnalogIn EMG_biceps_right_raw (A0);
-AnalogIn EMG_biceps_left_raw (A1);
-AnalogIn EMG_calf_raw (A2);
-
-QEI Encoder1(D12, D13, NC, 8400, QEI::X2_ENCODING); //Checken of die D12, D9 etc wel kloppen, 8400= gear ratio x 64
-QEI Encoder2(D9, D10, NC, 8400, QEI::X2_ENCODING);
-
-// OUTPUTS
PwmOut motor1(D6); // Misschien moeten we hiervoor DigitalOut gebruiken, moet
PwmOut motor2(D5); // samen kunnen gaan met de servo motor
DigitalOut motor1_dir(D7);
DigitalOut motor2_dir(D4);
+QEI Encoder1(D13, D12, NC, 64);
+QEI Encoder2(D10, D9, NC, 64);
+
// VARIABELEN VOOR ENCODER, MOTORHOEK ETC.
-int pulses_M1;
-int pulses_M2;
-int counts1;
-int counts2;
+double pulses_M1;
+double pulses_M2;
+double counts1;
+double counts2;
const double conversion_factor = (2.0*M_PI)/(64.0*131.25*2.0);
double theta_h_1_rad;
double theta_h_2_rad;
-
-// DEFINITIES VOOR FILTERS
-
-// BICEPS-RECHTS
-// Definities voor eerste BiQuadChain (High-pass en Notch)
-BiQuadChain bqcbr;
-BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
-BiQuad bqbr2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
-// Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
-// Definieer (twee Low-pass -> vierde orde verkrijgen):
-BiQuadChain bqcbr2;
-BiQuad bqbr3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
-BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
-
-// BICEPS-LINKS
-// Definities voor eerste BiQuadChain (High-pass en Notch)
-BiQuadChain bqcbl;
-BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
-BiQuad bqbl2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
-// Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
-// Definieer (twee Low-pass -> vierde orde verkrijgen):
-BiQuadChain bqcbl2;
-BiQuad bqbl3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
-BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
-
-// KUIT
-// Definities voor eerste BiQuadChain (High-pass en Notch)
-BiQuadChain bqck;
-BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
-BiQuad bqk2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
-// Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
-// Definieer (twee Low-pass -> vierde orde verkrijgen):
-BiQuadChain bqck2;
-BiQuad bqk3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
-BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
-
-// VARIABELEN VOOR EMG + FILTEREN
-double filtered_EMG_biceps_right;
-double filtered_EMG_biceps_left;
-double filtered_EMG_calf;
-
-double filtered_EMG_biceps_right_1;
-double filtered_EMG_biceps_left_1;
-double filtered_EMG_calf_1;
-
-double filtered_EMG_biceps_right_abs;
-double filtered_EMG_biceps_left_abs;
-double filtered_EMG_calf_abs;
-
-double filtered_EMG_biceps_right_total;
-double filtered_EMG_biceps_left_total;
-double filtered_EMG_calf_total;
-
-// Variabelen voor HIDScope
-HIDScope scope(3);
-
-// VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP
-bool calib = false;
-static int i_calib = 0;
-
-double mean_EMG_biceps_right;
-double mean_EMG_biceps_left;
-double mean_EMG_calf;
-
-// VARIABELEN VOOR OPERATION MODE (EMG)
-double normalized_EMG_biceps_right;
-double normalized_EMG_biceps_left;
-double normalized_EMG_calf;
+double offset1 = 0.0;
+double offset2 = 0.0;
-// VARIABELEN VOOR OPERATION MODE (RKI)
-double vx; // Gelijk aan variabele die snelheid aangeeft in EMG (in x-richting)
-double vy; // Gelijk aan variabele die snelheid aangeeft in EMG (in y-richting)
-
-const double delta_t = 0.002;
-
-double Joint_1_position_x = 0.0;
-double Joint_2_position_x = 0.0;
-
-double Joint_1_position_y = 0.0;
-double Joint_2_position_y = 0.0;
-
-double Joint_1_position_x_prev = 0.0;
-double Joint_2_position_x_prev = 0.0;
-
-double Joint_1_position_y_prev;
-double Joint_2_position_y_prev;
-
-double Joint_velocity_x[2][1] = {{0.0}, {0.0}};
-double Joint_velocity_y[2][1] = {{0.0}, {0.0}};
-
-double q1_dot_x;
-double q2_dot_x;
-
-double q1_dot_y;
-double q2_dot_y;
-
-double q1;
-double q2;
-
-double Motor_1_position_x = 0.0;
-double Motor_2_position_x = 0.0;
-
-double Motor_1_position_y = 0.0;
-double Motor_2_position_y = 0.0;
+HIDScope scope(1);
-// VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
-
-const double Kp = 17.5;
-const double Ki = 0.03;
-
-// Voor x-richting
-double theta_k_1_x = 0.0;
-double theta_k_2_x = 0.0;
-
-double error_integral_1_x = 0.0;
-double error_integral_2_x = 0.0;
-
-double u_i_1_x;
-double u_i_2_x;
-
-double theta_t_1_x;
-double theta_t_2_x;
-
-double error_q1_x;
-double error_q2_x;
-
-double abs_theta_t_1_x;
-double abs_theta_t_2_x;
-
-// Voor y-richting
-double theta_k_1_y = 0.0;
-double theta_k_2_y = 0.0;
-
-double error_integral_1_y = 0.0;
-double error_integral_2_y = 0.0;
-
-double u_i_1_y;
-double u_i_2_y;
-
-double theta_t_1_y;
-double theta_t_2_y;
+Ticker loop;
-double error_q1_y;
-double error_q2_y;
-
-double abs_theta_t_1_y;
-double abs_theta_t_2_y;
-
-// VOIDS
-
-// Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden).
-// Enige optie is resetten, dan wordt het script opnieuw opgestart.
-void emergency()
-{
- loop_ticker.detach();
- motor1.write(0);
- motor2.write(0);
- pc.printf("Ik ga exploderen!!!\r\n");
-}
-
-// Motoren uitzetten
-void motors_off()
-{
- motor1.write(0);
- motor2.write(0);
- pc.printf("Motoren uit functie\r\n");
-}
-
-// Motoren aanzetten
-void motors_on()
-{
- motor1.write(0.3);
- motor1_dir.write(0);
- motor2.write(0.3);
- motor2_dir.write(1);
- // Door motor 1 een richting van 1 te geven en motor 2 een van 0, draaien
- // beide motoren rechtsom
- pc.printf("Motoren aan functie\r\n");
-}
-
-void Inverse_Kinematics()
-{
- // Defining joint velocities based on calculations of matlab
-
- Joint_velocity_x[0][0] = (vx*(cos(q1+3.141592653589793/6.0)*-8.5E2-sin(q1)*4.25E2+cos(q1)*cos(q2)*2.25E2+cos(q1)*sin(q2)*6.77E2+cos(q2)*sin(q1)*6.77E2-sin(q1)*sin(q2)*2.25E2+sqrt(3.0)*cos(q1)*4.25E2-sqrt(3.0)*cos(q1)*cos(q2)*4.25E2+sqrt(3.0)*sin(q1)*sin(q2)*4.25E2)*(4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
- Joint_velocity_x[1][0] = (vx*(sin(q1)*-4.25E2+cos(q1)*cos(q2)*2.25E2+cos(q1)*sin(q2)*6.77E2+cos(q2)*sin(q1)*6.77E2-sin(q1)*sin(q2)*2.25E2+sqrt(3.0)*cos(q1)*4.25E2-sqrt(3.0)*cos(q1)*cos(q2)*4.25E2+sqrt(3.0)*sin(q1)*sin(q2)*4.25E2)*(-4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
-
- Joint_velocity_y[0][0] = (vy*(cos(q1)*4.25E2-sin(q1+3.141592653589793/6.0)*8.5E2-cos(q1)*cos(q2)*6.77E2+cos(q1)*sin(q2)*2.25E2+cos(q2)*sin(q1)*2.25E2+sin(q1)*sin(q2)*6.77E2+sqrt(3.0)*sin(q1)*4.25E2-sqrt(3.0)*cos(q1)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*sin(q1)*4.25E2)*(4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
- Joint_velocity_y[1][0] = (vy*(cos(q1)*4.25E2-cos(q1)*cos(q2)*6.77E2+cos(q1)*sin(q2)*2.25E2+cos(q2)*sin(q1)*2.25E2+sin(q1)*sin(q2)*6.77E2+sqrt(3.0)*sin(q1)*4.25E2-sqrt(3.0)*cos(q1)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*sin(q1)*4.25E2)*(-4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
-
- // Integratie
- Joint_1_position_x = Joint_1_position_x_prev + Joint_velocity_x[0][0]*delta_t;
- Joint_2_position_x = Joint_2_position_x_prev + Joint_velocity_x[1][0]*delta_t;
-
- Joint_1_position_y = Joint_1_position_y_prev + Joint_velocity_y[0][0]*delta_t;
- Joint_2_position_y = Joint_2_position_y_prev + Joint_velocity_y[1][0]*delta_t;
-
- Joint_1_position_x_prev = Joint_1_position_x;
- Joint_2_position_x_prev = Joint_2_position_x;
-
- Joint_1_position_y_prev = Joint_1_position_y;
- Joint_2_position_y_prev = Joint_2_position_y;
-
- Motor_1_position_x = Joint_1_position_x;
- Motor_2_position_x = Joint_1_position_x + Joint_2_position_x;
-
- Motor_1_position_y = Joint_1_position_y;
- Motor_2_position_y = Joint_1_position_y + Joint_2_position_y;
-}
-
-// PI-CONTROLLER X-RICHTING
-void PI_controller_x()
+void motors()
{
-// Proportional part:
- theta_k_1_x= Kp * error_q1_x;
- theta_k_2_x= Kp * error_q2_x;
-
-// Integral part
- error_integral_1_x = error_integral_1_x + error_q1_x *delta_t;
- error_integral_2_x = error_integral_2_x + error_q2_x *delta_t;
- u_i_1_x= Ki * error_integral_1_x;
- u_i_2_x= Ki * error_integral_2_x;
-
-// sum all parts and return it
- theta_t_1_x = theta_k_1_x + u_i_1_x;
- theta_t_2_x = theta_k_2_x + u_i_2_x;
-}
-
-// PI-CONTROLLER Y-RICHTING
-void PI_controller_y()
-{
-// Proportional part:
- theta_k_1_y= Kp * error_q1_y;
- theta_k_2_y= Kp * error_q2_y;
-
-// Integral part
- error_integral_1_y = error_integral_1_y + error_q1_y *delta_t;
- error_integral_2_y = error_integral_2_y + error_q2_y *delta_t;
- u_i_1_y= Ki * error_integral_1_y;
- u_i_2_y= Ki * error_integral_2_y;
-
-// sum all parts and return it
- theta_t_1_y = theta_k_1_y + u_i_1_y;
- theta_t_2_y = theta_k_2_y + u_i_2_y;
-}
-
-void Define_motor_dir_x()
-{
- if (theta_t_1_x >= 0 && theta_t_2_x >= 0) {
- motor1_dir.write(0);
- motor2_dir.write(1);
- }
- if (theta_t_1_x < 0 && theta_t_2_x >= 0) {
- motor1_dir.write(1);
- motor1_dir.write(1);
- }
- if (theta_t_1_x >= 0 && theta_t_2_x < 0) {
- motor1_dir.write(0);
- motor2_dir.write(0);
- } else {
- motor1_dir.write(1);
- motor2_dir.write(0);
- }
-}
-
-void Define_motor_dir_y()
-{
- if (theta_t_1_y >= 0 && theta_t_2_y >= 0) {
- motor1_dir.write(0);
- motor2_dir.write(1);
- }
- if (theta_t_1_y < 0 && theta_t_2_y >= 0) {
- motor1_dir.write(1);
- motor1_dir.write(1);
- }
- if (theta_t_1_y >= 0 && theta_t_2_y < 0) {
- motor1_dir.write(0);
- motor2_dir.write(0);
- } else {
- motor1_dir.write(1);
- motor2_dir.write(0);
- }
-}
-// Finite state machine programming (calibration servo motor?)
-void ProcessStateMachine(void)
-{
+ motor1.write(0.6);
+ motor1_dir.write(1);
+ motor2.write(0.6);
+ motor2_dir.write(1);
+
// Berekenen van de motorhoeken (in radialen)
pulses_M1 = Encoder1.getPulses();
- pulses_M2 = Encoder2.getPulses();
counts1 = pulses_M1*2;
- counts2 = pulses_M2*2;
- theta_h_1_rad = conversion_factor*counts1;
- theta_h_2_rad = conversion_factor*counts2;
-
- // Calculating joint angles based on motor angles (current encoder values)
- q1 = theta_h_1_rad; // Relative angle joint 1 (rad)
- q2 = theta_h_2_rad - theta_h_1_rad; // Relative angle joint 2 (rad)
-
- // Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal
- // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled'
- filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
- filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
- filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
-
- // Vervolgens wordt de absolute waarde hiervan genomen
- filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
- filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
- filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
-
- // Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter)
- // over het signaal gedaan
- filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
- filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
- filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
-
- // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope
- scope.set(0, normalized_EMG_biceps_right);
- scope.set(1, normalized_EMG_biceps_left);
- scope.set(2, normalized_EMG_calf);
+ theta_h_1_rad = conversion_factor*(counts1);
+
+ scope.set(0, theta_h_1_rad);
scope.send();
-
- // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die
- // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende
- // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
- // waarna het gemiddelde wordt bepaald.
- if (calib) {
- if (i_calib == 0) {
- filtered_EMG_biceps_right_total=0;
- filtered_EMG_biceps_left_total=0;
- filtered_EMG_calf_total=0;
- }
- if (i_calib <= 2500) {
- filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
- filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
- filtered_EMG_calf_total+=filtered_EMG_calf;
- i_calib++;
- }
- if (i_calib > 2500) {
- mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
- mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
- mean_EMG_calf=filtered_EMG_calf_total/2500.0;
- pc.printf("Ontspan spieren\r\n");
- pc.printf("Rechterbiceps_max = %f, Linkerbiceps_max = %f, Kuit_max = %f\r\n", mean_EMG_biceps_right, mean_EMG_biceps_left, mean_EMG_calf);
- calib = false;
- }
- }
-
- // Genormaliseerde EMG's berekenen
- normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
- normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
- normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
-
- // Finite state machine
- switch (currentState) {
- case Motors_off:
-
- if (stateChanged) {
- motors_off(); // functie waarbij motoren uitgaan
- stateChanged = false;
- pc.printf("Motors off state\r\n");
- }
- if (Emergency_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
- emergency();
- }
- if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
- currentState = Calib_motor;
- stateChanged = true;
- pc.printf("Moving to Calib_motor state\r\n");
- }
- break;
-
- case Calib_motor:
-
- if (stateChanged) {
- pc.printf("Zet motoren handmatig in home positie\r\n");
- stateChanged = false;
- }
-
- if (Emergency_button_pressed.read() == false) {
- emergency();
- }
- if (Motor_calib_button_pressed.read() == false) {
- theta_h_1_rad = 0;
- theta_h_2_rad = 0;
- 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);
- currentState = Calib_EMG;
- stateChanged = true;
- pc.printf("Moving to Calib_EMG state\r\n");
- }
- break;
-
- case Calib_EMG:
-
- if (stateChanged) {
- i_calib = 0;
- calib = true;
- pc.printf("Span spieren aan\r\n");
- stateChanged = false;
- }
-
- if (Emergency_button_pressed.read() == false) {
- emergency();
- }
-
- if (i_calib > 2500) {
- calib = false;
- currentState = Homing;
- stateChanged = true;
- pc.printf("Moving to Homing state\r\n");
- }
- break;
-
- case Homing:
-
- if (stateChanged) {
- // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
- // (inclusief de end-effector) in de juiste home positie wordt gezet
- stateChanged = false;
- }
- if (Emergency_button_pressed.read() == false) {
- emergency();
- }
-
- if (theta_h_1_rad != 0.0) {
- if (theta_h_1_rad < 0) {
- motor1.write(0.3);
- motor1_dir.write(0);
- } else {
- motor1.write(0.3);
- motor1_dir.write(1);
- }
- }
- if (theta_h_1_rad == 0.0) {
- motor1.write(0);
- }
- if (theta_h_2_rad != 0.0) {
- if (theta_h_2_rad < 0) {
- motor2.write(0.3);
- motor2_dir.write(1);
- } else {
- motor2.write(0.3);
- motor2_dir.write(0);
- }
- }
- if (theta_h_2_rad == 0.0) {
- motor2.write(0);
- }
-
- if (theta_h_1_rad == 0.0 && theta_h_2_rad == 0.0)
- // Veranderen wanneer encoders werken en motor in elkaar zit
- {
- currentState = Operation_mode;
- stateChanged = true;
- pc.printf("Moving to operation mode \r\n");
- }
-
- break;
-
- case Operation_mode: // Overgaan tot emergency wanneer referentie niet
- // overeenkomt met werkelijkheid
-
- // 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);
- if (stateChanged) {
- motors_off();
- Joint_1_position_x = 0;
- Joint_2_position_x = 0;
- Joint_1_position_y = 0;
- Joint_2_position_y = 0;
- Joint_1_position_x_prev = Joint_1_position_x;
- Joint_2_position_x_prev = Joint_2_position_x;
- Joint_1_position_y_prev = Joint_1_position_y;
- Joint_2_position_y_prev = Joint_2_position_y;
- Joint_velocity_x[0][0] = 0;
- Joint_velocity_x[1][0] = 0;
- Joint_velocity_y[0][0] = 0;
- Joint_velocity_y[1][0] = 0;
- Motor_1_position_x = 0;
- Motor_2_position_x = 0;
- Motor_1_position_y = 0;
- Motor_2_position_y = 0;
- theta_k_1_x = 0.0;
- theta_k_2_x = 0.0;
- error_integral_1_x = 0.0;
- error_integral_2_x = 0.0;
- theta_k_1_y = 0.0;
- theta_k_2_y = 0.0;
- error_integral_1_y = 0.0;
- error_integral_2_y = 0.0;
- stateChanged = false;
- }
- // Hier moet een functie worden aangeroepen die ervoor zorgt dat
- // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
- // zodat de robotarm kan bewegen
-
- // if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
- // motors_off();
- // currentState = Motors_off;
- // stateChanged = true;
- // pc.printf("Terug naar de state Motors_off\r\n");
- // }
- if (Emergency_button_pressed.read() == false) {
- emergency();
- }
- if (Motor_calib_button_pressed.read() == false) { // Is nu voor de homing
- motors_off();
- currentState = Homing;
- stateChanged = true;
- pc.printf("Terug naar de state Homing\r\n");
- }
- if (normalized_EMG_biceps_right >= 0.3) {
-
- if (normalized_EMG_calf < 0.3) {
- vx = 0.0;
- vy = 0.05;
- }
- if (normalized_EMG_calf >= 0.3) {
- vx = 0.0;
- vy = -0.05;
- }
- Inverse_Kinematics();
- error_q1_y = Motor_1_position_y - theta_h_1_rad;
- error_q2_y = Motor_2_position_y - theta_h_2_rad;
- PI_controller_y();
-
- abs_theta_t_1_y = abs(theta_t_1_y);
- abs_theta_t_2_y = abs(theta_t_2_y);
-
- motor1.write(abs_theta_t_1_y);
- motor2.write(abs_theta_t_2_y);
- Define_motor_dir_y();
-
- } else if (normalized_EMG_biceps_left >= 0.3) {
- if (normalized_EMG_calf < 0.3) {
- vx = 0.05;
- vy = 0.0;
- }
- if (normalized_EMG_calf >= 0.3) {
- vx = -0.05;
- vy = 0.0;
- }
- Inverse_Kinematics();
- error_q1_x = Motor_1_position_x - theta_h_1_rad;
- error_q2_x = Motor_2_position_x - theta_h_2_rad;
- PI_controller_x();
-
- abs_theta_t_1_x = abs(theta_t_1_x);
- abs_theta_t_2_x = abs(theta_t_2_x);
-
- motor1.write(abs_theta_t_1_x);
- motor2.write(abs_theta_t_2_x);
- Define_motor_dir_x();
-
- } else {
- motor1.write(0);
- motor2.write(0);
- }
-
- break;
-
- default:
- // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
- motors_off();
- pc.printf("Unknown or uninplemented state reached!\r\n");
-
- }
}
int main(void)
{
pc.printf("Opstarten\r\n");
-
- // Chain voor rechter biceps
- bqcbr.add(&bqbr1).add(&bqbr2);
- bqcbr2.add(&bqbr3).add(&bqbr4);
- // Chain voor linker biceps
- bqcbl.add(&bqbl1).add(&bqbl2);
- bqcbl2.add(&bqbl3).add(&bqbl4);
- // Chain voor kuit
- bqck.add(&bqk1).add(&bqk2);
- bqck2.add(&bqk3).add(&bqk4);
-
- loop_ticker.attach(&ProcessStateMachine, 0.002f);
-
- while(true) {
- // wait(0.2);
- /* do nothing */
+ motor1.period_us(56);
+ motor2.period_us(56);
+ loop.attach(&motors, 0.002f);
+ while(true)
+ {
}
}
\ No newline at end of file