Diff: main.cpp

Revision:

17:fa80f1bc899b

Parent:

16:a2a73d57d556

Child:

18:c5b408405e3d

--- a/main.cpp	Thu Nov 02 15:07:53 2017 +0000
+++ b/main.cpp	Thu Nov 02 16:39:42 2017 +0000
@@ -25,8 +25,8 @@
 int angle_start1 = 100;
 int angle_start2 = 100;
 
-Motor motor1(D13 , D12 , D7 , D6  , 50000 ,  50 , 0.6 , 0 , angle_start1 , 5  , 0, 0);
-Motor motor2(D11 , D10 , D4 , D5  , 50000 ,  30 , 0.5 , 0 , angle_start2 , 1 , 0 , 0 );
+Motor motor1(D13 , D12 , D7 , D6  , 50000 ,  50 , 0.6 , 0 , angle_start1 , 0.001  , 0.1, 0);
+Motor motor2(D11 , D10 , D4 , D5  , 50000 ,  30 , 0.5 , 0 , angle_start2 , 0.001 , 0.1 , 0 );
 
 
 /*****************************************************/
@@ -76,13 +76,13 @@
 void control_motors()
 {
 
-    float time_step = 0.002;            //set the sample time
+    float time_step = 1;            //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;
     float theta_2 = 2*3.14*motor2.set_angle()/360;                      //get the angles 
-    float speed_X_axis = get_X_control_signal();
-    float speed_Y_axis = get_Y_control_signal();                           //get the desired velocitys
+    float speed_X_axis = 0.1;//get_X_control_signal();
+    float speed_Y_axis = 0;//get_Y_control_signal();                           //get the desired velocitys
     static float q_setpoint1 = 2*3.14*angle_start1/360;
     static float q_setpoint2 = 2*3.14*angle_start2/360; //define the setpoint for motor 1 and 2
    
@@ -92,7 +92,7 @@
     double radius = sqrt(x_poss * x_poss + y_poss * y_poss) ;
     
     
-    //if( radius < (L1 + L2) )
+    if( fabs(q_setpoint1 - theta_1) <= 0.01 and fabs(q_setpoint2 -theta_2) <= 0.01 )
     {
      
         if( cos(theta_2) >= 0 and cos(theta_2) < threshold )