Diff: main.cpp

Revision:

5:b9d5d7311dac

Parent:

4:b4530fb376dd

Child:

6:1597888c9a56

--- a/main.cpp	Wed Oct 21 11:50:15 2015 +0000
+++ b/main.cpp	Thu Oct 22 13:11:55 2015 +0000
@@ -9,11 +9,17 @@
 PwmOut      led(D9);
 DigitalIn   button_1(PTC6); //counterclockwise
 DigitalIn   button_2(PTA4); //clockwise
+AnalogIn    PotMeter1(A5);
+AnalogIn    EMG(A0);
+//AnalogIn    EMG_bicepsright (A1);
+//AnalogIn    EMG_legleft (A2);
+//AnalogIn    EMG_legright (A3);
+Ticker      controller;
+Ticker      ticker_regelaar;
+Ticker      EMG_Control;
+//Timer       Timer_Calibration;
 Encoder     motor1(D12,D13);
-HIDScope    scope(1);
-AnalogIn PotMeter1(A1);
-Ticker controller;
-Ticker ticker_regelaar;
+HIDScope    scope(3);
 
 MODSERIAL   pc(USBTX, USBRX);
 volatile bool regelaar_ticker_flag;
@@ -26,7 +32,7 @@
 #define SAMPLETIME_REGELAAR 0.005
 
 //define states
-enum state {HOME, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP};
+enum state {HOME, CALIBRATIE, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP};
 uint8_t state = HOME;
 
 // Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering
@@ -61,6 +67,78 @@
     return motor1_Kp*error + motor1_Ki*err_int;
 } // De totale fout die wordt hersteld met behulp van PI control.
 
+//bool Cali = false;
+//double TimeCali = 5;
+
+// Filter1 = High pass filter tot 20 Hz
+double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0;
+const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
+const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
+// Filter2 = Low pass filter na 60 Hz
+double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0;
+const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
+const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
+// Filter3 = Notch filter at 50 Hz
+double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0;
+const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1;
+const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1;
+const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1;
+
+// Filter4 = Lowpass filter at 5 Hz
+double flp1_v1=0, flp1_v2=0, flp2_v1=0, flp2_v2=0;
+const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
+const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
+
+double y1, y2, y3, y4, y5, y6, y7, y8, y9, u1, u2, u3, u4, u5, u6, u7, u8, u9;
+double final_filter1;
+
+// Standaard formule voor het biquad filter
+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;
+}
+
+void Filters()
+{
+    u1 = EMG.read();
+    //Highpass
+    y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
+    u2 = y1;
+    y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
+    //Lowpass
+    u3 = y2;
+    y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
+    u4 = y3;
+    y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
+    // Notch
+    u5 = y4;
+    y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
+    u6 = y5;
+    y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
+    u7 = y6;
+    y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
+    y8 = fabs (y7);
+    //smooth
+    u8 = y8;
+    y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
+    u9 = y9;
+    final_filter1 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
+    
+    scope.set (0, y8);
+   // scope.set (1, y2);
+   // scope.set (2, y4);
+   // scope.set (3, y7);
+   scope.set (1, final_filter1);
+   //scope.set (2, final_filter1); 
+    scope.send ();
+    } 
+ 
+
 void motor1_controlPI()
 {
     double referencePI1 = PotMeter1.read();
@@ -107,22 +185,27 @@
 
 void move_motor1()
 {
-    if (button_1 == pressed) {
-        move_motor1_cw ();
-    } else if (button_2 == pressed) {
-        move_motor1_ccw ();
-    } else { 
-        motor1_speed = 0;
-    }
+        if (final_filter1 > 0.03){
+            pc.printf("Moving clockwise \n\r");
+            move_motor1_cw ();
+            }
+        else if (button_2 == pressed){
+            pc.printf("Moving counterclockwise \n\r");
+            move_motor1_ccw ();
+            }
+        else {
+            pc.printf("Not moving \n\r"); 
+            motor1_speed = 0;
+            }
 }
 
-void read_encoder1 ()    // aflezen van encoder via hidscope??
-{
-    scope.set(0,motor1.getPosition());
-    led.write(motor1.getPosition()/100.0);
-    scope.send();
-    wait(0.2f);
-}
+//void read_encoder1 ()    // aflezen van encoder via hidscope??
+//{
+//    scope.set(0,motor1.getPosition());
+    //led.write(motor1.getPosition()/100.0);
+//    scope.send();
+//    wait(0.2f);
+//}
 
 void print_position(){
     pc.printf("move motor \n\r");
@@ -132,20 +215,41 @@
 {    
     while (true) {
         pc.baud(9600);          //pc baud rate van de computer
+        EMG_Control.attach_us(Filters,1e3);
         
     switch (state) {                //zorgt voor het in goede volgorde uitvoeren van de cases
         
         case HOME:      //positie op 0 zetten voor arm 1
          {
             pc.printf("home\n\r");
-            read_encoder1();
+            //read_encoder1();
             gethome();
             pc.printf("Home-position is %f\n\r", H);
-            state = MOVE_MOTOR;
+            state = CALIBRATIE;
             wait(5);
             break;
         }
         
+        //case CALIBRATIE:
+        //{        
+            //pc.printf("Aanspannen in 10 \n\r");
+            //wait(10);
+            //EMG_Control.attach_us(MyController,1e3);
+            //Timer_Calibration.start();
+            //if (Timer_Calibration < TimeCali) {
+            //    Cali = true;
+            //    pc.printf("Aanspannen \n\r");
+            //} 
+            //else {
+            //    Cali = false;
+            //}  
+            //pc.printf("Stoppen \n\r"); 
+            //Timer_Calibration.stop();
+            //Timer_Calibration.reset();
+            //state = MOVE_MOTOR;
+          //  break;
+        //}
+        
         case MOVE_MOTOR:            //motor kan cw en ccw bewegen a.d.h.v. buttons
         {
             pc.printf("move_motor\n\r");
@@ -166,7 +270,8 @@
             wait (1);
             
             ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
-            
+            //EMG_Control.attach_us(Filters,1e3);
+
             while(state == BACKTOHOMEPOSITION){
                 movetohome();
                 while(regelaar_ticker_flag != true);