Diff: main.cpp

Revision:

21:59431788a42d

Parent:

20:c3be3bb3b515

--- a/main.cpp	Fri Nov 03 09:08:35 2017 +0000
+++ b/main.cpp	Mon Nov 06 09:57:21 2017 +0000
@@ -8,6 +8,7 @@
 //AnalogIn pot1(A0); //potmeters for testing
 //AnalogIn pot2(A1);
 double cont = 0 ;
+bool test;
 
 HIDScope scope(6); // 4 channels of data
 Ticker MainTicker;
@@ -22,11 +23,11 @@
 
 /****************************************************/
 //Initialise Motors:
-int angle_start1 = 100;//51.3676;
-int angle_start2 = 100;//140.2431;
+int angle_start1 = 90;//51.3676;
+int angle_start2 = 90;//140.2431;
 
-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 );
+Motor motor1(D13 , D12 , D7 , D6  , 50000 ,  180 , 0.7 , 0 , angle_start1 , 5 , 1, 0.00000);
+Motor motor2(D11 , D10 , D4 , D5  , 50000 ,  90 , 0.7 , 0 , angle_start2 , 7 , 1 , 0.00000 );
 
 
 /*****************************************************/
@@ -37,9 +38,9 @@
 
 double get_X_control_signal(){
     double emg_right = EMG_bi_r.filter();
-    double emg_left =  EMG_bi_l.filter();
+    double emg_left =  1.5*EMG_bi_l.filter();
    //scope.set(0 ,emg_right-emg_left);
-    if (fabs(emg_right - emg_left) < 0.008 )
+    if (fabs(emg_right - emg_left) < 0.002 )
     {
         return 0;
     }
@@ -55,9 +56,9 @@
 
 double get_Y_control_signal(){
     double emg_right = EMG_tri_r.filter();
-    double emg_left =  EMG_tri_l.filter();
+    double emg_left = 1.5*EMG_tri_l.filter();
     //scope.set(1 ,emg_right-emg_left);
-    if (fabs(emg_right - emg_left) < 0.008 )
+    if (fabs(emg_right - emg_left) < 0.002 )
     {
         return 0;
     }
@@ -76,75 +77,25 @@
 void control_motors()
 {
 
-    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 = 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
-   
-    double x_poss = L2*sin(theta_1 + theta_2) + L1*cos(theta_1);
-    double y_poss = L1*sin(theta_1) - L2*cos(theta_1 + theta_2);
-    
-    double radius = sqrt(x_poss * x_poss + y_poss * y_poss) ;
+    float speed_X_axis = 0 , speed_Y_axis = 0 ;
+    if (test)
+        speed_X_axis = get_X_control_signal();
+    else
+        speed_Y_axis = get_X_control_signal();                           //get the desired velocitys
     
     
-    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 )
-        {
-            q_setpoint1 = theta_1 + (time_step*(speed_Y_axis*cos(theta_1 + theta_2) - speed_X_axis*sin(theta_1 + theta_2)))/(L1*(threshold));
-            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*(threshold));
-        }
-        else if( cos(theta_2) < 0 and cos(theta_2) > -threshold)
-        {
-            q_setpoint1 = theta_1 + (time_step*(speed_Y_axis*cos(theta_1 + theta_2) - speed_X_axis*sin(theta_1 + theta_2)))/(L1*(-threshold));
-            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*(-threshold));
-        }
-        else
-        {
-            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.0/(2*3.14)>100)
-    {
-     q_setpoint1 = (100.0/360)*2*3.14;    
-    }
-    
-    if(q_setpoint1*360.0/(2*3.14)<50)
-    {
-     q_setpoint1 = (50.0/360)*2*3.14;   
-    }
-    if(q_setpoint2*360.0/(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;   
-    }    
-    
-    
-    }
     
     
     scope.set(0, theta_1*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){
-    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)));
-    scope.set(5, motor2.Control_angle(q_setpoint2*360/(2*3.14)));  
-    }
+    scope.set(1, theta_2*360/(2*3.14));// 
+    scope.set(2, speed_X_axis);     
+    scope.set(3, speed_Y_axis); 
+    scope.set(4, motor1.Control_angle(theta_1*360.0/(2*3.14) + speed_X_axis*5));
+    scope.set(5, motor2.Control_angle(theta_2*360.0/(2*3.14) + speed_Y_axis*5));  
+    
         
 }
 
@@ -172,8 +123,8 @@
     while(true)
     {
         if(a==0){
-            cont+=0.033;
-            wait(0.1);
+            test=!test;
+            wait(0.5);
             }
         if(b==0){
             cont-=0.033;