Diff: main.cpp

Revision:

31:967b455bc328

Parent:

30:0a328a9a4788

Child:

32:d651c23bbb77

--- a/main.cpp	Wed Oct 30 08:28:37 2019 +0000
+++ b/main.cpp	Wed Oct 30 10:59:10 2019 +0000
@@ -154,27 +154,45 @@
 double Motor_1_position_y = 0.0;
 double Motor_2_position_y = 0.0;
 
-double error_q1_y;
-double error_q2_y;
-
 // VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
-double error_y_integral_1 = 0.0;
-double error_y_integral_2 = 0.0;
 
 const double Kp = 17.5;
 const double Ki = 1.02;
 
-double theta_k_1 = 0.0;
-double theta_k_2 = 0.0;
+// 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 error_integral_1 = 0.0;
-double error_integral_2 = 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 u_i_1;
-double u_i_2;
+// 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 theta_t_1;
-double theta_t_2;
+double u_i_1_y;
+double u_i_2_y;
+
+double theta_t_1_y;
+double theta_t_2_y;
+
+double error_q1_y;
+double error_q2_y;
+
+double abs_theta_t_1_y;
+double abs_theta_t_2_y;
 
 // VOIDS
 
@@ -192,19 +210,19 @@
 void motors_off()
 {
     motor1.write(0);
-    motor1_dir.write(1);
     motor2.write(0);
-    motor2_dir.write(1);
     pc.printf("Motoren uit functie\r\n");
 }
 
 // Motoren aanzetten
 void motors_on()
 {
-    motor1.write(0.9);
-    motor1_dir.write(1);
-    motor2.write(0.1);
+    motor1.write(0.7);
+    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");
 }
 
@@ -244,24 +262,60 @@
     Motor_2_position_y = Joint_1_position_y + Joint_2_position_y;
 }
 
-// PI-controller y-richting
+// PI-CONTROLLER X-RICHTING
+void PI_controller_x()
+{
+// 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= Kp * error_q1_y;
-    theta_k_2= Kp * error_q2_y;
+    theta_k_1_y= Kp * error_q1_y;
+    theta_k_2_y= Kp * error_q2_y;
 
 // Integral part
-    error_integral_1 = error_integral_1 + error_q1_y *delta_t;
-    error_integral_2 = error_integral_2 + error_q2_y *delta_t;
-    u_i_1= Ki * error_integral_1;
-    u_i_2= Ki * error_integral_2;
+    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 = theta_k_1 + u_i_1;
-    theta_t_2 = theta_k_2 + u_i_2;
+    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()
+{
+    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)
 {
@@ -439,12 +493,14 @@
                 Motor_2_position_x = 0;
                 Motor_1_position_y = 0;
                 Motor_2_position_y = 0;
-                error_y_integral_1 = 0;
-                error_y_integral_2 = 0;
-                theta_k_1 = 0.0;
-                theta_k_2 = 0.0;
-                error_integral_1 = 0.0;
-                error_integral_2 = 0.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
@@ -467,21 +523,26 @@
                 pc.printf("Terug naar de state Homing\r\n");
             }
             if (normalized_EMG_biceps_right >= 0.3) {
-                vx = 0.0;
-                vy = 0.2;
+
+                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_1_rad;
+                error_q2_y = Motor_2_position_y - theta_h_2_rad;
                 PI_controller_y();
-
-                // return theta_t_1;
-                // return theta_t_2;
+                Define_motor_dir();
+                
+                abs_theta_t_1_y = abs(theta_t_1_y);
+                abs_theta_t_2_y = abs(theta_t_2_y);
 
-
-                motor1.write(0.3);
-                motor2.write(0.3);
-                motor1_dir.write(0);
-                motor2_dir.write(0);
                 //if (normalized_EMG_calf >= 0.3)
                 //{
                 //motor1.write(0.1);
@@ -489,10 +550,22 @@
                 //}
 
             } else if (normalized_EMG_biceps_left >= 0.3) {
-                motor2.write(0.9);
-                motor1.write(0.9);
+                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();
+
+
+
+
+
+
+                motor2.write(0.7);
+                motor1.write(0.7);
                 motor1_dir.write(1);
-                motor2_dir.write(1);
+                motor2_dir.write(0);
                 //if (normalized_EMG_calf >= 0.3)
                 //{
                 // motor1_dir = !motor1_dir;