Diff: main.cpp

Revision:

18:c5b408405e3d

Parent:

17:fa80f1bc899b

Child:

19:b51b84a1f195

--- a/main.cpp	Thu Nov 02 16:39:42 2017 +0000
+++ b/main.cpp	Fri Nov 03 00:37:57 2017 +0000
@@ -92,7 +92,7 @@
     double radius = sqrt(x_poss * x_poss + y_poss * y_poss) ;
     
     
-    if( fabs(q_setpoint1 - theta_1) <= 0.01 and fabs(q_setpoint2 -theta_2) <= 0.01 )
+    if( fabs(q_setpoint1*360/(2*3.14) - theta_1*360/(2*3.14)) <= 1 and fabs(q_setpoint2*360/(2*3.14) -theta_2*360/(2*3.14)) <= 1 )
     {
      
         if( cos(theta_2) >= 0 and cos(theta_2) < threshold )
@@ -110,6 +110,26 @@
             q_setpoint1 = theta_1 + (time_step*(speed_Y_axis*cos(theta_1 + theta_2) - speed_X_axis*sin(theta_1 + theta_2)))/(L1*cos(theta_2));
             q_setpoint2 = theta_2 + (time_step*(L1*speed_Y_axis*sin(theta_1) - L2*speed_Y_axis*cos(theta_1 + theta_2) + L2*speed_X_axis*sin(theta_1 + theta_2) + L1*speed_X_axis*cos(theta_1)))/(L1*L2*cos(theta_2));        
         }
+    if(q_setpoint1*360/(2*3.14)>100)
+    {
+     q_setpoint1 = (100.0/360)*2*3.14;    
+    }
+    
+    if(q_setpoint1*360/(2*3.14)<50)
+    {
+     q_setpoint1 = (50.0/360)*2*3.14;   
+    }
+    if(q_setpoint2*360/(2*3.14)>140)
+    {
+     q_setpoint2 = (140.0/360)*2*3.14;    
+    }
+    
+    if(q_setpoint2*360/(2*3.14)<100)
+    {
+     q_setpoint2 = (100.0/360)*2*3.14;   
+    }    
+    
+    
     }
     
     
@@ -118,8 +138,8 @@
     scope.set(2, q_setpoint1*360/(2*3.14));     
     scope.set(3, q_setpoint2*360/(2*3.14));  
     if (!a){
-    scope.set(4, motor1.Control_angle(5.0));
-    scope.set(5, motor2.Control_angle(150.0));              
+    scope.set(4, q_setpoint1*360/(2*3.14)-theta_1*360/(2*3.14));//motor1.Control_angle(5.0));
+    scope.set(5, q_setpoint2*360/(2*3.14)-theta_2*360/(2*3.14));//motor2.Control_angle(150.0));              
     }
     else{
     scope.set(4, motor1.Control_angle(q_setpoint1*360/(2*3.14)));