Diff: main.cpp

Revision:

7:9ea55ce667be

Parent:

6:e206abd0b2ca

--- a/main.cpp	Wed Jun 01 13:38:55 2016 +0000
+++ b/main.cpp	Tue Jun 21 15:02:23 2016 +0000
@@ -5,7 +5,6 @@
 #include "math.h"
 #include "biquadFilter.h"
 
-
 DigitalOut Rood(LED_RED);
 DigitalOut Groen(LED_GREEN);
 DigitalOut Blauw(LED_BLUE);
@@ -25,15 +24,12 @@
 // Verklaren van de in en outputs
 Ticker Loopticker;                  // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
 volatile bool LoopTimerFlag2 = 0;    // Volatile, omdat deze heel vaak verandert van waar naar onwaar.
-//Ticker Loopticker2;                  // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
-//volatile bool LoopTimerFlag3 = 0;
 AnalogIn Referentie(A0);            // De schuifpotmeter naast de geleiding van de draad is de referentie.
 AnalogIn Boardpotmeter(A2);         // POT1 op het board wordt gebruikt als input van de controlloop.
 AnalogIn Boardpotmeter2(A1);
 DigitalOut motor1direction(D7);     //D6 en D7 voor motor 1 (op het motorshield)
 FastPWM motor1speed(D6);
 
-
 float potmeter;
 float Error;
 float Referentie2;//=0.0;
@@ -57,10 +53,6 @@
 const double b0_LP = 0.020083365564211, b1_LP = 0.040166731128423, b2_LP = 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_LP2 =  -1.866892279711715, a2_LP2 = 0.875214548253684;
-//const double b0_LP2 = 0.002080567135492, b1_LP2 = 0.004161134270985, b2_LP2 = 0.002080567135492;
-//const double a1_HP = -1.982228929792529, a2_HP = 0.982385450614125;
-//const double b0_HP =  0.991153595101663, b1_HP = -1.982307190203327, b2_HP = 0.991153595101663;
 
 float time_;
 const float time_increment = 0.01;
@@ -81,7 +73,7 @@
 
 double Inputberekening(double B)
 {
-    Input = Baseline+(Amplitude*-cos(2*time_));
+    Input = Baseline+(Amplitude*-cos(1.5*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) {
@@ -92,15 +84,7 @@
 
 double Errorberekening(double Input,double Ref)
 {
-    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;
-    }
-    */
+    Error = Input-Ref;                 
     return Error;
 }
 double PID_controller(double Error, double KP, double KI, double KD, double Ts, double &Error_int, double &e_prev)
@@ -133,16 +117,16 @@
     if(Ref_der < 0) {
         if(Output>=0) {
             motor1direction.write(1);
-            motor1speed = 0.01*fabs(Output);
+            motor1speed = 0.1*fabs(Output);
         } else if (Output<0) {
             motor1direction.write(0);
-            motor1speed = 0.05*fabs(Output);
+            motor1speed = 0.25*fabs(Output);
         }
     }
     if(Ref_der > 0) {
         if(Output>=0) {
             motor1direction.write(1);
-            motor1speed = (20*fabs(Output));
+            motor1speed = (fabs(Output));
         } else if (Output<0) {
             motor1direction.write(0);
             motor1speed = 0.01*fabs(Output);
@@ -153,11 +137,11 @@
 void Motor_controller2(float Foutje)
     {
         if(Foutje>=0.01){
-            motor1direction.write(1);
-            motor1speed.write(0.05*Foutje);
+            motor1direction.write(0);
+            motor1speed.write(0.02*Foutje);
             }
         else if(Foutje<=-0.01){
-            motor1direction.write(0);
+            motor1direction.write(1);
             motor1speed.write(-Foutje);
             }
         else {
@@ -170,7 +154,7 @@
     Error = 0;
     int J = 0;
     time_ = 0;                                      // Voordat het hele programma begint, staat de Error op 0, zodat de motor niet spastisch gaat draaien om dit te compenseren.
-          while(1 && J<=320) {
+          while(1 && J<=480) {
             while(LoopTimerFlag2 !=1);                   // Als LTF 0 is, blijft hij 0 en stopt de loop.
             LoopTimerFlag2 = 0;                         // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
             J= J+1;
@@ -182,7 +166,7 @@
             Ref_der = Referentie2-Ref_prev;
             Ref_prev = Referentie2;
             Error = Errorberekening(Input, Referentie2);
-            Output = PID_controller(Error,5,1,0.1,0.01, Error_prev, Error_int);
+            Output = PID_controller(Error,0.5,0.1,0.01,0.01, Error_prev, Error_int); //,5,1,0.1,
             Motor_controller(Output);
         }
     //}
@@ -204,15 +188,11 @@
         } else if (bool (1)) {
             POT = Boardpotmeter2.read();
         }
-        Input = 10*POT; //Inputberekening2(POT,0);  Inputberekening(Boardpotmeter.read());          // 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.
-        // time_ += time_increment;
+        Input = 10*POT; // 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.
         double 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.
         Rood = 1;
-        //Ref_prev = Ref2;
         Error = Referentie2-Input;
-      // pc.printf("Input = %f Referentie= %f Error= %f \n", Input, Referentie2, Error);
         Motor_controller2(Error);
-       // pc.printf("output = %f \n", motor1speed.read());
         
     }
     
@@ -237,7 +217,6 @@
     Blauw = 1;
     Groen = 1;
     int i = 0;
-    pc.printf("Hello World! %i \n", i);
     Finitestatemachine.attach(tickerfunctie,0.1);
     Loopticker.attach(tickerfunctie2,0.01);
     Hidscope.attach(scopeSend,0.01);                // Verzenden naar HIDscope
@@ -246,16 +225,12 @@
         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
         if (Start==0 && Setting==0 && Dotask==0) { // In eerste instantie gaan de leds even uit
-
             Start = 1;
             Groen = 0;
-            pc.printf("State 1\n");
         }
-
         if (Start==1 && Setting==0 && Dotask==0) {      // State 2 heeft Geel als kleur
             Groen = 0;
             Rood = 0;
-            pc.printf("State 2\n");
             if (Button1pressed.read()==1 && Button2pressed.read()==0) {
                 if (Amplitude != 0){
                 Setting = 2;
@@ -263,14 +238,12 @@
                 Groen = 1;
                 Rood = 1;
                 N = 0;
-                pc.printf("State 2 D\n");
                 wait(0.5);}
                 else {
                 Setting = 1;
                 Start = false;
                 Groen = 1;
                 Rood = 1;
-                pc.printf("State 2 A\n");
                 wait(0.5);}
 
             }
@@ -279,20 +252,16 @@
                 Rood = 1;
                 Dotask = 1;
                 Start=0;
-                pc.printf("State 2 B\n");
                 wait(0.5);
             }
         }
         if (Start==0 && Setting==1 && Dotask==0) {          // Leds zijn uit, elke bepaling gaat het groene lampje even branden.
-            pc.printf("State 2 C\n");
             P_controller(0,0);
             double Maximum = 10*POT;
             Groen = 1;
-            pc.printf("Max = %f \n", Maximum);
             P_controller(0,1);
             double Minimum = 10*POT;
             Groen = 1;
-            pc.printf("Min = %f \n", Minimum);
             Amplitude = (Minimum+Maximum)/2; 
             Baseline = (Maximum-Minimum)/2;
             Rood = 0;
@@ -304,31 +273,22 @@
         }
 
         if (Start==0 && Setting==2 && Dotask==0) {          // Leds zijn uit, elke count gaat het groene lampje even branden.
-            pc.printf("State 3\n");
             Determinetask();                            // Aantal gewenste herhalingen van Dotask instellen
-            pc.printf("N = %i \n", N);
             if (Button1pressed.read()== 1 && Button2pressed.read()== 0) {
                 Setting = 0;
                 Dotask = 1;
-                pc.printf("State 3 B\n");
             }
         }
 
         if (Start==0 && Setting==0 && Dotask==1) {
-            pc.printf("State 4\n");
             while(i < N) {
-
-                pc.printf("i = %i  N = %i \n", i, N);
                 Extendfinger();
-                //int J = 0;
-                pc.printf("Extendfinger()afgerond\n", i, N);
                 i++;
             }
             if (i==N) {
                 Dotask = 0;
                 Start = 1;
                 i=0;
-                pc.printf("Final state \n");
             }
 
         }