Diff: main.cpp

Revision:

1:57926254e65b

Parent:

0:c727f4699e80

Child:

2:f5bb7fcfc7a2

--- a/main.cpp	Wed Jun 08 11:43:46 2016 +0000
+++ b/main.cpp	Wed Jun 08 12:39:14 2016 +0000
@@ -9,23 +9,20 @@
 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;
@@ -38,53 +35,32 @@
 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_LP5 = -1.822694925196308, a2_LP5 = 0.837181651256023 ;    
+const double b0_LP5 =  0.003621681514929 , b1_LP5 =  0.007243363029857, b2_LP5 = 0.003621681514929; 
 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);
+biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP);        // Lowpassfilter voor signaal van krachtsensor, 300 Hz, fk = 2
+biquadFilter Filter2(a1_HP, a2_HP, b0_HP, b1_HP, b2_HP);        // Highpassfilter voor signaal van krachtsensor, 300 Hz, fk = 10 
+biquadFilter Filter3(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2);   // Lowpassfilter voor signaal positie-potmeter, 300 Hz
+biquadFilter Filter4(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2);   // Lowpassfilter voor geschaalde signaal positie-potmeter, 300 Hz 
+biquadFilter Filter6(a1_LP5, a2_LP5, b0_LP5, b1_LP5, b2_LP5); // Laagdoorlaatfilter 100 Hz, fk is 2 Hz
+
 
 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_;
@@ -94,8 +70,7 @@
 void scopeSend()
 {
     scope.set(0,Force_nieuw);
-    //scope.set(1,Input);
-    scope.set(3,Input);//Boardpotmeter2.read());
+    scope.set(3,Input);
     scope.set(1,Y_filtder);
     scope.set(2,Plaats_der);
     scope.set(4,A);
@@ -129,7 +104,6 @@
 {
     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) {
@@ -138,64 +112,36 @@
     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){
+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");
-            }
+    } else if (Force_nieuw<=-2.5) {
+        if (A == -1) {
+            Input3 = Input_prev3+0.01;
+        }
+        if (A == 0 && Force_prev<-2.5) {
+            A = 0;
+        }
         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 {
+    } else {
         Input3 = Input_prev3 - A*0.01;
-       // pc.printf("Else A = %i \n", A);
     }
     Force_prev= Force_nieuw;
     if (Input3 >= 9) {
@@ -212,13 +158,7 @@
 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)
@@ -232,21 +172,16 @@
 
 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_der = Ref_der/0.01;
     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) {
@@ -264,42 +199,6 @@
         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()
@@ -315,14 +214,12 @@
         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);
+        Input = Inputberekening3(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) {