Diff: main.cpp

Revision:

4:2786719c73fd

Parent:

3:03db694efdbe

Child:

5:77ffa336d6eb

diff -r 03db694efdbe -r 2786719c73fd main.cpp
--- a/main.cpp	Thu Oct 26 21:07:45 2017 +0000
+++ b/main.cpp	Thu Oct 26 22:02:24 2017 +0000
@@ -1,9 +1,9 @@
 #include "mbed.h"
 #include "QEI.h"
+#include "BiQuad.h"
 #include "MODSERIAL.h"
 //#include "HIDScope.h"
 
-//there is something wrong with the code, the motor turns twice when the potmeters are at the minimum
 Serial pc (USBTX,USBRX);
 
 PwmOut Motor1Vel(D6);            //motor 1 velocity control
@@ -13,32 +13,83 @@
 DigitalOut led1(D2);
 AnalogIn emg1(A0);
 AnalogIn emg2(A1);
+AnalogIn emg3(A3);
 DigitalOut Button(D3);
 //DigitalOut ledb(LED1);
 
-QEI encoder1(D8,D9,NC,16);
+QEI encoder1(D8,D9,NC,16);      //define encoder pins
 QEI encoder2(D10,D11,NC,16);
 
-Ticker switch_tick;
+BiQuad bq1 ( 0.9150 , -1.8299 , 0.9150 , 1.0000 , -1.8227 , 0.8372); //Highpass
+BiQuad bq2 ( 0.0001 ,   0.0002  ,  0.0001  ,  1.0000 ,  -1.9733  ,  0.9737); //Lowpass
+
+Ticker motors_tick;
+Ticker emg_tick;
+
 float Angle1,Angle2;              //real angles of the motor
 float XPos, YPos;                 //position of the end-effector
 float XSet, YSet;                 //desired position of the end-effector
 float L = 300.0;                  //length of the arms of the robot
 const float Ts = 0.01;            //to control the tickers
-const float X0 = 14.5, X1 = 21.85, X2 = 31.94, slope = -17.34/6.6;  //values of special points in the workspace
 const float KV = 1;               //velocity constant, to scale the desired velocity of the end-effector
-float X,Y;                        //emg input for the motors
 
-const float pi = 3.14159265359,PulsesPerRev = 16.0,GearRatio = 131.15;   //to convert pulses to radians 
+volatile double y=0;
+const float pi = 3.14159265359,PulsesPerRev = 16.0,GearRatio = 131.15*3;   //to convert pulses to radians 
 const float Angle1_0 = pi/2 ,Angle2_0 = 0;    //initial angle of the motors
 
 //variables and constants for the PID
 const float KP = 10, KI = 5, KD = 2, N = 100;
 float M1_v1 = 0, M1_v2 = 0, M2_v1 = 0, M2_v2 = 0;
 
+volatile double a = 0, b = 0, c = 0;      //variables to store the highest emg signal to normalize the data
+double Y1, Y2;                // Y1 and Y2 are taken to conform Y to be used to change the set position X is also used to change the set position
+volatile double Y, X;          
+
 volatile bool ReturnIP;
 int counter;                      //to count the time of inactivity
 
+double emg1filt(double X)
+{
+    double xtemp=(bq2.step(fabs(bq1.step(emg1))));
+    if (xtemp>a)
+        a=xtemp;
+         
+    X = xtemp/a;
+    return X;
+}
+
+double emg2filt(double Y1)
+{
+    double y1temp = (bq2.step(fabs(bq1.step(emg1))));
+    if (y1temp>b)
+        b=y1temp;
+        
+    Y1 = y1temp/b;
+    return Y1;
+}
+
+int emg3filt(int Y2)
+{
+    double y2temp = (bq2.step(fabs(bq1.step(emg1))));
+    if (y2temp>c)
+    {
+        c=y2temp;
+    }    
+    Y2 = y2temp/c;
+    if(Y1 < 0.3)
+        Y2 = 1;
+    else
+        Y2 = -1;
+    return Y2;
+}
+
+void callemgfilt()
+{
+    X = emg1filt(X);
+    Y = emg2filt(Y1)*emg3filt(Y2);  // emg2filt gives the amplitude of Y and emg3filt the sign
+
+}
+
 // PID controller (Tustin approximation)
 float PID(float err, const float KP, const float KI, const float KD,const float Ts, const float N, float &v1, float &v2)
 {
@@ -97,9 +148,11 @@
     positionxy(Angle1,Angle2,XPos,YPos);
     MoveMotors();    
 }
+
 void InitialPosition()  //Go back to the initial position
 {  
-    const float  AllowedError = 1;
+    const float  AllowedError = 0.5;  
+    const float X0 = 14.5, X1 = 21.85, X2 = 31.94, slope = -17.34/6.6;  //values of special points in the workspace
     
     positionxy(Angle1,Angle2,XPos,YPos);
     if(sqrt(pow(XPos-XSet,2)+pow(YPos-YSet,2))<AllowedError)  //to call the function until the end effector is at the initial position
@@ -133,6 +186,7 @@
     }
     MoveMotors();
 }
+
 void CallFuncMovement()   //decide which case has to be used
 {                       
     positionxy(Angle1,Angle2,XPos,YPos);     //calculate the position of the end point
@@ -167,12 +221,11 @@
     } 
 }
 
-
 main()
 {
     pc.baud(115200);
-    switch_tick.attach(&CallFuncMovement,Ts);
+    emg_tick.attach(&callemgfilt,0.001);
+    motors_tick.attach(&CallFuncMovement,Ts);
     while (true)
-    {
-    }
+    { /*keep the program running*/ }
 }
\ No newline at end of file