Diff: main.cpp

Revision:

10:bb9a00d656c4

Parent:

9:22d79a4a0324

Child:

12:69a9cf74583e

--- a/main.cpp	Fri Oct 27 11:55:22 2017 +0000
+++ b/main.cpp	Mon Oct 30 15:24:49 2017 +0000
@@ -53,6 +53,7 @@
 //set speed of setpoints
 void control_motors()
 {
+    /*
     int row_J =2 , row_Speed=2 , column_J =2;
     float speed_setpoint[row_J] , J_inv[row_J][column_J] , speed[row_Speed];
     
@@ -72,6 +73,10 @@
     for(int i = 0; i < row_J; ++i)
     {
         speed_setpoint[i] = 0;
+        
+     
+        
+        
     }
 
     // Multiplying matrix firstMatrix and secondMatrix and storing in array mult.
@@ -82,14 +87,43 @@
              speed_setpoint[i] += J_inv[i][k] * speed[k];
         }
     }
+    float time = 0.002 ;                            
+    motor1.Control_angle(theta_1*360/(2*3.14) + speed_setpoint[0]*time*360/(2*3.14));
+    motor2.Control_angle(theta_2*360/(2*3.14) + speed_setpoint[1]*time*360/(2*3.14));
+    */
+    
+
+    float T = 0.002;            //set the sample time
+    float threshold = 0.01;     //set the threshold for cos(theta_2)
+    float L1 = 0.48, L2 = 0.84; //set the lenght of arm 1 and 2
+    float theta_1 = 2*3.14*motor1.set_angle()/360 , theta_2 = 2*3.14*motor2.set_angle()/360;    //get the angles 
+    float v_x = get_X_control_signal(), v_y = get_Y_control_signal();                           //get the desired velocitys
+    float q_setpoint1, q_setpoint2; //define the setpoint for motor 1 and 2
+    
+    if((cos(theta_2))>=0 && cos(theta_2)<threshold){
+
+    q_setpoint1=theta_1 + (T*(v_y*cos(theta_1 + theta_2) - v_x*sin(theta_1 + theta_2)))/(L1*(threshold));
+    q_setpoint2=theta_2 + (T*(L1*v_y*sin(theta_1) - L2*v_y*cos(theta_1 + theta_2) + L2*v_x*sin(theta_1 + theta_2) + L1*v_x*cos(theta_1)))/(L1*L2*(threshold));
+
+    }
+    else if((cos(theta_2))<0 && cos(theta_2)>-threshold){
+
+    q_setpoint1=theta_1 + (T*(v_y*cos(theta_1 + theta_2) - v_x*sin(theta_1 + theta_2)))/(L1*(-threshold));
+    q_setpoint2=theta_2 + (T*(L1*v_y*sin(theta_1) - L2*v_y*cos(theta_1 + theta_2) + L2*v_x*sin(theta_1 + theta_2) + L1*v_x*cos(theta_1)))/(L1*L2*(-threshold));
+    }
+    else{
+    q_setpoint1=theta_1 + (T*(v_y*cos(theta_1 + theta_2) - v_x*sin(theta_1 + theta_2)))/(L1*cos(theta_2));
+    q_setpoint2=theta_2 + (T*(L1*v_y*sin(theta_1) - L2*v_y*cos(theta_1 + theta_2) + L2*v_x*sin(theta_1 + theta_2) + L1*v_x*cos(theta_1)))/(L1*L2*cos(theta_2));        
+    }
+    
+    
     
     scope.set(0, theta_1*360/(2*3.14));
     scope.set(1, cont);
-    scope.set(2, speed_setpoint[0]);
-    scope.set(3, speed_setpoint[1]);
-    float time = 0.002 ;                            
-    motor1.Control_angle(theta_1*360/(2*3.14) + speed_setpoint[0]*time*360/(2*3.14));
-    motor2.Control_angle(theta_2*360/(2*3.14) + speed_setpoint[1]*time*360/(2*3.14));
+    scope.set(2, v_x);
+    scope.set(3, v_y);
+    motor1.Control_angle(q_setpoint1*360/(2*3.14));
+    motor2.Control_angle(q_setpoint2*360/(2*3.14));
 }
 
 /******************************************************/