Diff: main.cpp

Revision:

14:f561498eee28

Parent:

13:559f8946f16d

Child:

16:a2a73d57d556

--- a/main.cpp	Wed Nov 01 07:11:00 2017 +0000
+++ b/main.cpp	Wed Nov 01 13:08:04 2017 +0000
@@ -5,6 +5,8 @@
 #include "iostream"
 DigitalIn a(D3);    //buttons for testing
 DigitalIn b(D2);
+AnalogIn pot1(A0); //potmeters for testing
+AnalogIn pot2(A1);
 double cont = 0 ;
 
 HIDScope scope(6); // 4 channels of data
@@ -12,9 +14,7 @@
 MODSERIAL pc(USBTX, USBRX);
 
 /*****************************************************************/
-//Initialize Analog EMG inputs:
-
-    
+//Initialize Analog EMG inputs:  
 EMG EMG_bi_r(A0);                            // Move  the endpoint to the right (plus direction)
 EMG EMG_bi_l(A1);                            // Move the endpoint to the left (minus direction)
 EMG EMG_tri_r(A2);                           // Move the endpoint forward (plus direction)
@@ -22,11 +22,11 @@
 
 /****************************************************/
 //Initialise Motors:
-int angle_start1 = 80;
-int angle_start2 = 100;
+int angle_start1 = 5;
+int angle_start2 = 125;
 
-Motor motor1(D13 , D12 , D7 , D6  , 50000 ,  90 , 0.5  , 0 , angle_start1);
-Motor motor2(D11 , D10 , D4 , D5  , 50000 ,  50 , 0.5 , 0 , angle_start2);
+Motor motor1(D13 , D12 , D7 , D6  , 50000 ,  90 , 0.6  , 0 , angle_start1);
+Motor motor2(D11 , D10 , D4 , D5  , 50000 ,  50 , 0.6 , 0 , angle_start2);
 
 /*****************************************************/
 // Set control signals:
@@ -38,7 +38,7 @@
     double emg_right = EMG_bi_r.filter();
     double emg_left =  EMG_bi_l.filter();
                                                  // TODO: time_stepune emg to velocity mapping
-    return 0;// emg_right - emg_left;
+    return 0; // (pot2-0.5)/2;// emg_right - emg_left;
     
 }
 
@@ -47,7 +47,7 @@
     double emg_fwd= EMG_tri_r.filter();
     double emg_bwd= EMG_tri_l.filter();
                                                  // TODO: `time_stepune emg to velocity mapping    
-    return cont;// emg_fwd - emg_bwd;  
+    return 0.06;//(pot1-0.5)/2;// emg_fwd - emg_bwd;  
     
 }
 
@@ -57,7 +57,7 @@
 {
 
     float time_step = 0.002;            //set the sample time
-    float threshold = 0.05;     //set the threshold for cos(theta_2)
+    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 
@@ -65,14 +65,14 @@
     float speed_Y_axis = 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) ;
     
     
-    if( radius < (L1 + L2) )
+    //if( radius < (L1 + L2) )
     {
      
         if( cos(theta_2) >= 0 and cos(theta_2) < threshold )
@@ -93,13 +93,20 @@
     }
     
     
+    
     scope.set(0, theta_1*360/(2*3.14));
-    scope.set(1, theta_2*360/(2*3.14));
-    scope.set(2, cont);     
-    scope.set(3, radius);  
-    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, q_setpoint2);//theta_2*360/(2*3.14));
+    scope.set(2, (pot1-0.5)/2 );     
+    scope.set(3, q_setpoint1);  
+    if (!a){
+    scope.set(4, motor1.Control_angle(5.0));
+    scope.set(5, motor2.Control_angle(125.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)));  
+    }
+        
 }
 
 /******************************************************/