Diff: main.cpp

Revision:

9:22d79a4a0324

Parent:

8:0b7925095416

Child:

10:bb9a00d656c4

Child:

11:dd1976534a03

--- a/main.cpp	Fri Oct 20 14:38:14 2017 +0000
+++ b/main.cpp	Fri Oct 27 11:55:22 2017 +0000
@@ -3,80 +3,134 @@
 #include "HIDScope.h"
 #include "MODSERIAL.h"
 #include "iostream"
-DigitalIn a(D3);
-DigitalIn b(D4);
-HIDScope scope(2); // 4 channels of data
+DigitalIn a(D3);    //buttons for testing
+DigitalIn b(D2);
+double cont = 0 ;
+
+HIDScope scope(4); // 4 channels of data
 Ticker MainTicker;
 MODSERIAL pc(USBTX, USBRX);
 
 /*****************************************************************/
 //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)
-EMG EMG_tri_l(A3);// Move the endpoint backward (minus direction)
+    
+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)
+EMG EMG_tri_l(A3);                           // Move the endpoint backward (minus direction)
 
 /****************************************************/
+//Initialise Motors:
 
-Motor motor2;
-
+Motor motor2(D13 , D12 , D7 , D6  , 50000 ,  180 , 0.6 );
+Motor motor1(D11 , D10 , D4 , D5  , 50000 ,  180 , 0.2 );
 
 /*****************************************************/
 // Set control signals:
 
-double x_control_signal=0 , y_control_signal; //x direction is the righ/left movement
+//x direction is the righ/left movement
+//y direction is forward/backward movement
+
+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
+    return emg_right - emg_left;
+    
+}
 
 
-double make_X_control_signal(){
-    double emg=EMG_bi_r.filter()/10;
-    double emg2=EMG_bi_l.filter()/10;
-    
-        if(abs(emg-emg2)>=0.0025)
-        {
-            return x_control_signal+emg2-emg;
-        }
-        else
-            return x_control_signal;
+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    
+    return cont;//emg_fwd - emg_bwd;  
     
 }
 
 /******************************************************/
+//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))  ;
+    
+    // Initializing elements of matrix mult to 0.
+    for(int i = 0; i < row_J; ++i)
+    {
+        speed_setpoint[i] = 0;
+    }
 
+    // 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];
+        }
+    }
+    
+    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));
+}
+
+/******************************************************/
 // Ticker Function:
-void ReadAndFilterEMG()
+
+void mainTicker()
 {    
-    //motor2.set_angle();
-    //x_control_signal= make_X_control_signal();
+    
+    
+    control_motors();
     
-    motor2.Control_angle(x_control_signal);
-    scope.set(0, x_control_signal);
-    scope.set(1, motor2.set_angle());
+
+    
+    
+    
+   // scope.set(0, x_control_signal);
+   // scope.set(1, motor2.set_angle());
     scope.send();
     
 }
 
 
 /***************************************************/
+//Main Function:
 
-//Main Function:
 int main(void)
 {
     
-    double sample_time= 0.002;   //fs = 500Hz
-    pc.baud(115200);    //Set Baud rate for Serial communication
-    MainTicker.attach(&ReadAndFilterEMG, sample_time);    //Attach time based interrupt
+    double sample_time= 0.002;                            //fs = 500Hz
+    pc.baud(115200);                                      //Set Baud rate for Serial communication
+    MainTicker.attach(&mainTicker, sample_time);          //Attach time based interrupt
     
-    
+  
     while(true)
     {
         if(a==0){
-            x_control_signal+=10;
+            cont+=0.1;
             wait(0.1);
             }
         if(b==0){
-            x_control_signal-=10;
+            cont-=0.1;
             wait(0.1);
             }
     }