Revision 18:b42a884bca02, committed 2017-11-03

Comitter:

Annelotte

Date:

Fri Nov 03 03:13:41 2017 +0000

Parent:

17:1246d6b0c5d0

Commit message:

Met demo modus
; hoop ik

Changed in this revision

diff -r 1246d6b0c5d0 -r b42a884bca02 main.cpp
--- a/main.cpp	Thu Nov 02 23:36:43 2017 +0000
+++ b/main.cpp	Fri Nov 03 03:13:41 2017 +0000
@@ -38,6 +38,7 @@
 double Tor2 = 0;
 double w1= 0;
 double w2= 0;
+bool automode = false;
 
 Encoder motor1(D13,D12,true);
 MODSERIAL pc(USBTX,USBRX);
@@ -83,27 +84,107 @@
 }
 
 void SetpointRobot()
-{    
-    double Potmeterwaarde2 = potMeter2.read();
-    double Potmeterwaarde1 = potMeter1.read();
+{   
+    if automode == 0
+    {
+        for (int n = 0; n < 100; n++)
+        {
+            Rsy -= 0.002;
+            RKI();
+            // hier the control of the control system
+            float Huidigepositie = Encoder(); 
+            float error = (refP - Huidigepositie);// make an error
+            float motorValue = FeedBackControl(error, e_prev, e_int);
+            SetMotor1(motorValue);
+
+            float Huidigepositie2 = Encoder2(); 
+            float error2 = (refP2 - Huidigepositie2);// make an error
+            float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+            SetMotor2(motorValue2);
+            //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
+            pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
+        }
+        
+        for (int n = 0; n < 100; n++)
+        {
+            Rsy += 0.002;
+            RKI();
+            // hier the control of the control system
+            float Huidigepositie = Encoder(); 
+            float error = (refP - Huidigepositie);// make an error
+            float motorValue = FeedBackControl(error, e_prev, e_int);
+            SetMotor1(motorValue);
+
+            float Huidigepositie2 = Encoder2(); 
+            float error2 = (refP2 - Huidigepositie2);// make an error
+            float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+            SetMotor2(motorValue2);
+            //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
+            pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
+        }
+        
+        for (int n = 0; n < 100; n++)
+        {
+            Rsx += 0.002;
+            RKI();
+            // hier the control of the control system
+            float Huidigepositie = Encoder(); 
+            float error = (refP - Huidigepositie);// make an error
+            float motorValue = FeedBackControl(error, e_prev, e_int);
+            SetMotor1(motorValue);
+
+            float Huidigepositie2 = Encoder2(); 
+            float error2 = (refP2 - Huidigepositie2);// make an error
+            float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+            SetMotor2(motorValue2);
+            //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
+            pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
+        }
     
-    if  (Potmeterwaarde2>0.6) {
-        Rsx += 0.001;                    //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
+        for (int n = 0; n < 100; n++)
+        {
+            Rsx -= 0.002;
+            RKI()
+            // hier the control of the control system
+            float Huidigepositie = Encoder(); 
+            float error = (refP - Huidigepositie);// make an error
+            float motorValue = FeedBackControl(error, e_prev, e_int);
+            SetMotor1(motorValue);
+
+            float Huidigepositie2 = Encoder2(); 
+            float error2 = (refP2 - Huidigepositie2);// make an error
+            float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+            SetMotor2(motorValue2);
+            //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
+            pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
+        }
+    
+        automode = 1;
     }
-    else if  (Potmeterwaarde2<0.4) {
-        Rsx -= 0.001;                    //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
-    }
-    else {                               //de x-waarde van de setpoint verandert niet
+    
+    else automode = 1
+    {
+        double Potmeterwaarde2 = potMeter2.read();
+        double Potmeterwaarde1 = potMeter1.read();
+    
+        if  (Potmeterwaarde2>0.6) {
+            Rsx += 0.001;                    //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
+        }
+        else if  (Potmeterwaarde2<0.4) {
+            Rsx -= 0.001;                    //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
+        }
+        else {                               //de x-waarde van de setpoint verandert niet
         }
 
-    if (Potmeterwaarde1>0.6) {           //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
-        Rsy += 0.001;
+        if (Potmeterwaarde1>0.6) {           //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
+            Rsy += 0.001;
+        }
+        else if (Potmeterwaarde1<0.4) {      //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4
+            Rsy -= 0.001;                       
+        }
+        else {                               //de y-waarde van de setpoint verandert niet
+        }
     }
-    else if (Potmeterwaarde1<0.4) {      //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4
-        Rsy -= 0.001;                       
-    }
-    else {                               //de y-waarde van de setpoint verandert niet
-        }
 }
     
 float FeedBackControl(float error, float &e_prev, float &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)