Diff: main.cpp

Revision:

1:99754fe781b0

Parent:

0:d5fb29bc0847

Child:

2:bc01b1ce8fb6

--- a/main.cpp	Mon Oct 23 10:04:37 2017 +0000
+++ b/main.cpp	Mon Oct 23 10:34:30 2017 +0000
@@ -43,10 +43,10 @@
 const float Ts = 0.1;                   // tickettijd/ sample time
 float e_prev = 0; 
 float e_int = 0;
-
+float refP = 0;
 
 //FILTERS
-void emgverwerk ()
+/*void emgverwerk ()
 {
     double emgNotch = NF.step(emg.read() );  // Notch filter
     double emgHP = HPF.step(emgNotch);       // High-pass filter: also normalises around 0.
@@ -62,15 +62,26 @@
     scope.set(1,emg.read());
     scope.send();    
 }    
+*/
 
 
 // PID CONTROLLER
-float GetReferencePosition() 
+float GetReferencePosition(double emgFilt, float refP )  //    anders met emg
 {
-    float Potmeterwaarde = potMeter2.read();
-    int maxwaarde = 4096;                   // = 64x64
-    float refP = Potmeterwaarde*maxwaarde;
-    return refP;                            // value between 0 and 4096 
+    if (.45<emgFilt<.80)
+    {
+        refP += 1;
+        }
+    else if (emgFilt>=.80)
+    {
+        refP += 4;
+    }
+    else 
+    {  }
+    
+    int maxwaarde = 4096;                   // = 64x64 aantal posities die de motor kan hebben
+    float refPos = refP*maxwaarde;
+    return refPos;                            // value between 0 and 4096 
 }
     
 float FeedBackControl(float error, float &e_prev, float &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
@@ -121,8 +132,23 @@
 
 void MeasureAndControl(void)
 {
+    //emgverwerken
+    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)
+    {
+        emgFiltered=1.00;
+    }
+    scope.set(0,emgFiltered);
+    scope.set(1,emg.read());
+    scope.send();    
+    
     // hier the control of the control system
-    float refP = GetReferencePosition(); 
+    float refPos = GetReferencePosition(emgFiltered, refP); 
     float Huidigepositie = Encoder(); 
     float error = (refP - Huidigepositie);// make an error
     float motorValue = FeedBackControl(error, e_prev, e_int);
@@ -135,10 +161,10 @@
     NF.add( &N1 );
     HPF.add( &HP1 ).add( &HP2 );
     LPF.add( &LP1 ).add( &LP2 );
-    emgverwerkticker.attach(&emgverwerk,dt);
+    //emgverwerkticker.attach(&emgverwerk, dt);
     
     M1E.period(PwmPeriod);
-    Treecko.attach(MeasureAndControl, Ts);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
+    Treecko.attach(MeasureAndControl, dt); //Ts);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
                                             //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
      
     while(1) 
@@ -146,8 +172,8 @@
         wait(0.2);
         pc.baud(115200);
         float B = motor1.getPosition();
-        float Potmeterwaarde = potMeter2.read();
+        //float Potmeterwaarde = potMeter2.read();
         //float positie = B%4096;
-        pc.printf("pos: %d, speed %f, potmeter = %f V, \r\n",motor1.getPosition(), motor1.getSpeed(),(potMeter2.read()*3.3)); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V
+        //pc.printf("pos: %d, speed %f, potmeter = %f V, \r\n",motor1.getPosition(), motor1.getSpeed(),(potMeter2.read()*3.3)); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V
     }
 }