Diff: main.cpp

Revision:

5:4b2ff2a4664a

Parent:

4:8b1df22779a7

Child:

6:aa62e6650559

--- a/main.cpp	Tue Oct 25 09:29:12 2016 +0000
+++ b/main.cpp	Fri Oct 28 10:26:11 2016 +0000
@@ -1,10 +1,9 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "QEI.h"
-
 #include "math.h"
 #include "HIDScope.h"
-#include "Biquad.h"
+#include "BiQuad.h"
  
 
 ///////////////////////////
@@ -30,10 +29,8 @@
 //--------------------Encoders--------------------------------
 
 //Variables
-   volatile int movement = 0,direction_L =0, direction_R =0;
-    volatile double Signal = 0.0;
 
-
+volatile double Signal = 0.0;
 const int SampleFrequency = 100;
 const double AnglePerPulse = 2*pi/4200; //Soms doet dit een beetje vaag, dus dit moet wel af en toe uitgetest worden
 volatile double Position_L = 0.0; //The position of the left motor in radiants
@@ -48,25 +45,19 @@
 QEI Encoder_R (D13, D12,NC, 64); //D 13 is poort A 12 is poort b
 
 //--------------------AnalogInput---------------------------------
-AnalogIn EMG_L(A0); //Motorspeed control Left
-AnalogIn EMG_R(A1); //MotorSpeed control Right
-AnalogIn EMG_Change(A2);//EMG signal for other controls
+AnalogIn Left(A0); //Motorspeed control Left
+AnalogIn Right(A1); //MotorSpeed control Right
+AnalogIn Change(A2);//EMG signal for other controls
+double EMG_L;
+double EMG_R;
+double EMG_Change; 
 //-------------------Hidscope----------------------------------
 HIDScope scope(4); // Sending two sets of data
 
-//-------------------Interrupts-------------------------------
-InterruptIn Dir_L(D0); //Motor Dirrection left
-InterruptIn Dir_R(D1); //Motor Dirrection Right
-InterruptIn OnOff(SW3); //System On/off
-
-
-//------------------Tickers------------------------------------
-
-Ticker Tick_Sample;// ticker for data sampling
-
 //--------------------------------------------------------------
 //--------------------All Motors--------------------------------
 //--------------------------------------------------------------
+volatile int movement = 0,direction_L =0, direction_R =0; //determining the direction of the motors
 
 DigitalOut M_L_D(D4); //Richting motor links-> 
 //while M_L_D is zero the motor's direction is counterclockwise and the pulses are positive
@@ -89,7 +80,7 @@
 
 //-----------------------on of sitch of the sytem-----------------------------
 
-
+InterruptIn OnOff(SW3); //System On/off
 volatile bool Active = false;
 void Start_Stop(){
     Active = !Active;
@@ -118,6 +109,9 @@
     
     
 //--------------------------------Sampling------------------------------------------   
+
+Ticker Tick_Sample;// ticker for data sampling
+
 bool Sample_Flag = false;
 
 void Set_Sample_Flag(){
@@ -138,12 +132,7 @@
 //Speed in RadPers second
     Speed_L = (Position_L-Previous_Position_L)*SampleFrequency;
     Speed_R = (Position_R-Previous_Position_R)*SampleFrequency;
-               scope.set(0,Speed_L);
-               scope.set(1,Signal+Speed_L);
-               scope.set(2,Speed_R);
-               scope.set(3,Signal+Speed_R);
 
-               scope.send();
                Encoder_L.reset();
                Encoder_R.reset();
                
@@ -156,33 +145,19 @@
     void Set_controller_Flag(){
         Controller_Flag = true;
         }
+
+
     
-   //---------------------------Defining the biquad filter function for controller----------------
-double biquad( double u, double &v1, double &v2, const double a1, const double a2,
- const double b0, const double b1, const double b2 ){
- double v = u - (a1*v1) -(a2*v2);
- double y = b0*v + b1*v1 + b2*v2;
- v2 = v1; v1 = v;
- return y;
- }
-   
+//-------------------------------PI ---------------------------------
     
-//-------------------------------PID ---------------------------------
-    
-double PID(double e, const double Kp, const double Ki, const double Kd, double Sf,
- double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1,
- const double f_a2, const double f_b0, const double f_b1, const double f_b2){
+double PI(double e, const double Kp, const double Ki, double Sf,
+ double &e_int){
     //This function is a PID controller that returns the errorsignal for the plant
 
 
-     // Derivative Part with filtering
-    double e_der = (e - e_prev)/Sf;
-  //  e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 );
-    e_prev = e;
-    // Integral Part
     e_int = e_int + Sf * e;
     // PID
-    return Kp*e + Ki*e_int + Kd * e_der;
+    return Kp*e + Ki*e_int ;
     }
  
  //---------------------------Controller---------------------------------------  
@@ -190,12 +165,9 @@
  
 
  // Controller gains (motor1−Kp,−Ki,...)
- const double Kpm= 1, Kim = 0*0.02, Kdm = 0*0.041465;
- double er_int = 0, prev_er = 0;
- // Derivative filter coeicients (motor1−filter−b0,−b1,...)
- const double F_A1 = 1.0, F_A2 = 2.0, F_B0 = 1.0, F_B1 = 3.0, F_B2 = 4.0;
- // Filter variables (motor1−filter−v1,−v2)
- double f_v1 = 0, f_v2 = 0; 
+ const double Kpm= 0.1, Kim = 0.05;
+ double er_int = 0;
+ 
     double Controller_L(int direction, double signal, double reference ){
         //This funcition returns a value that will be sent to drive the motor
         // - Motor_Direction_Input is the Digital in of the motor
@@ -204,8 +176,7 @@
         // and -1 for direction means the opposite
         
         //PID controller
-       double Speed_Set = PID( reference-signal*direction,Kpm, Kim, Kdm, SampleFrequency, er_int,
- prev_er, f_v1, f_v2, F_A1, F_A2, F_B0, F_B1,F_B2 );
+       double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, er_int );
     //Determining rotational direction of the motor
             if ((reference-signal*direction >= 0)){
                  M_L_D = 1;
@@ -222,9 +193,8 @@
         // 1 for dirrection means that a clockwise rotation wil be regarded as positive
         // and -1 for direction means the opposite
         
-        //PID controller
-       double Speed_Set = PID( reference-signal*direction,Kpm, Kim, Kdm, SampleFrequency, er_int,
- prev_er, f_v1, f_v2, F_A1, F_A2, F_B0, F_B1,F_B2 );
+        //PI controller
+       double Speed_Set = PI( reference-signal*direction,Kpm, Kim,  SampleFrequency, er_int);
     //Determining rotational direction of the motor
             if ((reference-signal*direction >= 0)){
                  M_R_D = 0;
@@ -232,7 +202,11 @@
                  M_R_D = 1;
              }
             if (fabs(Speed_Set)< 0.4){
+                if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
+                    return  0;
+                    } else {
             return fabs(Speed_Set);
+            }
             } else {
                 return 0.4;
             
@@ -280,11 +254,30 @@
         Movement_Flag = false;
         }
     }
+    //-----------------------------EMG Signal determinator----------------------
+BiQuadChain bqc_L;
+BiQuadChain bqc_R;
+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 );
+
+BiQuad bq1_L( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
+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){
+ 
+  
+    
+    return EMG_Out;
+}
 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);
@@ -294,7 +287,45 @@
     while (true) { 
         if (Sample_Flag == true){
             Sample();
-            Signal = pi*(EMG_R.read()-EMG_L.read());
+            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;
+            Signal = pi*(EMG_R-EMG_L);
+        
+
+                         scope.send();
             Sample_Flag = false;
             }
     //Main statement that states if the system is active or not