Diff: main.cpp

Revision:

3:123f3fd0daf6

Parent:

2:9dc7b40286d1

Child:

4:7052ee491e12

--- a/main.cpp	Thu Jun 12 08:44:25 2014 +0000
+++ b/main.cpp	Thu Jun 12 10:10:37 2014 +0000
@@ -1,14 +1,10 @@
 #include "mbed.h"
 #include "encoder.h"
 #include "MODSERIAL.h"
-#include <iostream>
-#include <fstream>
-using namespace std;
 #include "time.h"
 
 MODSERIAL pc(USBTX,USBRX);
 Encoder Enc(PTA1,PTA2);
-//Encoder Encinv(PTA0,PTD0);
 PwmOut motorplus(PTD3);
 PwmOut motormin(PTC3);
 
@@ -20,7 +16,7 @@
 double Move[25];
 double Tijd[9];
 double Referentie[9];
-int n,tr;
+int n;
 
 void setlooptimerflag(void)
 {
@@ -36,11 +32,15 @@
 }
 void printdata(void)
 {
-    pc.printf("Speed %f Pos %f\n\r",speed, pos);
+    pc.printf("Speed %f Pos %f del %f\n\r",speed, pos, deliver);
 }
 void printdata2(void)
 {
-    pc.printf("Ref %f T %f\n\r",Ref,t);
+    pc.printf("Del %f Ctrl %f\n\r",deliver,Ctrl);
+}
+void printdata3(void)
+{
+    pc.printf("P %f R %f\n\r",pos, Ref);
 }
 
 int main()
@@ -61,9 +61,9 @@
     motorplus.period(1.0/60000.0);
     motormin.period(1.0/60000.0);
 
-    Kp = 0.01;
-    Ki = 0.01;
-    pwmotor=-0.5/2.0;
+    Kp = 0.5;
+    Ki = 0.4;
+    pwmotor=-0.9/2.0;
     deliver=0.01*pwmotor;
     pos1=0;
     n=0;
@@ -76,10 +76,10 @@
     Referentie[1]=17.0;
     Referentie[2]=14.0;
     Referentie[3]=20.0;
-    Referentie[4]=14.0;
+    Referentie[4]=16.0;
     Referentie[5]=20.0;
     Referentie[6]=14.0;
-    Referentie[7]=29.0;
+    Referentie[7]=25.0;
     Referentie[8]=23.0;
 
     Tijd[0]=0;
@@ -135,47 +135,57 @@
     }
     Enc.setPosition(0.0);
     pwmotor=0.0;
-    deliver=0.01*pwmotor;
+    deliver=0.01;
     pc.printf("Begin loop\n\r");
     wait(1);
-    //printer.attach(printdata2,tsd);
+    printer.attach(printdata3,tsd);
     looptimer.attach(setlooptimerflag,ts);
+    Ref=Referentie[0];
     while(1) {
-        while (looptimerflag != true) {
-            tr = clock();
-            looptimerflag = false;
-            pc.printf("true\n\r");
+        while (looptimerflag != true) {}
+        looptimerflag = false;
 //Encoder
-            pos=Enc.getPosition()/1024;
-            speed=Enc.getSpeed();
+        pos=Enc.getPosition()/1024;
+        speed=Enc.getSpeed();
 
-            if (t == Tijd[n]) {
-                Ref=Referentie[n];
-                n +=1;
+        if (t >= Tijd[n]) {
+            Ref=Referentie[n];
+            n +=1;
+            if (n >=7) {
+                n=7;
             }
-            t = t + ts;
+        }
+          t = t + ts;
 
 //Regelaar
-            Error=Ref-pos;
-            Prop=Error*Kp;
-            Int=(Int1+ts*Error)*Ki;
-            Ctrl=Prop+Int;
+        Error=Ref-pos;
+        Prop=Error*Kp;
+        Int1=Int;
+        Int=(Int1+0.001*ts*Error)*Ki;
+        Ctrl=Prop+Int;
 
-            pwmotor=Ctrl;
-            if (abs(deliver) <= abs(pwmotor)) {
-                deliver=deliver*1.01;
-            }
-            if (abs(deliver) >= abs(pwmotor)) {
-                deliver=deliver*0.99;
-            }
-            motorplus.write(0.5+deliver);
-            motormin.write(0.5-deliver);
-            pc.printf("true2\n\r");
-            //wait(1);
-            //cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " seconds." << endl;
-            //cout << double( clock() - startTime ) << " seconds." << endl;
-            tr =clock()-tr;
-            pc.printf("cyc %d time %f \n\r",tr,((float)tr)/CLOCKS_PER_SEC);
+        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");
+            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");
+            return 0;
         }
     }
 }
\ No newline at end of file