Diff: main.cpp

Revision:

4:7052ee491e12

Parent:

3:123f3fd0daf6

Child:

5:31120c4c08f0

--- a/main.cpp	Thu Jun 12 10:10:37 2014 +0000
+++ b/main.cpp	Thu Jun 12 12:17:23 2014 +0000
@@ -2,6 +2,7 @@
 #include "encoder.h"
 #include "MODSERIAL.h"
 #include "time.h"
+#include "ref.h"
 
 MODSERIAL pc(USBTX,USBRX);
 Encoder Enc(PTA1,PTA2);
@@ -61,8 +62,8 @@
     motorplus.period(1.0/60000.0);
     motormin.period(1.0/60000.0);
 
-    Kp = 0.5;
-    Ki = 0.4;
+    Kp = 1.1471/1;
+    Ki = 4.9157/1;
     pwmotor=-0.9/2.0;
     deliver=0.01*pwmotor;
     pos1=0;
@@ -72,6 +73,7 @@
     pos=Enc.getPosition();
     speed=Enc.getSpeed();
 
+/*
     Referentie[0]=20.0;
     Referentie[1]=17.0;
     Referentie[2]=14.0;
@@ -81,7 +83,8 @@
     Referentie[6]=14.0;
     Referentie[7]=25.0;
     Referentie[8]=23.0;
-
+*/
+/*
     Tijd[0]=0;
     Tijd[1]=5.5;
     Tijd[2]=7.0;
@@ -91,7 +94,7 @@
     Tijd[6]=17.5;
     Tijd[7]=23.0;
     Tijd[8]=25.0;
-
+*/
     wait(1);
     pc.printf("start2\n\r");
     wait(1);
@@ -161,30 +164,23 @@
         Error=Ref-pos;
         Prop=Error*Kp;
         Int1=Int;
-        Int=(Int1+0.001*ts*Error)*Ki;
+        Int=(Int1+0.01*ts*Error)*Ki;
         Ctrl=Prop+Int;
 
         pwmotor=Ctrl;
         deliver=pwmotor;
-        //if (abs(deliver) <= abs(pwmotor)) {
-        //    deliver=deliver*1.01;
-        //}
-        //if (abs(deliver) >= abs(pwmotor)) {
-        //    deliver=deliver*0.99;
-        //}
-        //pc.printf("del %f pwm %f\n\r",deliver,pwmotor);
         motorplus.write(0.5+deliver);
         motormin.write(0.5-deliver);
         if (t >= 30.0) {
             motorplus.write(0.0);
             motormin.write(0.0);
-            pc.printf("Beindigd door tijd");
+            pc.printf("\n\rBeindigd door tijd");
             return 0;
         }
         if (abs(pos) >= 30 || (t >=10.0 && pos <=0.5)) {
             motorplus.write(0.0);
             motormin.write(0.0);
-            pc.printf("Beindigd door positie");
+            pc.printf("\n\rBeindigd door positie\n\r");
             return 0;
         }
     }