Diff: main.cpp

Revision:

6:e0e5da2c068f

Parent:

5:daa916945271

Child:

7:05c71a859d27

--- a/main.cpp	Mon Oct 30 13:33:32 2017 +0000
+++ b/main.cpp	Mon Oct 30 16:09:46 2017 +0000
@@ -10,6 +10,7 @@
 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);
 
@@ -17,12 +18,13 @@
 const double Ts = 0.1;                   // tickettijd/ sample time
 double e_prev = 0; 
 double e_int = 0;
+double tijdstap = 0.002;
 
 //globalvariables filter
 
 //Hidscope aanmaken
 HIDScope scope(2);
-double maxi = 0.12;                                           // max signal after filtering, 0.1-0.12
+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 );
@@ -33,40 +35,45 @@
 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
 
-double GetReferencePosition() 
+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.
     double emgFiltered = emgLP/emgMax;       // Scale to maximum signal: useful for motor
-    if (emgFiltered >1)
+    /*if (emgFiltered >1)
     {
         emgFiltered=1.00;
-    }
-    scope.set(0,emgFiltered);
-    scope.set(1,emg.read());
-    scope.send();    
+    } */
+    
+    //scope.set(1,emgFiltered);
+    //scope.set(0,emg.read());
+    //scope.send();    
     pc.baud(115200);
-    printf("emgread = %f , emgFiltered = %f \r\n",emg.read(), emgFiltered);
-    int maxwaarde = 4096;                   // = 64x64
-    double refP = emgFiltered*maxwaarde;
-    return refP;                            // value between 0 and 4096 
+    printf("emgread = %f , emgFiltered = %f, loop = %f \r\n",emg.read(), emgFiltered, looptime.read());
+    //int maxwaarde = 4096;                   // = 64x64
+    //double refP = emgFiltered*maxwaarde;
+    //return refP;                            // value between 0 and 4096 
 }
-
+/*
 double Encoder ()
 {
     double Huidigepositie = motor1.getPosition ();
     return Huidigepositie;             // huidige positie = current position
 }
-/*
+
 double FeedBackControl(double error, double &e_prev, double &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
 {
     double kp = 0.001;                           // has jet to be scaled
@@ -85,7 +92,7 @@
     double motorValue = Proportional + Integrator + Derivative;
     return motorValue;
 }
-*/
+
 void SetMotor1(double motorValue)
 {
     if (motorValue >= 0)
@@ -117,20 +124,21 @@
     SetMotor1(motorValue);
 }
 
-
+*/
 int main()
 {
     NF.add( &N1 );
     HPF.add( &HP1 ).add( &HP2 );
     LPF.add( &LP1 ).add( &LP2 );
     
-    Treecko.attach(MeasureAndControl, 0.1);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
+    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.
     
     //emgverwerkticker.attach(&emgverwerk,dt);
     //HIDinticker.attach(&ReadFilteredSignal(emgFiltered), 0.01);
     while(true)
     {
+       // if(..)
         }