Diff: main.cpp

Revision:

3:9f9ef68a25a2

Parent:

2:1dd9e630a7b5

Child:

4:4ad3fc99c356

--- a/main.cpp	Tue May 31 14:53:58 2016 +0000
+++ b/main.cpp	Wed Jun 01 11:35:38 2016 +0000
@@ -41,6 +41,8 @@
 float Input;
 float Output;
 float POT;
+float Foutje;
+float Potmetertje;
 double Error_prev = 0;
 double Error_int = 0;
 double Error_der;
@@ -132,12 +134,7 @@
     LoopTimerFlag2 = 1;
 }
 
-void tickerfunctie3()                //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
-{
-    LoopTimerFlag3 = 1;
-}
-
-void Motor_controller()                 // De P_controller, door de ticker elke honderdste seconde uitgevoerd.
+void Motor_controller(float Output)                 // De P_controller, door de ticker elke honderdste seconde uitgevoerd.
 {
     if(Ref_der < 0) {
         if(Output>=0) {
@@ -148,7 +145,7 @@
             motor1speed = 0.05*fabs(Output);
         }
     }
-    if(Ref_der >= 0) {
+    if(Ref_der > 0) {
         if(Output>=0) {
             motor1direction.write(1);
             motor1speed = (20*fabs(Output));
@@ -157,16 +154,40 @@
             motor1speed = 0.01*fabs(Output);
         }
     }
+   /* if(bool Banaan = 1) {
+        //pc.printf("Banaan \n");
+        if(Output>=0) {
+            motor1direction.write(1);
+            motor1speed = fabs(Output);
+        } else if (Output<0) {
+            motor1direction.write(0);
+            motor1speed = fabs(Output);
+        }
+    }*/
 }
+
+void Motor_controller2(float Foutje)
+    {
+        if(Foutje>=0.01){
+            motor1direction.write(1);
+            motor1speed.write(0.05*Foutje);
+            }
+        else if(Foutje<=-0.01){
+            motor1direction.write(0);
+            motor1speed.write(-Foutje);
+            }
+        else {
+            motor1speed.write(0);
+            }
+    }
+
 void Extendfinger()
 {
     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) {
-        Loopticker.attach(tickerfunctie2,0.01);
-        Hidscope.attach(scopeSend,0.01);                // Verzenden naar HIDscope
-        while(1 && J<=320) {
+    //while(1 && J<=320) {
+          while(1 && J<=320) {
             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;
@@ -179,9 +200,9 @@
             Ref_prev = Referentie2;
             Error = Errorberekening(Input, Referentie2);
             Output = PID_controller(Error,5,1,0.1,0.01, Error_prev, Error_int);
-            Motor_controller();
+            Motor_controller(Output);
         }
-    }
+    //}
     motor1speed.write(0);
 
 }
@@ -198,54 +219,29 @@
     double Set_Error = Referentie.read()-Max;
 }*/
 
-double P_controller(double Max, bool Keuze)
+void P_controller(double Max, bool Keuze)
 {
-    while (Button1pressed.read()==1) {
-        Loopticker2.attach(tickerfunctie3,0.01);
-        while(LoopTimerFlag3 !=1);                  // Als LTF 0 is, blijft hij 0 en stopt de loop.
+    while (Button1pressed.read()==1) { 
+        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
-        //pc.printf("Hello World! \n");
         if (bool (0)) {
             POT = Boardpotmeter.read();
         } else if (bool (1)) {
             POT = Boardpotmeter2.read();
         }
-        float Input = 10*POT; //Inputberekening2(POT,0);           // 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.
-        //pc.printf(" J is %i \n",J);
-        Led = 1;
-        float 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.
-        Ref_der = Ref2-Ref_prev;
-        Ref_prev = Ref2;
-        Error = Input-Ref2;
-        pc.printf("Input = %f Referentie= %f Error= %f \n", Input, Ref2, Error);
-        Output = PID_controller(Error,5,1,0.1,0.01, Error_prev, Error_int);
-        Motor_controller();
+        float 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;
+        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.
+        //Ref_prev = Ref2;
+        float 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());
         
     }
 
-
-
-    /* //double Max = Filter2.step(Val);
-     double Ref3 = Referentieschaling(Referentie.read()/2,Val);
-     double Set_Error = Ref3-Val;
-     pc.printf("Maximum = %d Referentie= %d Error= %d \n", Val, Ref3, Set_Error);
-     Output = Set_Error;
-     if(Set_Error>=0.02) {
-         motor1direction.write(1);
-         motor1speed = fabs(Set_Error);
-     }
-     else if (Set_Error<-0.02) {
-         motor1direction.write(0);
-         motor1speed = fabs(Set_Error);
-     }
-     else {
-         motor1speed = 0;
-     } */
-
-
     Groen = 0;
     wait (0.5);
-    return Max;
 }
 
 void Determinetask()
@@ -267,6 +263,8 @@
     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
 
     while (true) {
         while(LoopTimerFlag !=1);                   // Als LTF 0 is, blijft hij 0 en stopt de loop.
@@ -301,12 +299,14 @@
                 wait(0.5);
             }
         }
-        if (Start==0 && Setting==1 && Dotask==0) {          // Leds zijn uit, elke count gaat het groene lampje even branden.
+        if (Start==0 && Setting==1 && Dotask==0) {          // Leds zijn uit, elke bepaling gaat het groene lampje even branden.
             pc.printf("State 2 C\n");
-            double Maximum = P_controller(0,0);
+            P_controller(0,0);
+            double Maximum = 10*POT;
             Groen = 1;
             pc.printf("Max = %f \n", Maximum);
-            double Minimum = P_controller(0,1);
+            P_controller(0,1);
+            double Minimum = 10*POT;
             Groen = 1;
             pc.printf("Min = %f \n", Minimum);
             Setting = 2;