Diff: main.cpp

Revision:

6:aa62e6650559

Parent:

5:4b2ff2a4664a

Child:

7:075ba23f3147

diff -r 4b2ff2a4664a -r aa62e6650559 main.cpp
--- a/main.cpp	Fri Oct 28 10:26:11 2016 +0000
+++ b/main.cpp	Mon Oct 31 11:16:29 2016 +0000
@@ -86,28 +86,7 @@
     Active = !Active;
     }
     
-/////////////-----------------------Booting---------------------------------------=
-void Boot(){
-  
-  //Fucnction that initializes variables that have to be initalized within the main of the script
-  //And does the same thing for functions like encoder reset
-    pc.baud(115200);
-    pc.printf("\r\n**BOARD RESET**\r\n");
-    
-    M_L_S.period(1.0/Motor_Frequency);
-    M_L_S = 0.0;
-    M_R_S.period(1.0/Motor_Frequency);
-    M_R_S= 0.0;
-    
-    Encoder_L.reset();
-    Encoder_R.reset();
-    
-    Grap = 1;
-    grip1 = 1;
-    }
 
-    
-    
 //--------------------------------Sampling------------------------------------------   
 
 Ticker Tick_Sample;// ticker for data sampling
@@ -257,6 +236,7 @@
     //-----------------------------EMG Signal determinator----------------------
 BiQuadChain bqc_L;
 BiQuadChain bqc_R;
+BiQuadChain bqc_Change;
 BiQuad bq1_R( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
 BiQuad bq2_R( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
 BiQuad bq3_R( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
@@ -265,65 +245,95 @@
 BiQuad bq2_L( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
 BiQuad bq3_L( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
 
-double getEMG(double EMG_Out){
+
+BiQuad bq1_Change( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
+BiQuad bq2_Change( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
+BiQuad bq3_Change( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
+
+void getEMG(){
+     
  
+    EMG_Change = bqc_Change.step(Change.read());
+    EMG_Change = fabs(EMG_Change);
+    EMG_Change = bq3_Change.step(EMG_Change)*1.0;
+         scope.set(0,EMG_Change);//=------------------------------------------------------------------------Scope         
+     if (EMG_Change < 0.3){
+        EMG_Change=0;
+    } else {
+        EMG_Change=1.0;
+    }
+           
+            
+    EMG_L =   bqc_L.step(Left.read());
+    EMG_L= fabs(EMG_L);
+    EMG_L= bq3_L.step( EMG_L)*1.0;
+                    scope.set(1,EMG_L);//=------------------------------------------------------------------------Scope         
+    if (EMG_L < 0.3){
+        EMG_L=0;
+    } else {
+        EMG_L=1.0;
+    }
+    
+               
   
+    EMG_R =  bqc_R.step(Right.read());
+    EMG_R= fabs(EMG_R);
+    EMG_R= bq3_R.step( EMG_R)*1.0;        
+          scope.set(2,EMG_R);//=------------------------------------------------------------------------Scope                  
+    if (EMG_R < 0.3){
+        EMG_R=0;
+    } else {
+        EMG_R= 1.0;
+    }
     
-    return EMG_Out;
 }
+
+/////////////-----------------------Booting---------------------------------------=
+void Boot(){
+  
+  //Fucnction that initializes variables that have to be initalized within the main of the script
+  //And does the same thing for functions like encoder reset
+    pc.baud(115200);
+    pc.printf("\r\n**BOARD RESET**\r\n");
+    
+    M_L_S.period(1.0/Motor_Frequency);
+    M_L_S = 0.0;
+    M_R_S.period(1.0/Motor_Frequency);
+    M_R_S= 0.0;
+                       rotation = 1;
+                   translation = 0;
+    Encoder_L.reset();
+    Encoder_R.reset();
+    
+    Grap = 1;
+    grip1 = 1;
+    
+        bqc_L.add( &bq1_L ).add( &bq2_L );
+    bqc_R.add(&bq1_R).add(&bq2_R);
+    bqc_Change.add(&bq1_Change).add(&bq2_Change);
+    Tick_Sample.attach(Set_Sample_Flag, 1.0/SampleFrequency);    Tick_Controller.attach(Set_controller_Flag, 1.0/SampleFrequency);
+    Tick_Movement.attach(Set_movement_Flag, 0.25);
+    OnOff.fall(Start_Stop);
+    }
+
+    
+    
 int main()
 {
 //---------------------Setting constants and system booting---------------------
    
     Boot();
-    bqc_L.add( &bq1_L ).add( &bq2_L );
-    bqc_R.add(&bq1_R).add(&bq2_R);
-    Tick_Sample.attach(Set_Sample_Flag, 1.0/SampleFrequency);    Tick_Controller.attach(Set_controller_Flag, 1.0/SampleFrequency);
-    Tick_Movement.attach(Set_movement_Flag, 0.25);
-    OnOff.fall(Start_Stop);
+
     //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
     //\/\/\/\/\/\/\/\/\/\/\/\/\/\Main Loop for program\/\/\/\/\/\/\/\/\/\/\/\/\/
     //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/  
     while (true) { 
-        if (Sample_Flag == true){
+       if (Sample_Flag == true){
             Sample();
-            EMG_L =   bqc_L.step(Left.read());
-             EMG_L= fabs(EMG_L);
-    EMG_L= bq3_L.step( EMG_L)*10.0;
-                    scope.set(0,EMG_L);
-    if (EMG_L < 0.2){
-        EMG_L=0;
-    } else if (EMG_L >= 0.2 && EMG_L < 0.5){
-        EMG_L=0.33;
-    } else if (EMG_L >= 0.5 && EMG_L < 0.8){
-        EMG_L=0.67;
-    } else {
-        EMG_L=1.0;
-    }
-    
-                scope.set(1,EMG_L);
-  
-            EMG_R =  bqc_R.step(Right.read());
-             EMG_R= fabs(EMG_R);
-    EMG_R= bq3_R.step( EMG_R)*10.0;
-        
-                            scope.set(2,EMG_R);
-    if (EMG_R < 0.2){
-        EMG_R=0;
-    } else if (EMG_R >= 0.2 && EMG_R < 0.5){
-        EMG_R=0.33;
-    } else if (EMG_R >= 0.5 && EMG_R < 0.8){
-        EMG_R=0.67;
-    } else {
-        EMG_R= 1.0;
-    }
-    
-                            scope.set(3,EMG_R);
-
-          //  EMG_Change = getEMG(  bqc.step(Change.read()));
-            EMG_Change = 0;
+            getEMG();
+              //  EMG_Change = getEMG(  bqc.step(Change.read()));
             Signal = pi*(EMG_R-EMG_L);
-        
+                scope.set(3,Signal);
 
                          scope.send();
             Sample_Flag = false;
@@ -339,12 +349,16 @@
                    //Clockwise rotation of the robot
                    direction_L = -1;
                    direction_R = -1;
+                   rotation = 1;
+                   translation = 0;
                    break;
                    
                    case 1:
                    //Radial movement outwards for a positive value
                    direction_L = 1;
                    direction_R = -1;
+                   rotation = 0;
+                   translation = 1;
                    break;
                    }
                M_L_S = Controller_L(direction_L,Signal,Speed_L);