Diff: main.cpp

Revision:

20:c3be3bb3b515

Parent:

19:b51b84a1f195

Child:

21:59431788a42d

--- a/main.cpp	Fri Nov 03 00:40:40 2017 +0000
+++ b/main.cpp	Fri Nov 03 09:08:35 2017 +0000
@@ -5,8 +5,8 @@
 #include "iostream"
 DigitalIn a(D3);    //buttons for testing
 DigitalIn b(D2);
-AnalogIn pot1(A0); //potmeters for testing
-AnalogIn pot2(A1);
+//AnalogIn pot1(A0); //potmeters for testing
+//AnalogIn pot2(A1);
 double cont = 0 ;
 
 HIDScope scope(6); // 4 channels of data
@@ -22,11 +22,11 @@
 
 /****************************************************/
 //Initialise Motors:
-int angle_start1 = 100;
-int angle_start2 = 100;
+int angle_start1 = 100;//51.3676;
+int angle_start2 = 100;//140.2431;
 
-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 );
+Motor motor1(D13 , D12 , D7 , D6  , 50000 ,  90 , 0.5 , 0 , angle_start1 , 0.05 , 0.01, 0.00000);
+Motor motor2(D11 , D10 , D4 , D5  , 50000 ,  50 , 0.6 , 0 , angle_start2 , 0.07 , 0.01 , 0.00000 );
 
 
 /*****************************************************/
@@ -38,8 +38,8 @@
 double get_X_control_signal(){
     double emg_right = EMG_bi_r.filter();
     double emg_left =  EMG_bi_l.filter();
-   // scope.set(0 ,emg_right-emg_left);
-    if (1)//fabs(emg_right - emg_left) < 0.008 )
+   //scope.set(0 ,emg_right-emg_left);
+    if (fabs(emg_right - emg_left) < 0.008 )
     {
         return 0;
     }
@@ -76,12 +76,12 @@
 void control_motors()
 {
 
-    float time_step = 1;            //set the sample time
+    float time_step = 0.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 = 0.1;//get_X_control_signal();
+    float speed_X_axis = 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( 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( fabs(q_setpoint1*360.0/(2*3.14) - theta_1*360/(2*3.14)) <= 1 and fabs(q_setpoint2*360.0/(2*3.14) -theta_2*360.0/(2*3.14)) <= 1 )
     {
      
         if( cos(theta_2) >= 0 and cos(theta_2) < threshold )
@@ -110,23 +110,23 @@
             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)
+    if(q_setpoint1*360.0/(2*3.14)>100)
     {
-        q_setpoint1 = (100.0/360)*2*3.14;    
+     q_setpoint1 = (100.0/360)*2*3.14;    
     }
     
-    if(q_setpoint1*360/(2*3.14)<50)
+    if(q_setpoint1*360.0/(2*3.14)<50)
     {
-        q_setpoint1 = (50.0/360)*2*3.14;   
+     q_setpoint1 = (50.0/360)*2*3.14;   
     }
-    if(q_setpoint2*360/(2*3.14)>140)
+    if(q_setpoint2*360.0/(2*3.14)>140)
     {
-        q_setpoint2 = (140.0/360)*2*3.14;    
+     q_setpoint2 = (140.0/360)*2*3.14;    
     }
     
     if(q_setpoint2*360/(2*3.14)<100)
     {
-        q_setpoint2 = (100.0/360)*2*3.14;   
+     q_setpoint2 = (100.0/360)*2*3.14;   
     }    
     
     
@@ -134,7 +134,7 @@
     
     
     scope.set(0, theta_1*360/(2*3.14));     
-    scope.set(1, theta_2*360/(2*3.14)); 
+    scope.set(1, speed_X_axis);//theta_2*360/(2*3.14));// 
     scope.set(2, q_setpoint1*360/(2*3.14));     
     scope.set(3, q_setpoint2*360/(2*3.14));  
     if (!a){