Diff: main.cpp

Revision:

5:31120c4c08f0

Parent:

4:7052ee491e12

Child:

6:7ff223862008

--- a/main.cpp	Thu Jun 12 12:17:23 2014 +0000
+++ b/main.cpp	Thu Jun 12 12:59:32 2014 +0000
@@ -3,6 +3,7 @@
 #include "MODSERIAL.h"
 #include "time.h"
 #include "ref.h"
+#include "Tijd.h"
 
 MODSERIAL pc(USBTX,USBRX);
 Encoder Enc(PTA1,PTA2);
@@ -12,11 +13,11 @@
 double rpm, pos, pos1, t, Ref, Error, deliver;
 double Prop, Kp, Int, Int1, Ki, Ctrl;
 double pwmotor;
-double speed, move;
+double speed, move, size;
 volatile bool looptimerflag, calflag, scriptflag;
 double Move[25];
-double Tijd[9];
-double Referentie[9];
+//double Tijd[9];
+//double Referentie[9];
 int n;
 
 void setlooptimerflag(void)
@@ -41,7 +42,11 @@
 }
 void printdata3(void)
 {
-    pc.printf("P %f R %f\n\r",pos, Ref);
+    pc.printf("P %f R %f E %f\n\r",pos, Ref,Error);
+}
+void printdata4(void)
+{
+    pc.printf("%f\n",Error);
 }
 
 int main()
@@ -62,122 +67,130 @@
     motorplus.period(1.0/60000.0);
     motormin.period(1.0/60000.0);
 
-    Kp = 1.1471/1;
-    Ki = 4.9157/1;
+    //Kp = 1.1471/1;
+    //Ki = 4.9157/1;
+    Kp=1.5;
+    Ki=0.5;
     pwmotor=-0.9/2.0;
     deliver=0.01*pwmotor;
     pos1=0;
     n=0;
+    size=sizeof(Referentie)/sizeof(float);
     t=0;
     Move[24]=pwmotor*-100.0;
     pos=Enc.getPosition();
     speed=Enc.getSpeed();
 
-/*
-    Referentie[0]=20.0;
-    Referentie[1]=17.0;
-    Referentie[2]=14.0;
-    Referentie[3]=20.0;
-    Referentie[4]=16.0;
-    Referentie[5]=20.0;
-    Referentie[6]=14.0;
-    Referentie[7]=25.0;
-    Referentie[8]=23.0;
-*/
-/*
-    Tijd[0]=0;
-    Tijd[1]=5.5;
-    Tijd[2]=7.0;
-    Tijd[3]=9.25;
-    Tijd[4]=12.0;
-    Tijd[5]=15.5;
-    Tijd[6]=17.5;
-    Tijd[7]=23.0;
-    Tijd[8]=25.0;
-*/
+    /*
+        Referentie[0]=20.0;
+        Referentie[1]=17.0;
+        Referentie[2]=14.0;
+        Referentie[3]=20.0;
+        Referentie[4]=16.0;
+        Referentie[5]=20.0;
+        Referentie[6]=14.0;
+        Referentie[7]=25.0;
+        Referentie[8]=23.0;
+    */
+    /*
+        Tijd[0]=0;
+        Tijd[1]=5.5;
+        Tijd[2]=7.0;
+        Tijd[3]=9.25;
+        Tijd[4]=12.0;
+        Tijd[5]=15.5;
+        Tijd[6]=17.5;
+        Tijd[7]=23.0;
+        Tijd[8]=25.0;
+    */
     wait(1);
     pc.printf("start2\n\r");
     wait(1);
 
     scriptflag = true;
-    printer.attach(printdata,tsd);
+    //printer.attach(printdata,tsd);
     //script.attach(setscriptflag, ts);
     calibratie.attach(setcalflag,ts);
     while(scriptflag == true) {
-        while(calflag == true) {
+        while(calflag != true) {
+        }
+        calflag=false;
+        if (abs(deliver) <= abs(pwmotor)) {
+            deliver=deliver*1.01;
+        }
+        motorplus.write(0.5+deliver);
+        motormin.write(0.5-deliver);
+        //pc.printf("pwm %f\n\r",deliver);
+        speed=Enc.getSpeed();
+        pos1=pos;
+        pos=Enc.getPosition()/1024.0;
+        Move[n]=pos-pos1;
+        n += 1;
+        if (n >24) {
+            n=0;
+        }
+        move=Move[24]-Move[0];
+        //pc.printf("Pwm %f Pos %f\n\r",deliver,pos);
+
+        if(Move[24] == 0.0 && Move[0] == 0.0 && abs(pos) > 0.5) {
+            motorplus.write(0.5);
+            motormin.write(0.5);
+            //pwmotor = pwmotor*-1.0;
+            //deliver=0.01*pwmotor;
+            pc.printf("Einde\n\r");
+            calibratie.detach();
+            printer.detach();
             calflag=false;
-            if (abs(deliver) <= abs(pwmotor)) {
-                deliver=deliver*1.01;
-            }
-            motorplus.write(0.5+deliver);
-            motormin.write(0.5-deliver);
-            //pc.printf("pwm %f\n\r",deliver);
-            speed=Enc.getSpeed();
-            pos1=pos;
-            pos=Enc.getPosition()/1024.0;
-            Move[n]=pos-pos1;
-            n += 1;
-            if (n >24) {
-                n=0;
-            }
-            move=Move[24]-Move[0];
-            pc.printf("Pwm %f Pos %f\n\r",deliver,pos);
-            if(Move[24] == 0.0 && Move[0] == 0.0 && abs(pos) > 0.5) {
-                motorplus.write(0.5);
-                motormin.write(0.5);
-                //pwmotor = pwmotor*-1.0;
-                //deliver=0.01*pwmotor;
-                pc.printf("Einde\n\r");
-                calibratie.detach();
-                printer.detach();
-                calflag=false;
-                scriptflag=false;
-                wait(1);
-            }
+            scriptflag=false;
+            wait(1);
         }
+
     }
+
+
+
     Enc.setPosition(0.0);
     pwmotor=0.0;
     deliver=0.01;
     pc.printf("Begin loop\n\r");
     wait(1);
-    printer.attach(printdata3,tsd);
+    printer.attach(printdata4,tsd);
     looptimer.attach(setlooptimerflag,ts);
     Ref=Referentie[0];
     while(1) {
         while (looptimerflag != true) {}
         looptimerflag = false;
 //Encoder
-        pos=Enc.getPosition()/1024;
+        pos=Enc.getPosition()/1024.0;
         speed=Enc.getSpeed();
 
         if (t >= Tijd[n]) {
             Ref=Referentie[n];
             n +=1;
-            if (n >=7) {
-                n=7;
+            if (n >=size) {
+                n=size;
             }
         }
-          t = t + ts;
+        t = t + ts;
 
 //Regelaar
         Error=Ref-pos;
         Prop=Error*Kp;
         Int1=Int;
-        Int=(Int1+0.01*ts*Error)*Ki;
+        Int=(Int1+ts*Error)*Ki;
         Ctrl=Prop+Int;
 
         pwmotor=Ctrl;
         deliver=pwmotor;
         motorplus.write(0.5+deliver);
         motormin.write(0.5-deliver);
-        if (t >= 30.0) {
+        if (t >= size/100) {
             motorplus.write(0.0);
             motormin.write(0.0);
             pc.printf("\n\rBeindigd door tijd");
             return 0;
         }
-        if (abs(pos) >= 30 || (t >=10.0 && pos <=0.5)) {
+        if (abs(pos) >= 30 || (t >=5.0 && pos <=0.5)) {
             motorplus.write(0.0);
             motormin.write(0.0);
             pc.printf("\n\rBeindigd door positie\n\r");