Diff: main.cpp

Revision:

0:69faee47383f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 30 18:57:06 2017 +0000
@@ -0,0 +1,92 @@
+//libaries
+#include "mbed.h"
+#include "BiQuad.h"
+#include "HIDScope.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+
+//globalvariables Motor
+Ticker Treecko;             //We make a awesome ticker for our control system
+Ticker Gekkie;     
+PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
+DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
+
+Encoder motor1(D13,D12,true);
+MODSERIAL pc(USBTX,USBRX);
+
+double PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
+double e_prev = 0; 
+double e_int = 0;
+double tijdstap = 0.002;
+volatile double emgFiltered;
+
+
+//globalvariables filter
+double maxi = 0.30;            //is voor iedereen verschillend --> moet calibreren                               // max signal after filtering, 0.1-0.12
+
+// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
+BiQuad N1( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
+BiQuadChain NF;
+BiQuad HP1( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
+BiQuad HP2( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
+BiQuadChain HPF;
+BiQuad LP1( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
+BiQuad LP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
+BiQuadChain LPF;
+Timer looptime;
+
+//hierdoubles neerzetten
+
+double f = 500;       // frequency
+double dt = 1/f;      // sample frequency
+AnalogIn emg(A0);   // EMG lezen
+
+void GetReferencePosition() 
+{
+    looptime.reset();
+    looptime.start();
+    double emgNotch = NF.step(emg.read() );  // Notch filter
+    double emgHP = HPF.step(emgNotch);       // High-pass filter: also normalises around 0.
+    double emgAbsHP = abs(emgHP);            // Take absolute value
+    double emgLP = LPF.step(emgAbsHP);       // Low-pass filter: creates envelope
+    double emgMax = maxi;                      //(emgLP);             // moet waarde 'schatten' voor de max, want je leest de data live. voorbeeld: 0.1, maar mogelijk 0.2 kiezen voor veiligheidsfactor. Dan gaat motor alleen maximaal op 1/2 vermogen.
+    emgFiltered = emgLP/emgMax;       // Scale to maximum signal: useful for motor
+    
+
+    //pc.baud(115200);
+    //pc.printf("emgread = %f , emgFiltered = %f, loop = %f \r\n",emg.read(), emgFiltered, looptime.read());
+    
+}
+
+void Motortje ()
+{
+    double k;
+    if (emgFiltered <= .4)
+    {
+        k = 0;
+    }
+    else
+    {
+        k = 0.8;
+    }
+    
+    //M1D = !M1D;
+    M1E = k;            //Set brightness of let (0,1)
+    pc.baud(115200);
+    pc.printf("emgFiltered = %f  \r\n", emgFiltered);
+}  
+
+int main()
+{
+    NF.add( &N1 );
+    HPF.add( &HP1 ).add( &HP2 );
+    LPF.add( &LP1 ).add( &LP2 );
+    M1E.period(PwmPeriod);      //Set PWM period at 5000 Hz
+    M1D = false;               // true is met de klok mee evenals false?
+    
+    Treecko.attach(&GetReferencePosition, tijdstap);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
+                                                        //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
+    Gekkie.attach(&Motortje, tijdstap);
+
+    
+}
\ No newline at end of file