Renate de Boer
/
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
Diff: main.cpp
- Revision:
- 32:d651c23bbb77
- Parent:
- 31:967b455bc328
- Child:
- 33:f81f68a912b3
- Child:
- 35:113a4015d408
- Child:
- 37:ea621fdf306a
--- a/main.cpp Wed Oct 30 10:59:10 2019 +0000
+++ b/main.cpp Wed Oct 30 14:33:50 2019 +0000
@@ -4,7 +4,7 @@
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"
-#define M_PI 3.14159265358979323846 /* pi */
+#define M_PI 3.14159265358979323846 /* pi */
#include <math.h>
#include "Servo.h"
#include <cmath>
@@ -29,8 +29,8 @@
AnalogIn EMG_biceps_left_raw (A1);
AnalogIn EMG_calf_raw (A2);
-QEI Encoder1(D12, D13, NC, 8400, QEI::X4_ENCODING); //Checken of die D12, D9 etc wel kloppen, 8400= gear ratio x 64
-QEI Encoder2(D9, D10, NC, 8400, QEI::X4_ENCODING);
+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
@@ -40,13 +40,11 @@
DigitalOut motor2_dir(D4);
// VARIABELEN VOOR ENCODER, MOTORHOEK ETC.
+int pulses_M1;
+int pulses_M2;
int counts1;
int counts2;
-const int CPR = 64; // Counts per revolution
-const int full_degrees = 360;
-const int half_degrees = 180;
-double theta_h_1_deg;
-double theta_h_2_deg;
+const double conversion_factor = (2.0*M_PI)/(64.0*131.25*2.0);
double theta_h_1_rad;
double theta_h_2_rad;
@@ -157,7 +155,7 @@
// VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
const double Kp = 17.5;
-const double Ki = 1.02;
+const double Ki = 0.03;
// Voor x-richting
double theta_k_1_x = 0.0;
@@ -175,6 +173,9 @@
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;
@@ -217,7 +218,7 @@
// Motoren aanzetten
void motors_on()
{
- motor1.write(0.7);
+ motor1.write(0.3);
motor1_dir.write(0);
motor2.write(0.3);
motor2_dir.write(1);
@@ -229,18 +230,12 @@
void Inverse_Kinematics()
{
// Defining joint velocities based on calculations of matlab
-
- 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)));
- 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)));
+
+ 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*(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)));
- 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)));
-
- q1_dot_x = Joint_velocity_x[0][0];
- q2_dot_x = Joint_velocity_x[1][0];
-
- q1_dot_y = Joint_velocity_y[0][0];
- q2_dot_y = Joint_velocity_y[1][0];
+ 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;
@@ -298,7 +293,26 @@
theta_t_2_y = theta_k_2_y + u_i_2_y;
}
-void Define_motor_dir()
+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);
@@ -320,12 +334,12 @@
void ProcessStateMachine(void)
{
// Berekenen van de motorhoeken (in radialen)
- counts1 = Encoder1.getPulses();
- counts2 = Encoder2.getPulses();
- theta_h_1_deg=(counts1/(double)CPR)*(double)full_degrees;
- theta_h_2_deg=(counts2/(double)CPR)*(double)full_degrees;
- theta_h_1_rad=(theta_h_1_deg/half_degrees)*M_PI;
- theta_h_2_rad=(theta_h_2_deg/half_degrees)*M_PI;
+ 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)
@@ -445,23 +459,43 @@
}
break;
- case Homing: // NOG NAAR KIJKEN
+ 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
- motors_on();
stateChanged = false;
}
if (Emergency_button_pressed.read() == false) {
emergency();
}
- // Hier moet iets van encoder counts waarde maal -1 worden gedaan om
- // naar positie 0 te bewegen
+ 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 (Power_button_pressed.read() == false)
- //if (theta_h_1_rad == 0.0 && theta_h_2_rad == 0.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;
@@ -524,13 +558,11 @@
}
if (normalized_EMG_biceps_right >= 0.3) {
- if (normalized_EMG_calf < 0.3)
- {
+ if (normalized_EMG_calf < 0.3) {
vx = 0.0;
vy = 0.05;
}
- if (normalized_EMG_calf >= 0.3)
- {
+ if (normalized_EMG_calf >= 0.3) {
vx = 0.0;
vy = -0.05;
}
@@ -538,40 +570,35 @@
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();
- Define_motor_dir();
-
+
abs_theta_t_1_y = abs(theta_t_1_y);
abs_theta_t_2_y = abs(theta_t_2_y);
- //if (normalized_EMG_calf >= 0.3)
- //{
- //motor1.write(0.1);
- //motor1_dir = !motor1_dir;
- //}
+ 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) {
- vx = 0.05;
- vy = 0.0;
+ 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);
- motor2.write(0.7);
- motor1.write(0.7);
- motor1_dir.write(1);
- motor2_dir.write(0);
- //if (normalized_EMG_calf >= 0.3)
- //{
- // motor1_dir = !motor1_dir;
- // pc.printf("Richting zou om moeten draaien");
- // motor2_dir = !motor2_dir;
- //}
+ 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);