Diff: main.cpp

Revision:

1:cfcd4825fe81

Parent:

0:6f71e969a0b5

Child:

2:1cd28768a9bf

diff -r 6f71e969a0b5 -r cfcd4825fe81 main.cpp
--- a/main.cpp	Wed Oct 28 08:11:57 2015 +0000
+++ b/main.cpp	Wed Oct 28 11:22:08 2015 +0000
@@ -6,10 +6,12 @@
 #include <iostream>
 #include "QEI.h"
 
-
-
+//DigitalOut led_green(LED_GREEN);
+//DigitalOut led_red(LED_RED);
 
 Serial pc(USBTX, USBRX);
+AnalogIn analog_emg_left(A0);
+AnalogIn analog_emg_right(A1);
 
 Ticker HIDScope_timer;
 Ticker Filteren_timer;
@@ -34,8 +36,7 @@
     Flag_HIDScope  = true;
 }
 
-AnalogIn analog_emg_left(A0);
-AnalogIn analog_emg_right(A1);
+
 
 double input_left = 0 ;
 double input_right = 0 ;
@@ -44,11 +45,12 @@
 double filter_signal_hid_right = 0;
 //double input_right = 0;
 // defining threshold
-  double high_threshold = 1200;
-    double low_threshold = 500;
+double high_threshold = 1400;
+double low_threshold = 700;
 
 //*** making the v for everything and conquer the world***
 
+//for left
 //for Notchfilter
 double notch_v11=0;
 double notch_v21=0;
@@ -80,6 +82,39 @@
 double n4 = 0;
 double n5 = 0;
 
+// for right
+//for Notchfilter
+double notch_v11_b=0;
+double notch_v21_b=0;
+double notch_v12_b=0;
+double notch_v22_b=0;
+double notch_v13_b=0;
+double notch_v23_b=0;
+
+//for highpass filter
+double high_v11_b=0;
+double high_v21_b=0;
+double high_v12_b=0;
+double high_v22_b=0;
+double high_v13_b=0;
+double high_v23_b=0;
+
+// for lowpasfilter
+double low_v11_b=0;
+double low_v21_b=0;
+double low_v12_b=0;
+double low_v22_b=0;
+double low_v13_b=0;
+double low_v23_b=0;
+
+// for moving average
+double n1_b = 0;
+double n2_b = 0;
+double n3_b = 0;
+double n4_b = 0;
+double n5_b = 0;
+
+
 double filter_left;
 double filter_right;
 
@@ -188,7 +223,7 @@
 
 
 
-double Filteren(double input)
+double Filteren_left(double input)
 {
 
     input = input-0.45; //FIRST SUBTRACT MEAN THEN FILTER
@@ -214,25 +249,62 @@
 
     // moving average
     double filter_signal = moving_average(y10,n1,n2,n3,n4,n5);
-  
+
     return(filter_signal);
 }
 
+double Filteren_right(double input_b)
+{
+
+    input_b = input_b-0.45; //FIRST SUBTRACT MEAN THEN FILTER
+    //input_right = analog_emg_right.read();
+
+    // notch filter
+    double y1_b = biquad(input_b, notch_v11_b, notch_v21_b, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2);
+    double y2_b = biquad(y1_b, notch_v12_b, notch_v22_b, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2);
+    double y3_b = biquad(y2_b, notch_v13_b, notch_v23_b, notch3_a1, notch3_a2, notch3_b0, notch3_b1, notch3_b2);
+
+    //higpass filter
+    double y4_b = biquad(y3_b, high_v11_b, high_v21_b, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2);
+    double y5_b = biquad(y4_b, high_v12_b, high_v22_b, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2);
+    double y6_b = biquad(y5_b, high_v13_b, high_v23_b, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2);
+
+    //rectivier
+    double y7_b = fabs(y6_b);
+
+    //lowpas filter cascade
+    double y8_b = biquad(y7_b, low_v11_b, low_v21_b, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
+    double y9_b = biquad(y8_b, low_v12_b, low_v22_b, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
+    double y10_b= biquad(y9_b, low_v13_b, low_v23_b, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2);
+
+    // moving average
+    double filter_signal_b = moving_average(y10_b,n1_b,n2_b,n3_b,n4_b,n5_b);
+
+    return(filter_signal_b);
+}
+
 /*************************************************************BEGIN motor control******************************************************************************************************/
 // Define pin for motor control
 DigitalOut directionPin(D4);
 PwmOut PWM(D5);
+DigitalOut directionPin_key(D6);
+PwmOut PWM_key(D7);
 DigitalIn buttonccw(PTA4);
 DigitalIn buttoncw(PTC6);
 
 QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in)
-// define ticker
+QEI wheel_key (PTD0, PTD2, NC, 624); // Pin for counting (analog in)
+
+// saying buttons are not pressed
+const int Buttoncw_pressed = 0;
+const int Buttonccw_pressed = 0;
+
 
 // define rotation direction and begin possition
 const int cw = 1;
 const int ccw = 0;
 double setpoint = 0; //setpoint is in pulses
-
+double setpoint_key_press = 0;
 
 // Controller gain proportional and intergrator
 const double motor1_Kp = 5; // more or les random number.
@@ -240,7 +312,14 @@
 const double M1_timestep = 0.01; // reason ticker works with 100 Hz.
 double motor1_error_integraal = 0; // initial value of integral error
 // Defining pulses per revolution (calculating pulses to rotations in degree.)
-const double pulses_per_revolution = 4200 ;//8400 counts is aangegeven op de motor for x4.  10 - 30 counts oveshoot. for moter 1(tape)! Motor 2 almost the same(nice)
+const double pulses_per_revolution = 3200 ;//  motor gear is 1:1000
+
+const double motor1_Kp_key = 5; // more or les random number.
+const double motor1_Ki_key = 0;
+const double M1_timestep_key = 0.01; // reason ticker works with 100 Hz.
+double motor1_error_integraal_key = 0; // initial value of integral error
+// Defining pulses per revolution (calculating pulses to rotations in degree.)
+const double pulses_per_revolution_key = 4200 ; //8400 counts is aangegeven op de motor for x4 is 1:131.25
 /*
 double Rotation = -2; // rotation
 double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree.
@@ -279,6 +358,8 @@
     return PI_output;
 }
 // Next task, measure the error and apply the output to the plant
+
+// control for movement left right
 void motor1_Controller()
 {
     double reference = setpoint; // setpoint is in pulses
@@ -300,7 +381,34 @@
 
 
 }
+/*
+//control for the keypress
+void motor_key_Controller()
+{
+    double reference_key = setpoint_key_press; // setpoint is in pulses
+    double position_key = wheel_key.getPulses();
+    double error_pulses_key = (reference_key - position_key); // calculate the error in pulses
+    double error_rotation_key = error_pulses_key / pulses_per_revolution_key; //calculate how much the rotaions the motor has to turn
 
+    while (fabs(error_pulses_key) > 40) {
+        //kijken = wheel_two.getPulses()/pulses_per_revolution;
+
+        double output_key = abs(PI( error_rotation_key, motor1_Kp_key, motor1_Ki_key, M1_timestep_key, motor1_error_integraal_key ));
+
+        if(error_pulses_key > 0) {
+            directionPin_key.write(cw);
+
+        } else if(error_pulses_key < 0) {
+            directionPin_key.write(ccw);
+        } else {
+            output_key = 0;
+        }
+
+        PWM.write(output_key); // out of the if loop due to abs output
+
+
+    }
+}*/
 /*************************************************************END motor control******************************************************************************************************/
 
 void HIDScope_kijken()
@@ -315,14 +423,20 @@
 
 int main()
 {
+   
+
     aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning
-    HIDScope_timer.attach(&Go_flag_HIDScope, 0.02);
-    Filteren_timer.attach(&Go_flag_filteren,0.04);
+    HIDScope_timer.attach(&Go_flag_HIDScope, 0.2);//0.02 had worked
+    Filteren_timer.attach(&Go_flag_filteren,0.4);// 0.04 had worked
     while(1) {
+ pc.printf("hi here, \n");
+       // led_green = 1;
+       // led_red = 1;
+
         if(Flag_filteren) {
             Flag_filteren = false;
-           // filter left and set bool
-            filter_signal_hid_left = Filteren(analog_emg_left.read());
+            // filter left and set bool
+            filter_signal_hid_left = Filteren_left(analog_emg_left.read());
 
             if (filter_signal_hid_left > high_threshold) {
                 left_movement = true;
@@ -330,7 +444,7 @@
                 left_movement = false;
             }
             // make filter right and set bool
-            filter_signal_hid_right = Filteren(analog_emg_right.read());
+            filter_signal_hid_right = Filteren_right(analog_emg_right.read());
 
             if (filter_signal_hid_right > high_threshold) {
                 right_movement = true;
@@ -341,6 +455,8 @@
 
         }
 
+
+
         if(Flag_HIDScope) {
             Flag_HIDScope = false;
             HIDScope_kijken();
@@ -357,13 +473,28 @@
 
 // Pussing buttons to get input signal
 
-        if(left_movement) {
+        if( buttoncw.read() == Buttoncw_pressed) {
             setpoint = making_setpoint(cw);
+            //led_green = 0;
 
         }
-        if(right_movement) {
+        if(buttonccw.read() == Buttonccw_pressed ) {
             setpoint =  making_setpoint(ccw);
         }
+
+     //  if(/*right_movement and left_movement or */buttoncw.read() == Buttoncw_pressed and buttonccw.read() == Buttonccw_pressed) {
+
+/*
+
+            setpoint_key_press = 1050;
+            motor_key_Controller();
+            setpoint_key_press = 0;
+            motor_key_Controller();
+
+
+        }
+
+*/
     }
 }