Diff: main.cpp

Revision:

2:9dc7b40286d1

Parent:

1:95dd5c626960

Child:

3:123f3fd0daf6

--- a/main.cpp	Thu Jun 12 07:16:37 2014 +0000
+++ b/main.cpp	Thu Jun 12 08:44:25 2014 +0000
@@ -1,10 +1,12 @@
 #include "mbed.h"
 #include "encoder.h"
+#include "MODSERIAL.h"
 #include <iostream>
 #include <fstream>
 using namespace std;
+#include "time.h"
 
-Serial pc(USBTX,USBRX);
+MODSERIAL pc(USBTX,USBRX);
 Encoder Enc(PTA1,PTA2);
 //Encoder Encinv(PTA0,PTD0);
 PwmOut motorplus(PTD3);
@@ -12,13 +14,13 @@
 
 double rpm, pos, pos1, t, Ref, Error, deliver;
 double Prop, Kp, Int, Int1, Ki, Ctrl;
-volatile double pwmotor;
-double tick, speed, move;
-volatile bool looptimerflag, calflag, scriptflag, loadingflag;
+double pwmotor;
+double speed, move;
+volatile bool looptimerflag, calflag, scriptflag;
 double Move[25];
 double Tijd[9];
 double Referentie[9];
-int n;
+int n,tr;
 
 void setlooptimerflag(void)
 {
@@ -28,10 +30,18 @@
 {
     scriptflag = true;
 }
+void setcalflag(void)
+{
+    calflag = true;
+}
 void printdata(void)
 {
     pc.printf("Speed %f Pos %f\n\r",speed, pos);
 }
+void printdata2(void)
+{
+    pc.printf("Ref %f T %f\n\r",Ref,t);
+}
 
 int main()
 {
@@ -39,46 +49,17 @@
     wait(1);
     pc.printf("start\n\r");
     wait(1);
-    loadingflag = true;
     //Inladen data. Courtesy of http://runnable.com/UpSpP5ymsslIAAFo/reading-a-file-into-an-array-for-c%2B%2B-and-fstream
 
-    /*
-        while(loadingflag == true) {
-            int array_size = 2847;
-            char * array = new char[array_size];
-            int index = 0;
-            ifstream file("/local/m3l5xascii.txt");
-            if(file.is_open()) {
-                cout << "Bestand geopend. Laden...";
-                while(!file.eof() && index < array_size) {
-                    file.get(array[index]);
-                    index++;
-                }
-                array[index-1] = '\0';
-                cout << "De data tot nu" << endl;
-                for(int i = 0; array[i] != '\0'; i++) {
-                    cout <<array[i];
-                }
-            } else {
-                cout << "Bestand kan niet worden geopend" << endl;
-            }
-            loadingflag = false;
-            cout << array;
-            return 0;
-        }
-        */
-
-    wait(1);
     //Nu gaan we weer verder met het normale programma
     Ticker looptimer;
     Ticker printer;
     Ticker script;
-    const double ts=(1.0/5000.0);
-    const double tsd=(1.0/5.0);
-    //20 kHz kan hij nog aan, in een simpel script.
+    Ticker calibratie;
+    double ts=(1.0/5000.0);
+    double tsd=(1.0/5.0);
     motorplus.period(1.0/60000.0);
     motormin.period(1.0/60000.0);
-    //Die moet duidelijk gefilterd worden.
 
     Kp = 0.01;
     Ki = 0.01;
@@ -86,7 +67,10 @@
     deliver=0.01*pwmotor;
     pos1=0;
     n=0;
+    t=0;
     Move[24]=pwmotor*-100.0;
+    pos=Enc.getPosition();
+    speed=Enc.getSpeed();
 
     Referentie[0]=20.0;
     Referentie[1]=17.0;
@@ -108,18 +92,17 @@
     Tijd[7]=23.0;
     Tijd[8]=25.0;
 
-    printer.attach(printdata,tsd);
-    script.attach(setscriptflag, ts);
-    pos=Enc.getPosition();
-    speed=Enc.getSpeed();
-    //pc.printf("start2");
+    wait(1);
+    pc.printf("start2\n\r");
     wait(1);
-    calflag = true;
 
+    scriptflag = true;
+    printer.attach(printdata,tsd);
+    //script.attach(setscriptflag, ts);
+    calibratie.attach(setcalflag,ts);
     while(scriptflag == true) {
-        scriptflag = false;
-        wait(3);
         while(calflag == true) {
+            calflag=false;
             if (abs(deliver) <= abs(pwmotor)) {
                 deliver=deliver*1.01;
             }
@@ -135,53 +118,64 @@
                 n=0;
             }
             move=Move[24]-Move[0];
-            pc.printf("pwm %f move %f\n\r",deliver,move);
-            if(Move[24] == 0.0 && Move[0] == 0.0 && abs(pos) > 3) {
-                motorplus.write(0.0);
-                motormin.write(0.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;
-                pc.printf("Einde");
-                script.detach();
+                scriptflag=false;
                 wait(1);
             }
         }
     }
     Enc.setPosition(0.0);
+    pwmotor=0.0;
+    deliver=0.01*pwmotor;
+    pc.printf("Begin loop\n\r");
+    wait(1);
+    //printer.attach(printdata2,tsd);
     looptimer.attach(setlooptimerflag,ts);
-    pwmotor=0.0;
-    while(looptimerflag == false);
-    {
-        motorplus.write(0.5+deliver);
-        motormin.write(0.5-deliver);
-    }
-    looptimerflag = false;
-
+    while(1) {
+        while (looptimerflag != true) {
+            tr = clock();
+            looptimerflag = false;
+            pc.printf("true\n\r");
 //Encoder
-    tick=Enc.getPosition();
-    speed=Enc.getSpeed();
-    pos=tick/1024;
+            pos=Enc.getPosition()/1024;
+            speed=Enc.getSpeed();
 
-    if (t==Tijd[n]) {
-        Ref=Referentie[n];
-        n +=1;
-    }
-    t += ts;
+            if (t == Tijd[n]) {
+                Ref=Referentie[n];
+                n +=1;
+            }
+            t = t + ts;
 
 //Regelaar
-    Error=Ref-pos;
-    Prop=Error*Kp;
-    Int=(Int1+ts*Error)*Ki;
-    Ctrl=Prop+Int;
+            Error=Ref-pos;
+            Prop=Error*Kp;
+            Int=(Int1+ts*Error)*Ki;
+            Ctrl=Prop+Int;
 
-    pwmotor=Ctrl;
-    if (abs(deliver) <= abs(pwmotor)) {
-        deliver=deliver*1.01;
+            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);
+        }
     }
-    if (abs(deliver) >= abs(pwmotor)) {
-        deliver=deliver*0.99;
-    }
-    motorplus.write(0.5+deliver);
-    motormin.write(0.5-deliver);
 }
\ No newline at end of file