Jurriën Bos
Filter werkt eindelijk, echter zijn alle kanalen hetzelfde
Dependencies: HIDScope MODSERIAL Matrix QEI biquadFilter mbed
Fork of
Turning_Motor_V3
by 
Thom Kuenen
Revision 7:c5c648898881, committed 2018-10-30
- Comitter:
- JurrienBos
- Date:
- Tue Oct 30 15:41:58 2018 +0000
- Parent:
- 6:056ad27636ff
- Commit message:
- Filter werkt, echter niet op alle kanalen.
Changed in this revision
diff -r 056ad27636ff -r c5c648898881 Matrix.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Matrix.lib Tue Oct 30 15:41:58 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/Yo_Robot/code/Matrix/#a4014ab0a8cf
diff -r 056ad27636ff -r c5c648898881 biquadFilter.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/biquadFilter.lib Tue Oct 30 15:41:58 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305
diff -r 056ad27636ff -r c5c648898881 main.cpp
--- a/main.cpp Fri Oct 26 06:56:18 2018 +0000
+++ b/main.cpp Tue Oct 30 15:41:58 2018 +0000
@@ -2,6 +2,7 @@
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
+#include "BiQuad.h"
MODSERIAL pc(USBTX, USBRX);
DigitalOut DirectionPin1(D4);
@@ -17,7 +18,7 @@
AnalogIn emg2( A2 );
AnalogIn emg3( A3 );
-QEI Encoder1(D12,D13,NC,64);
+QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
//DigitalOut LED(LED_RED);
@@ -25,7 +26,20 @@
Ticker StateTicker;
Ticker printTicker;
-HIDScope scope( 4 );
+HIDScope scope(4);
+
+BiQuadChain bqc1;
+BiQuadChain bqc2;
+BiQuadChain bqc3;
+BiQuadChain bqc4;
+BiQuadChain bqc5;
+BiQuadChain bqc6;
+BiQuadChain bqc7;
+BiQuadChain bqc8;
+BiQuad BqNotch1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
+BiQuad BqNotch2( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad BqHP( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+BiQuad BqLP( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
volatile float Bicep_Right = 0.0;
volatile float Bicep_Left = 0.0;
@@ -44,15 +58,52 @@
volatile int counts2 ;
volatile float rad_m1;
volatile float rad_m2;
+volatile float q_1;
+volatile float q_2;
+volatile float r_1;
+volatile float r_2;
+volatile const float r_3 = 2.0; //
+
+volatile float FilterHP_Bi_R;
+volatile float Filterabs_Bi_R;
+volatile float Filtered_Bi_R;
+volatile float FilterHP_Bi_L;
+volatile float Filterabs_Bi_L;
+volatile float Filtered_Bi_L;
+volatile float FilterHP_Tri_R;
+volatile float Filterabs_Tri_R;
+volatile float Filtered_Tri_R;
+volatile float FilterHP_Tri_L;
+volatile float Filterabs_Tri_L;
+volatile float Filtered_Tri_L;
+void filter()
+{
+ FilterHP_Bi_R = bqc1.step( emg0.read() );
+ Filterabs_Bi_R = fabs(FilterHP_Bi_R);
+ Filtered_Bi_R = bqc2.step( Filterabs_Bi_R );
+
+ FilterHP_Bi_L = bqc3.step( emg1.read() );
+ Filterabs_Bi_L = fabs(FilterHP_Bi_L);
+ Filtered_Bi_L = bqc4.step( Filterabs_Bi_L );
+
+ FilterHP_Tri_R = bqc5.step( emg2.read() );
+ Filterabs_Tri_R = fabs(FilterHP_Tri_R);
+ Filtered_Tri_R = bqc6.step( Filterabs_Tri_R );
+
+ FilterHP_Tri_L = bqc7.step( emg3.read() );
+ Filterabs_Tri_L = fabs(FilterHP_Tri_L);
+ Filtered_Tri_L = bqc8.step( Filterabs_Tri_L );
+}
+
void Encoding()
{
counts1 = Encoder1.getPulses();
counts2 = Encoder2.getPulses();
// Hier gaat iets fout waardoor het 0 wordt!!!
- float rad_m1 = ((2.0*pi)/32.0)* (float)counts1;
- float rad_m2 = ((2.0*pi)/32.0)* (float)counts2;
+ rad_m1 = ((2.0*pi)/32.0)* (float)counts1;
+ rad_m2 = ((2.0*pi)/32.0)* (float)counts2;
// pc.printf("%f & %f ....\n",rad_m1, rad_m2);
}
@@ -68,10 +119,10 @@
void sample()
{
- scope.set(0, emg0.read() );
- scope.set(1, emg1.read() );
- scope.set(2, emg2.read() );
- scope.set(3, emg3.read() );
+ scope.set(0, Filtered_Bi_R );
+ scope.set(1, Filtered_Bi_L );
+ scope.set(2, Filtered_Tri_R );
+ scope.set(3, Filtered_Tri_L );
scope.send();
}
@@ -142,6 +193,18 @@
pc.printf("%i %i \n",counts1,counts2);
}
+void inverse_kinematics()
+{
+ float JacPs [2][2];
+ JacPs[0][0] = 2.0;
+ JacPs[0][1] = 3.0;
+ JacPs[1][0] = 4.0;
+ JacPs[1][1] = 5.0;
+ pc.printf("%f ", JacPs[0][0]);
+}
+
+
+
void StateMachine()
{
switch (Active_State)
@@ -154,10 +217,10 @@
pc.printf("Entering Homing state \n");
Active_State = Homing;
}
-
+ filter();
sample();
- EMG_Read();
- Encoding();
+ //EMG_Read();
+ Encoding();
break;
@@ -169,9 +232,9 @@
pc.printf("Entering Funtioning State \n");
Active_State = Function;
}
-
+ filter();
sample();
- EMG_Read();
+ //EMG_Read();
Encoding();
break;
@@ -189,9 +252,9 @@
Active_State = Calibration;
}
-
+ filter();
sample();
- EMG_Read();
+ //EMG_Read();
Encoding();
velocity1();
velocity2();
@@ -209,7 +272,14 @@
{
pc.baud(115200);
PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz
-
+ bqc1.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP );
+ bqc2.add(&BqLP);
+ bqc3.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP );
+ bqc4.add(&BqLP);
+ bqc5.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP );
+ bqc6.add(&BqLP);
+ bqc7.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP );
+ bqc8.add(&BqLP);
StateTicker.attach(StateMachine, 0.002);
printTicker.attach(&Printing, 2.0);