Diff: main.cpp

Revision:

7:05c71a859d27

Parent:

6:e0e5da2c068f

Child:

8:c4ec359af35d

--- a/main.cpp	Mon Oct 30 16:09:46 2017 +0000
+++ b/main.cpp	Tue Oct 31 15:42:05 2017 +0000
@@ -10,7 +10,6 @@
 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);
 
@@ -19,12 +18,16 @@
 double e_prev = 0; 
 double e_int = 0;
 double tijdstap = 0.002;
-
-//globalvariables filter
+volatile double emgFiltered;
 
-//Hidscope aanmaken
-HIDScope scope(2);
-double maxi = 0.30;            //is voor iedereen verschillend --> moet calibreren                               // max signal after filtering, 0.1-0.12
+//buttons  en leds voor calibration
+DigitalIn button1(PTA4);
+DigitalOut ledred(LED_RED);
+DigitalOut ledblue(LED_BLUE);
+double maxi = 0;
+bool caldone = false; 
+int CalibrationSample = 5000; //How long will we calibrate? Timersampletime*Calibrationsample
+int Timescalibration = 0;
 
 // 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 );
@@ -35,38 +38,80 @@
 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;
+
+Timer looptime; //moetuiteindelijk weg
 
-//hierdoubles neerzetten
+//filters
+double emgNotch;
+double emgHP;
+double emgAbsHP;
+double emgLP;
+double emgMax; //magdezeweg??
+
+
 
 double f = 500;       // frequency
 double dt = 1/f;      // sample frequency
 AnalogIn emg(A0);   // EMG lezen
 
-void GetReferencePosition() 
+double Filteren() 
 {
     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
+    emgNotch = NF.step(emg.read() );  // Notch filter
+    emgHP = HPF.step(emgNotch);       // High-pass filter: also normalises around 0.
+    emgAbsHP = abs(emgHP);            // Take absolute value
+    emgLP = LPF.step(emgAbsHP);       // Low-pass filter: creates envelope
+    emgFiltered = emgLP/maxi;       // Scale to maximum signal: useful for motor
+   
     /*if (emgFiltered >1)
     {
         emgFiltered=1.00;
     } */
-    
-    //scope.set(1,emgFiltered);
-    //scope.set(0,emg.read());
-    //scope.send();    
-    pc.baud(115200);
-    printf("emgread = %f , emgFiltered = %f, loop = %f \r\n",emg.read(), emgFiltered, looptime.read());
+pc.printf("emgread = %f , emgFiltered = %f, loop = %f, maxi = %f \r\n",emg.read(), emgFiltered, looptime.read(),maxi);
     //int maxwaarde = 4096;                   // = 64x64
     //double refP = emgFiltered*maxwaarde;
     //return refP;                            // value between 0 and 4096 
+     return emgFiltered;
 }
+
+void Calibration()
+{
+    Timescalibration++;
+    emgNotch = NF.step(emg.read());             // Notch filter
+    emgHP = HPF.step(emgNotch);                 // High-pass filter: also normalises around 0.
+    emgAbsHP = abs(emgHP);                      // Take absolute value
+    emgLP = LPF.step(emgAbsHP);                 // Low-pass filter: creates envelope
+    double emgfinal = emgLP;
+    if (emgfinal > maxi)
+    {                                       //determine what the highest reachable emg signal is
+          maxi = emgfinal;
+    }    
+    if(Timescalibration>5000)
+    {
+         caldone=true; 
+    }
+   // pc.printf("caldone = %i , Timescalibration = %i maxi = %f \r\n",caldone,Timescalibration,maxi);
+        
+        //for(int n =0; n<CalibrationSample;n++)          //read for 5000 samples as calibration
+        //{                          
+         //   emgNotch = NF.step(emg.read());             // Notch filter
+           // emgHP = HPF.step(emgNotch);                 // High-pass filter: also normalises around 0.
+            //emgAbsHP = abs(emgHP);                      // Take absolute value
+            //emgLP = LPF.step(emgAbsHP);                 // Low-pass filter: creates envelope
+            //double emgfinal = emgLP;
+            //if (emgfinal > maxi)
+              //  {                                       //determine what the highest reachable emg signal is
+               // maxi = emgfinal;
+                //}
+           // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
+        //}
+                                         //PAS ALS DEZE TRUE IS, MOET DE MOTOR PAS BEWEGEN!!!
+  //return maxi;
+}
+        
+        
+
 /*
 double Encoder ()
 {
@@ -113,32 +158,51 @@
         M1E = fabs(motorValue);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
     }
 }
+*/
 void MeasureAndControl ()
 {
         // hier the control of the control system
-    double refP = GetReferencePosition(); 
+     
+    if(caldone==false)
+    {
+        if(button1.read()==false)  
+        {
+            Calibration();
+        }
+    }
+    if (caldone==true)
+    
+    {
+        double EMGfilter = Filteren();
+        //rest
+    }
+    
     //double Huidigepositie = Encoder(); 
     //double error = (refP - Huidigepositie);// make an error
     //double motorValue = FeedBackControl(error, e_prev, e_int);
-    double motorValue = refP;
-    SetMotor1(motorValue);
+    //double motorValue = refP;
+    //SetMotor1(motorValue);
 }
 
-*/
+
 int main()
 {
+    //voor EMG filteren
     NF.add( &N1 );
     HPF.add( &HP1 ).add( &HP2 );
     LPF.add( &LP1 ).add( &LP2 );
     
-    Treecko.attach(GetReferencePosition, tijdstap);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
+    //voor serial
+    pc.baud(115200);
+    
+    //motor
+    M1E.period(PwmPeriod); //set PWMposition at 5000hz
+    //Ticker
+    Treecko.attach(MeasureAndControl, 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(..)
         }