Diff: main.cpp

Revision:

3:36e706d6b3d2

Parent:

2:bc01b1ce8fb6

Child:

4:5607088ef6f5

--- a/main.cpp	Mon Oct 23 11:18:57 2017 +0000
+++ b/main.cpp	Mon Oct 23 15:26:44 2017 +0000
@@ -5,12 +5,24 @@
 #include "encoder.h"
 #include "MODSERIAL.h"
 
+//globalvariables Motor
+Ticker Treecko;             //We make a awesome ticker for our control system
+PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
+DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
 
-//globale variabelen FILTERS
+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)
+const double Ts = 0.1;                   // tickettijd/ sample time
+double e_prev = 0; 
+double e_int = 0;
+
+//globalvariables filter
 
 //Hidscope aanmaken
 HIDScope scope(2);
-double maxi = 0.12;                     // max signal after filtering, 0.1-0.12
+double maxi = 0.12;                                           // 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 );
@@ -22,31 +34,11 @@
 BiQuad LP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
 BiQuadChain LPF;
 
-float f = 500;       // frequency
-float dt = 1/f;      // sample frequency
-Ticker emgverwerkticker;
+double f = 500;       // frequency
+double dt = 1/f;      // sample frequency
 AnalogIn emg(A0);   // EMG lezen
 
-// globale variabelen PID controller
-
-Ticker AInTicker;           //We make a ticker named AIn (use for HIDScope)
-
-Ticker Treecko;             //We make a awesome ticker for our control system
-//AnalogIn potMeter2(A1);     //Analoge input of potmeter 2 (will be use for te reference position) --> emgFiltered
-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);
-
-float PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
-const float Ts = 0.1;                   // tickettijd/ sample time
-float e_prev = 0; 
-float e_int = 0;
-float refP_prev = 0;
-
-//FILTERS
-/*void emgverwerk ()
+double GetReferencePosition() 
 {
     double emgNotch = NF.step(emg.read() );  // Notch filter
     double emgHP = HPF.step(emgNotch);       // High-pass filter: also normalises around 0.
@@ -61,50 +53,37 @@
     scope.set(0,emgFiltered);
     scope.set(1,emg.read());
     scope.send();    
-}    
-*/
-
-
-// PID CONTROLLER
-float GetReferencePosition(double emgFilt, double &refP_prev )  //    anders met emg
-{
-    double refP; 
-    if (.45<emgFilt<.80)
-    {
-        refP= refP_prev + 0.001;
-        }
-    else if (emgFilt>=.80)
-    {
-        refP= refP_prev + 0.04;
-    }
-    else 
-    {  }
-    
-    int maxwaarde = 4096;                   // = 64x64 aantal posities die de motor kan hebben
-    refP = refP*maxwaarde;
+    //pcbaud moet erbij
+    printf("emgFiltered = %d", emgFiltered);
+    int maxwaarde = 4096;                   // = 64x64
+    double refP = emgFiltered*maxwaarde;
     return refP;                            // 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)
+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)
 {
-    float kp = 0.001;                             // kind of scaled.
-    float Proportional= kp*error;
+    double kp = 0.001;                           // has jet to be scaled
+    double Proportional= kp*error;
     
-    float kd = 0.0004;                           // kind of scaled. 
-    float VelocityError = (error - e_prev)/Ts; 
-    float Derivative = kd*VelocityError;
+    double kd = 0.0004;                            // has jet to be scaled
+    double VelocityError = (error - e_prev)/Ts; 
+    double Derivative = kd*VelocityError;
     e_prev = error;
     
-    float ki = 0.00005;                           // kind of scaled.
+    double ki = 0.00005;                           // has jet to be scaled 
     e_int = e_int+Ts*error;
-    float Integrator = ki*e_int;
+    double Integrator = ki*e_int;
     
     
-    float motorValue = Proportional + Integrator + Derivative;
+    double motorValue = Proportional + Integrator + Derivative;
     return motorValue;
 }
 
-void SetMotor1(float motorValue)
+void SetMotor1(double motorValue)
 {
     if (motorValue >= 0)
     {
@@ -124,35 +103,13 @@
         M1E = fabs(motorValue);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
     }
 }
-
-float Encoder ()
-{
-    float Huidigepositie = motor1.getPosition ();
-    return Huidigepositie;             // huidige positie = current position
-}
-
-void MeasureAndControl(void)
+void MeasureAndControl ()
 {
-    //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(emgFiltered, refP_prev); 
-    float Huidigepositie = Encoder(); 
-    float error = (refP - Huidigepositie);// make an error
-    float motorValue = FeedBackControl(error, e_prev, e_int);
+        // hier the control of the control system
+    double refP = GetReferencePosition(); 
+    double Huidigepositie = Encoder(); 
+    double error = (refP - Huidigepositie);// make an error
+    double motorValue = FeedBackControl(error, e_prev, e_int);
     SetMotor1(motorValue);
 }
 
@@ -162,19 +119,15 @@
     NF.add( &N1 );
     HPF.add( &HP1 ).add( &HP2 );
     LPF.add( &LP1 ).add( &LP2 );
-    //emgverwerkticker.attach(&emgverwerk, dt);
+    
+    Treecko.attach(MeasureAndControl, 0.1);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
+                                            //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
     
-    M1E.period(PwmPeriod);
-    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) 
+    //emgverwerkticker.attach(&emgverwerk,dt);
+    //HIDinticker.attach(&ReadFilteredSignal(emgFiltered), 0.01);
+    while(true)
     {
-        wait(0.2);
-        pc.baud(115200);
-        float B = motor1.getPosition();
-        //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
-    }
-}
+        }
+    
+    
+}
\ No newline at end of file