Diff: main.cpp

Revision:

12:69a9cf74583e

Parent:

11:dd1976534a03

Parent:

10:bb9a00d656c4

Child:

13:559f8946f16d

--- a/main.cpp	Mon Oct 30 15:32:27 2017 +0000
+++ b/main.cpp	Mon Oct 30 16:28:21 2017 +0000
@@ -23,8 +23,8 @@
 /****************************************************/
 //Initialise Motors:
 
-Motor motor2(D13 , D12 , D7 , D6  , 50000 ,  180 , 0.5 );
-Motor motor1(D11 , D10 , D4 , D5  , 50000 ,  180 , 0.5 );
+Motor motor2(D13 , D12 , D7 , D6  , 50000 ,  50 , 0.3 );
+Motor motor1(D11 , D10 , D4 , D5  , 50000 ,  50 , 0.5 );
 
 /*****************************************************/
 // Set control signals:
@@ -35,7 +35,7 @@
 double get_X_control_signal(){
     double emg_right = EMG_bi_r.filter();
     double emg_left =  EMG_bi_l.filter();
-                                                 // TODO: Tune emg to velocity mapping
+                                                 // time_stepODO: time_stepune emg to velocity mapping
     return  emg_right - emg_left;
     
 }
@@ -44,7 +44,7 @@
 double get_Y_control_signal(){
     double emg_fwd= EMG_tri_r.filter();
     double emg_bwd= EMG_tri_l.filter();
-                                                 // TODO: `Tune emg to velocity mapping    
+                                                 // time_stepODO: `time_stepune emg to velocity mapping    
     return cont;// emg_fwd - emg_bwd;  
     
 }
@@ -53,46 +53,39 @@
 //set speed of setpoints
 void control_motors()
 {
-    int row_J =2 , row_Speed=2 , column_J =2;
-    float speed_setpoint[row_J] , J_inv[row_J][column_J] , speed[row_Speed];
-    
-    speed[0] = 0;//get_X_control_signal();
-    speed[1] = get_Y_control_signal();
-    
-    float theta_1 = 2*3.14*motor1.set_angle()/360 , theta_2 = 2*3.14*motor2.set_angle()/360; 
-    float L1 = 0.48;
-    float L2 = 0.84;
-    
-    J_inv[0][0] = -sin(theta_1 + theta_2)/(L1*cos(theta_2)) ;
-    J_inv[0][1] = cos(theta_1 + theta_2)/(L1*cos(theta_2))  ;
-    J_inv[1][0] = (L2*sin(theta_1 + theta_2) + L1*cos(theta_1))/(L1*L2*cos(theta_2))  ;
-    J_inv[1][1] = -(L2*cos(theta_1 + theta_2) - L1*sin(theta_1))/(L1*L2*cos(theta_2))  ;
+
+    float time_step = 0.002;            //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 , theta_2 = 2*3.14*motor2.set_angle()/360;                      //get the angles 
+    float speed_X_axis = 0;//get_X_control_signal()
+    float speed_Y_axis = get_Y_control_signal();                           //get the desired velocitys
+    float q_setpoint1, q_setpoint2; //define the setpoint for motor 1 and 2
     
-    // Initializing elements of matrix mult to 0.
-    for(int i = 0; i < row_J; ++i)
-    {
-        speed_setpoint[i] = 0;
+    if((cos(theta_2))>=0 && 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 && cos(theta_2)>-threshold){
 
-    // Multiplying matrix firstMatrix and secondMatrix and storing in array mult.
-    for(int i = 0; i < row_J; ++i)
-    {
-        for(int k=0; k<column_J; ++k)
-        {
-             speed_setpoint[i] += J_inv[i][k] * speed[k];
-        }
+    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));
     }
-     float time = 0.002 ; 
+    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));        
+    }
+    
+    
     
     scope.set(0, theta_1*360/(2*3.14));
-    scope.set(1, cont);
-    scope.set(2, theta_1*360/(2*3.14) + speed_setpoint[0]*time*360/(2*3.14));
-    scope.set(3, theta_2*360/(2*3.14) + speed_setpoint[1]*time*360/(2*3.14));
-    
-   
-                               
-    scope.set(4, motor1.Control_angle(theta_1*360/(2*3.14) + speed_setpoint[0]*time*360/(2*3.14)) );
-    scope.set(5, motor2.Control_angle(theta_2*360/(2*3.14) + speed_setpoint[1]*time*360/(2*3.14)) );
+    scope.set(1, cont);     
+    scope.set(2, theta_1*360/(2*3.14));
+    scope.set(3, speed_Y_axis);                    
+    scope.set(4, motor1.Control_angle(q_setpoint1*360/(2*3.14)) );
+    scope.set(5, motor2.Control_angle(q_setpoint2*360/(2*3.14)) );
     
 }
 
@@ -127,11 +120,11 @@
     while(true)
     {
         if(a==0){
-            cont+=0.1;
+            cont+=0.01;
             wait(0.1);
             }
         if(b==0){
-            cont-=0.1;
+            cont-=0.01;
             wait(0.1);
             }
     }