Diff: main.cpp

Revision:

0:c727f4699e80

Child:

1:57926254e65b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 08 11:43:46 2016 +0000
@@ -0,0 +1,337 @@
+#include "mbed.h"
+#include "HIDScope.h"
+#include "biquadFilter.h"
+#include "FastPWM.h"
+
+// Define the HIDScope and Ticker object
+HIDScope    scope(5);
+Ticker      scopeTimer;
+volatile bool LoopTimerFlag;
+Ticker      ForceTimer;
+volatile bool LoopTimerFlag2;
+//Serial pc(USBTX, USBRX);
+
+// Read the analog input
+//AnalogIn    a_in(A0); // Potmeter
+AnalogIn    a_in2(A3); //OUT-
+AnalogIn    a_in3(A4); //OUT+
+
+Ticker Loopticker;                  // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
+AnalogIn Referentie(A0);            // De schuifpotmeter naast de geleiding van de draad is de referentie.
+AnalogIn Boardpotmeter(A1);        // POT1 op het board wordt gebruikt als input van de controlloop.
+AnalogIn Boardpotmeter2(A2);
+DigitalOut motor1direction(D7);     //D6 en D7 voor motor 1 (op het motorshield)
+FastPWM motor1speed(D6);
+DigitalOut Led(LED_RED);
+DigitalOut Roodisplus(D5);
+DigitalOut Versterker(D4);
+float potmeter;
+float Error;
+float Referentie2;//=1.0;
+float Output;
+float Input;
+double Error_prev = 0;
+double Error_int = 0;
+double Error_der;
+double Ref_der = 0;
+double Ref_prev = 0;
+double Ref_prev2 = 0;
+double Ref2;
+double Input3;
+double Input_der=0;
+double Input_prev=0;
+double Input_prev2=4;
+double Plaats_der;
+double Force_nieuw=0;
+double Plaats_acc;
+double Ref_der_prev;
+double Plaats_der1;
+
+//LowPass filter 2 Hz
+//double low1_v1 = 0, low1_v2 = 0;
+//double low2_v1 = 0, low2_v2 = 0;
+const double a1_LP =  -1.940778135263835, a2_LP = 0.942482022027066;
+const double b0_LP = 0.000425971690807714, b1_LP = 0.000851943381615428, b2_LP = 0.000425971690807714;
+const double a1_HP = -1.982228929792529, a2_HP = 0.982385450614125;
+const double b0_HP =  0.991153595101663, b1_HP = -1.982307190203327, b2_HP = 0.991153595101663;
+const double a1_LP5 = -1.822694925196308, a2_LP5 = 0.837181651256023 ;    //-1.911197067426073, a2_LP5 =  0.914975834801434;
+const double b0_LP5 =  0.003621681514929 , b1_LP5 =  0.007243363029857, b2_LP5 = 0.003621681514929;  // 0.000944691843840, b1_LP5 =  0.001889383687680 , b2_LP5 = 0.000944691843840;
+
+const double a1_LP3 =  -1.561018075800718, a2_LP3 = 0.641351538057563;
+const double b0_LP3 = 0.020083365564211, b1_LP3 = 0.040166731128423, b2_LP3 = 0.020083365564211;
+const double a1_LP2 =  -1.561018075800718, a2_LP2 = 0.641351538057563;
+const double b0_LP2 = 0.020083365564211, b1_LP2 = 0.040166731128423, b2_LP2 = 0.020083365564211;
+const double a1_LP4 =  -1.561018075800718, a2_LP4 = 0.641351538057563;
+const double b0_LP4 = 0.020083365564211, b1_LP4 = 0.040166731128423, b2_LP4 = 0.020083365564211;
+
+/*
+const double a1_LP =  -1.911197067426073, a2_LP = 0.914975834801434;
+const double b0_LP = 0.067455273889072, b1_LP = 0.134910547778144, b2_LP = 0.067455273889072;
+const double a1_HP = -1.982228929792529, a2_HP = 0.982385450614125;
+const double b0_HP =  0.991153595101663, b1_HP = -1.982307190203327, b2_HP = 0.991153595101663;
+*/
+biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP);
+biquadFilter Filter2(a1_HP, a2_HP, b0_HP, b1_HP, b2_HP);
+biquadFilter Filter3(a1_LP3, a2_LP3, b0_LP3, b1_LP3, b2_LP3);
+biquadFilter Filter4(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2);
+biquadFilter Filter5(a1_LP4, a2_LP4, b0_LP4, b1_LP4, b2_LP4);
+biquadFilter Filter6(a1_LP5, a2_LP5, b0_LP5, b1_LP5, b2_LP5); // fk is 2 Hz
+biquadFilter Filter7(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP);
+
+double Y_filt;
+double Y_filt2;
+double Y_filtprev;
+double Y_filtprev2;
+double Y_filtprev3;
+double Y_filtprev4;
+double Y_filtprev5;
+double Y_filtder;
+int A;
+float time_;
+const float time_increment = 0.005;
+
+// The data read and send function
+void scopeSend()
+{
+    scope.set(0,Force_nieuw);
+    //scope.set(1,Input);
+    scope.set(3,Input);//Boardpotmeter2.read());
+    scope.set(1,Y_filtder);
+    scope.set(2,Plaats_der);
+    scope.set(4,A);
+    scope.send();
+
+}
+
+void tickerfunctie()                //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
+{
+    LoopTimerFlag = 1;
+}
+
+void tickerfunctie2()                //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
+{
+    LoopTimerFlag2 = 1;
+}
+
+
+double Referentieschaling(double A, double B)
+{
+    double Y= 787.3008916207206*pow(A,4) -565.1143141517078*pow(A,3) + 122.8516837382677*pow(A,2) + 0.0556616744031*A + 0.0912411880277;
+    Referentie2 = Filter4.step(Y);
+    if (Referentie2<=2.5 or Referentie2>=9.2) {
+        Led = 0;
+        Referentie2 = B;
+    }
+    return Referentie2;
+}
+
+double Inputberekening(double B)
+{
+    double Inp = 10*B;            // De potmeter geeft ook waardes tussen 0 en 1, dit wordt met een factor 10 geschaald zodat deze als een positie in cm opgelegd kunnen worden.
+    Input = Filter3.step(Inp);
+    //Input = 6+(2.5*sin(time_));
+    if (Input>=9) {                             // De Input moet binnnen een aantal grenzen blijven. Groter dan 7 is mechanisch niet mogelijk.
+        Input=9;                                // Bij een waarde kleiner dan 1,5 zijn de strings niet meer gewikkeld en werkt de controller averechts en is deze uiterst instabiel.
+    } else if (Input<=3.0) {
+        Input = 3.0;
+    }
+    return Input;
+}
+
+/*double Input2(double B)
+{
+    // if (Y_filtder
+    if (Y_filtder>=1) {
+        Input3 = Input_prev2+0.01;
+        wait(0);
+    }
+    if (Y_filtder<= -1) {
+        Input3 = Input_prev2-0.01;
+        wait(0);
+    }
+    if (Input3 >= 9) {
+        Input3 =9;
+    }
+    if (Input3 <= 2.5) {
+        Input3 =2.5;
+    }
+
+    Input_prev2 = Input3;
+    return Input3;
+}*/
+
+
+double Force_prev;
+double Input_prev3;
+
+double Inputberekening3(double B){
+    if (Force_nieuw>=2.5) {
+        if (A == 1) {
+            Input3 = Input_prev3-0.01;
+        }
+        if (A == 0 && Force_prev>2.5) {
+            A = 0;
+            //pc.printf("A = 0 en Forceprev>1.5 \n");
+        }
+        if (A <= 0 && Force_prev<=2.5) {
+            A++;
+            Input3 = Input_prev3+0.01;
+           // pc.printf("A = 0 en Forceprev<1.5 \n");
+        }
+    }
+    else if (Force_nieuw<=-2.5) {
+        if (A == -1)
+            { Input3 = Input_prev3+0.01;
+            }
+            if (A == 0 && Force_prev<-2.5) {
+                A = 0;
+               // pc.printf("A = 0 en Forceprev<1.5 Force<-1.5 \n");
+            }
+        if (A >= 0 && Force_prev>=-2.5) {
+            A--;
+            Input3 = Input_prev3-0.01;
+           // pc.printf("A = 0 en Forceprev<1.5 Force<-1.5 \n");
+        }
+    } 
+    else {
+        Input3 = Input_prev3 - A*0.01;
+       // pc.printf("Else A = %i \n", A);
+    }
+    Force_prev= Force_nieuw;
+    if (Input3 >= 9) {
+        Input3 =9;
+    }
+    if (Input3 <= 3.5) {
+        Input3 =3.5;
+    }
+    Input_prev3 = Input3;
+    return Input3;
+
+}
+
+double Errorberekening(double Ref,double Input)
+{
+    Error = Input-Ref;                  // Het Error-signaal wordt ook gebruikt voor de PWMOut, dus mag deze niet hoger worden dan 1, 1 is immers al full speed voor de motor.
+    /* if (Error>=1) {
+        Error=1;
+    } else if (Error<=-1) {
+        Error = -1;
+    }
+    else if (fabs(Error)<0.01)
+    { Error = 0; }*/
+    return Error;
+}
+double PID_controller(double Error, double KP, double KI, double KD, double Ts, double &Error_int, double &e_prev)
+{
+    Error_der = (Error-Error_prev)/Ts;
+    Error_prev = Error;
+    Error_int = Error_int + Ts*Error;
+
+    return KP*Error+KI*Ts*Error_int+KD*Ts*Error_der;
+}
+
+void Afgeleide_Force()
+{
+    //while(LoopTimerFlag2 !=1);                  // Als LTF 0 is, blijft hij 0 en stopt de loop.
+    //LoopTimerFlag2 = 0;
+    Y_filtder = (Y_filt-Y_filtprev)/0.01;
+    //Y_filtder = 3*Filter7.step(Y_filtder);
+    Y_filtprev = Y_filt;
+}
+
+void Afgeleide_Plaats()
+{
+    //while(LoopTimerFlag2 !=1);                  // Als LTF 0 is, blijft hij 0 en stopt de loop.
+    LoopTimerFlag2 = 0;
+    Plaats_der = Ref_der/0.01; //(Referentie2-Ref_prev2)/0.1;
+    Plaats_der1 = Filter6.step(Plaats_der);
+    Plaats_der = (Plaats_der1-Ref_der_prev)/0.1;
+    //Plaats_der = Filter5.step(Plaats_acc);
+    if (Plaats_der>=0) {
+        Plaats_der = Plaats_der;
+    } else if (Plaats_der<0) {
+        Plaats_der = 0.5*Plaats_der;
+    }
+    Ref_der_prev= Plaats_der1;
+}
+
+void Motor_controller()
+{
+    if(Output>=0) {
+        motor1direction.write(1);
+        motor1speed = 0.2+fabs(Output);
+    } else if (Output<0) {
+        motor1direction.write(0);
+        motor1speed = 0.05*fabs(Output);
+    }
+    /* if(Input_der < 0)
+     {
+            // motor1direction.write(0);
+            // motor1speed = 0.05*fabs(Output);
+         if(Output>=0)
+             {
+             motor1direction.write(1);
+             motor1speed = 0.3*fabs(Output);
+             }
+         else if (Output<0)
+             {
+             motor1direction.write(0);
+             motor1speed = 0.09*fabs(Output);
+             }
+     }
+     if(Input_der >= 0)
+     {
+    // motor1direction.write(1);
+     //motor1speed = 1;
+         if(Output>=-0.50)
+             {
+             motor1direction.write(1);
+             motor1speed = (fabs(Output)+0.3);
+             }
+         else if (Output<-0.5)
+             {
+             motor1direction.write(0);
+             motor1speed = 0.01*fabs(Output);
+             }
+     }*/
+
+
+    //if (fabs(Output)>=0.01)
+    // motor1speed = fabs(Output);
+    //else if (fabs(Output)<0.01)
+    //{motor1speed= 0;}
+}
+
+int main()
+{
+    Roodisplus.write(1);
+    Versterker.write(1);
+    // Attach the data read and send function at 100 Hz
+    scopeTimer.attach_us(&tickerfunctie, 1/0.0003);// 1e4);
+    ForceTimer.attach_us(&tickerfunctie2, 1e4);
+    while(1)    {
+        while(LoopTimerFlag !=1);                  // Als LTF 0 is, blijft hij 0 en stopt de loop.
+        LoopTimerFlag = 0;    // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
+        Led = 1;
+        Y_filt = 3*Filter1.step(a_in3.read());
+        Y_filt2 = Filter2.step(Y_filt);
+        Input = Inputberekening3(Force_nieuw); //Inputberekening(Boardpotmeter.read()); //Input2(Y_filtder); //Inputberekening(Boardpotmeter.read()); Input3(Force_nieuw);
+        time_ += time_increment;
+        Ref2 =  Referentieschaling(Referentie.read()/2,Input);       // De referentiewaarde is via deze functie (gevonden met metingen en polyfit) verbonden aan de afstand in centimeters voor waarden tussen 0 en 0.5.
+        Error = Errorberekening(Ref2, Input);
+        Ref_der = Referentie2-Ref_prev;
+        Input_der = Input-Input_prev;
+        Ref_prev = Referentie2;
+        Input_prev = Input;
+        Output = PID_controller(Error,3,2,1,0.01, Error_prev, Error_int);
+        Motor_controller();
+        if (LoopTimerFlag2==1) {
+            Afgeleide_Force();
+            Afgeleide_Plaats();
+            Force_nieuw = Y_filtder-Boardpotmeter2.read()*Plaats_der;
+        }
+        scopeSend();
+    }
+
+
+}
\ No newline at end of file