Diff: main.cpp

Revision:

4:4ad3fc99c356

Parent:

3:9f9ef68a25a2

Child:

5:f97bf30bec14

--- a/main.cpp	Wed Jun 01 11:35:38 2016 +0000
+++ b/main.cpp	Wed Jun 01 12:44:55 2016 +0000
@@ -32,12 +32,11 @@
 AnalogIn Boardpotmeter2(A1);
 DigitalOut motor1direction(D7);     //D6 en D7 voor motor 1 (op het motorshield)
 FastPWM motor1speed(D6);
-DigitalOut Led(LED_RED);
 
 
 float potmeter;
 float Error;
-float Referentie2=1.0;
+float Referentie2;//=0.0;
 float Input;
 float Output;
 float POT;
@@ -48,6 +47,8 @@
 double Error_der;
 double Ref_der = 0;
 double Ref_prev = 0;
+float Amplitude;
+float Baseline;
 // Define the HIDScope and Ticker object
 HIDScope    scope(4);
 Ticker Hidscope;
@@ -72,7 +73,7 @@
     double Y= 787.3008916207206*pow(A,4) -565.1143141517078*pow(A,3) + 122.8516837382677*pow(A,2) + 0.0556616744031*A + 0.0912411880277;
     Referentie2 = Filter1.step(Y);
     if (Referentie2<=2.5 or Referentie2>=9.2) {
-        Led = 0;
+        Rood = 0;
         Referentie2 = B;
     }
     return Referentie2;
@@ -80,7 +81,7 @@
 
 double Inputberekening(double B)
 {
-    Input = 5.5+(2.5*-cos(2*time_));
+    Input = Baseline+(Amplitude*-cos(2*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) {
@@ -192,7 +193,7 @@
             LoopTimerFlag2 = 0;                         // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
             J= J+1;
             //pc.printf(" J is %i \n",J);
-            Led = 1;
+            Rood = 1;
             double Input = Inputberekening(Boardpotmeter.read());
             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.
@@ -229,17 +230,18 @@
         } else if (bool (1)) {
             POT = Boardpotmeter2.read();
         }
-        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.
+        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.
+        Rood = 1;
         //Ref_prev = Ref2;
-        float Error = Referentie2-Input;
+        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());
         
     }
-
+    
     Groen = 0;
     wait (0.5);
 }
@@ -309,6 +311,13 @@
             double Minimum = 10*POT;
             Groen = 1;
             pc.printf("Min = %f \n", Minimum);
+            Amplitude = (Minimum+Maximum)/2; 
+            Baseline = (Maximum-Minimum)/2;
+            Rood = 0;
+            Blauw = 0;
+            wait (2);
+            Rood = 1;
+            Blauw = 1;
             Setting = 2;
         }