Diff: main.cpp

Revision:

11:dd1976534a03

Parent:

9:22d79a4a0324

Child:

12:69a9cf74583e

--- a/main.cpp	Fri Oct 27 11:55:22 2017 +0000
+++ b/main.cpp	Mon Oct 30 15:32:27 2017 +0000
@@ -7,7 +7,7 @@
 DigitalIn b(D2);
 double cont = 0 ;
 
-HIDScope scope(4); // 4 channels of data
+HIDScope scope(6); // 4 channels of data
 Ticker MainTicker;
 MODSERIAL pc(USBTX, USBRX);
 
@@ -23,8 +23,8 @@
 /****************************************************/
 //Initialise Motors:
 
-Motor motor2(D13 , D12 , D7 , D6  , 50000 ,  180 , 0.6 );
-Motor motor1(D11 , D10 , D4 , D5  , 50000 ,  180 , 0.2 );
+Motor motor2(D13 , D12 , D7 , D6  , 50000 ,  180 , 0.5 );
+Motor motor1(D11 , D10 , D4 , D5  , 50000 ,  180 , 0.5 );
 
 /*****************************************************/
 // Set control signals:
@@ -36,7 +36,7 @@
     double emg_right = EMG_bi_r.filter();
     double emg_left =  EMG_bi_l.filter();
                                                  // TODO: Tune emg to velocity mapping
-    return emg_right - emg_left;
+    return  emg_right - emg_left;
     
 }
 
@@ -45,7 +45,7 @@
     double emg_fwd= EMG_tri_r.filter();
     double emg_bwd= EMG_tri_l.filter();
                                                  // TODO: `Tune emg to velocity mapping    
-    return cont;//emg_fwd - emg_bwd;  
+    return cont;// emg_fwd - emg_bwd;  
     
 }
 
@@ -82,14 +82,18 @@
              speed_setpoint[i] += J_inv[i][k] * speed[k];
         }
     }
+     float time = 0.002 ; 
     
     scope.set(0, theta_1*360/(2*3.14));
     scope.set(1, cont);
-    scope.set(2, speed_setpoint[0]);
-    scope.set(3, speed_setpoint[1]);
-    float time = 0.002 ;                            
-    motor1.Control_angle(theta_1*360/(2*3.14) + speed_setpoint[0]*time*360/(2*3.14));
-    motor2.Control_angle(theta_2*360/(2*3.14) + speed_setpoint[1]*time*360/(2*3.14));
+    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)) );
+    
 }
 
 /******************************************************/
@@ -102,9 +106,6 @@
     control_motors();
     
 
-    
-    
-    
    // scope.set(0, x_control_signal);
    // scope.set(1, motor2.set_angle());
     scope.send();