Diff: main.cpp

Revision:

1:df3d7f71db4b

Parent:

0:4a9c733c3b53

Child:

2:8632e61cafc8

diff -r 4a9c733c3b53 -r df3d7f71db4b main.cpp
--- a/main.cpp	Mon Oct 22 14:52:06 2018 +0000
+++ b/main.cpp	Mon Oct 29 14:59:34 2018 +0000
@@ -2,31 +2,31 @@
 #include "math.h"
 #include "Matrix.h"
 
-const float L0;                     // Length between two motors [meter]
-const float L1;                     // Length first beam from right motor2 [meter]
-const float L2;                     // Length second beam from right motor2 [meter]
-const float L3;                     // Length beam between L2 and L4 [meter]
-const float L4;                     // Length first beam from left motor1 [meter]
-const float L5;                     // Length from L3 to end-effector [meter]
+const float L0 = 0.15;                      // Length between two motors [meter]
+const float L1 = 0.10;                      // Length first beam from right motor2 [meter]
+const float L2 = 0.30;                      // Length second beam from right motor2 [meter]
+const float L3 = 0.15;                      // Length beam between L2 and L4 [meter]
+const float L4 = 0.30;                      // Length first beam from left motor1 [meter]
+const float L5 = 0.35;                      // Length from L3 to end-effector [meter]
 const double PI = 3.14159265359;
-volatile float Pe_x;                // x-coordinate of end-effector from frame 0 [meter]
-volatile float Pe_y;                // y-coordinate of end-effector from frame 0 [meter]
-volatile float motor_angle1;        // Desired angle of motor 1 (left) [rad]
-volatile float motor_angle2;        // Desired angle of motor 2 (right) [rad]
+volatile float Pe_x;                        // x-coordinate of end-effector from frame 0 [meter]
+volatile float Pe_y;                        // y-coordinate of end-effector from frame 0 [meter]
+volatile static float des_motor_angle1;     // Desired angle of motor 1 (left) based on kinematics [rad]
+volatile static float des_motor_angle2;     // Desired angle of motor 2 (right) based on kinematics [rad]
 
-ticker IK                           // Ticker function for inverse kinematics
+Ticker kinematics_ticker;                   // Ticker function for inverse kinematics
 
 void InverseKinematics()
 {
     // Calculation of the position of joint 3 in frame 0
-    float n = sqrt(pow(Pe_x,2) + pow(Pe_y,2);                           // Radius between origin frame 0 and endeffector [meter]
+    float n = sqrt(pow(Pe_x,2) + pow(Pe_y,2));                          // Radius between origin frame 0 and endeffector [meter]
     float omega = acos(-(pow(n,2) - pow(L4,2) - pow(L5,2))/(2*L4*L5));  // Angle between L4 and L5 [rad]
     float q4 = PI - omega;                                              // Angle of joint 3 between L3 and L4
-    float theta = atan(L5*sin(q4)/(L4 + L5*cos(q4));                    // Angle between n and L4
+    float theta = atan(L5*sin(q4)/(L4 + L5*cos(q4)));                   // Angle between n and L4
     float lambda = PI - atan(abs(Pe_y/Pe_x));                           // Entire angle between x-axis frame 0 and n
-    float motor_angle1 = lambda - theta;
-    float J3x_0 = L4*cos(q3);                                           // x-coordinate of joint 3 in frame 0
-    float J3y_0 = L4*sin(q3);                                           // y-coordinate of joint 3 in frame 0
+    float des_motor_angle1 = lambda - theta;
+    float J3x_0 = L4*cos(des_motor_angle1);                             // x-coordinate of joint 3 in frame 0
+    float J3y_0 = L4*sin(des_motor_angle1);                             // y-coordinate of joint 3 in frame 0
     
     // Calculation of the position of joint 2 in frame 0 
     float S = abs(J3y_0 - Pe_y);                                        // Distance between height endeffector and joint 3
@@ -37,27 +37,27 @@
     // Calculation of the position of joint 1 in frame 0
     float J2x_1 = J2x_0 - L0;                                           // x-coordinate of joint 2 in frame 1
     float J2y_1 = J2y_0;                                                // y-coordinate of joint 2 in frame 1
-    float r = sqrt(pow(J2x_1,2) + pow(J2y_1,2);                         // Radius between origin frame 1 and J2
-    float alfa = acos(-(pow(r,2) - pow(L1,2) - pow(L2,2))/(2*L1*L2      // Angle opposite of radius r
+    float r = sqrt(pow(J2x_1,2) + pow(J2y_1,2));                        // Radius between origin frame 1 and J2
+    float alfa = acos( -(pow(r,2) - pow(L1,2) - pow(L2,2))/(2*L1*L2) ); // Angle opposite of radius r
     float q2 = PI - alfa;                                               // Angle between L1 and L2
     
     // Calculation of motor_angle2
     float beta = atan(L2*sin(q2)/(L1+L2*cos(q2)));                      // Angle between r and L1
-    float gamma = PI - atan(abs(J2y_1/J2x_1);                           // Angle between r and x-axis
+    float gamma = PI - atan(abs(J2y_1/J2x_1));                          // Angle between r and x-axis
     // check if gamma works!
-    m_anle2 = gamma - beta;
-    float J1x_0 = L0 + L1*cos(q1);                                      // x-coordinate of joint 1 in frame 0
-    float J1y_0 = L1*sin(q1);                                           // y-coordinate of joint 1 in frame 0   
+    des_motor_angle2 = gamma - beta;
+    float J1x_0 = L0 + L1*cos(des_motor_angle2);                        // x-coordinate of joint 1 in frame 0
+    float J1y_0 = L1*sin(des_motor_angle2);                             // y-coordinate of joint 1 in frame 0   
     
-    
-    
-    
-    return motor_angle1
-    return motor_angle2
+
+    // Determining angle delta for safety
+    float m = sqrt(pow((abs(J3x_0)+J1x_0),2) + pow((J3y_0 - J1y_0),2)); // Radius between Joint 1 and Joint 3
+    float delta = acos(- (pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3)); // Angle between L2 and L3
+
 }    
 
 
 int main()
 {
-    IK.attach(InverseKinematics)
+    kinematics_ticker.attach(InverseKinematics,0.5);
 }
\ No newline at end of file