Diff: main.cpp

Revision:

1:95dd5c626960

Parent:

0:095ff84c3ee9

Child:

2:9dc7b40286d1

--- a/main.cpp	Wed Jun 11 08:22:59 2014 +0000
+++ b/main.cpp	Thu Jun 12 07:16:37 2014 +0000
@@ -1,17 +1,23 @@
 #include "mbed.h"
 #include "encoder.h"
+#include <iostream>
+#include <fstream>
+using namespace std;
 
 Serial pc(USBTX,USBRX);
 Encoder Enc(PTA1,PTA2);
-Encoder Encinv(PTA0,PTD0);
+//Encoder Encinv(PTA0,PTD0);
 PwmOut motorplus(PTD3);
 PwmOut motormin(PTC3);
 
-double rpm, rpmmin,pos,pos1,posinv;
+double rpm, pos, pos1, t, Ref, Error, deliver;
+double Prop, Kp, Int, Int1, Ki, Ctrl;
 volatile double pwmotor;
-double tickinv, tick, speed,move, speedinv;
-volatile bool looptimerflag, calflag, scriptflag;
+double tick, speed, move;
+volatile bool looptimerflag, calflag, scriptflag, loadingflag;
 double Move[25];
+double Tijd[9];
+double Referentie[9];
 int n;
 
 void setlooptimerflag(void)
@@ -29,6 +35,41 @@
 
 int main()
 {
+    pc.baud(115200);
+    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;
@@ -38,24 +79,53 @@
     motorplus.period(1.0/60000.0);
     motormin.period(1.0/60000.0);
     //Die moet duidelijk gefilterd worden.
-    pc.baud(115200);
 
-    pwmotor=0.3/2.0;
+    Kp = 0.01;
+    Ki = 0.01;
+    pwmotor=-0.5/2.0;
+    deliver=0.01*pwmotor;
     pos1=0;
     n=0;
-    Move[24]=-10;
-    calflag = true;
+    Move[24]=pwmotor*-100.0;
+
+    Referentie[0]=20.0;
+    Referentie[1]=17.0;
+    Referentie[2]=14.0;
+    Referentie[3]=20.0;
+    Referentie[4]=14.0;
+    Referentie[5]=20.0;
+    Referentie[6]=14.0;
+    Referentie[7]=29.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;
 
     printer.attach(printdata,tsd);
-    wait(tsd);
+    script.attach(setscriptflag, ts);
+    pos=Enc.getPosition();
+    speed=Enc.getSpeed();
+    //pc.printf("start2");
+    wait(1);
+    calflag = true;
 
-    script.attach(setscriptflag, ts);
-    while(scripttflag == true) {
+    while(scriptflag == true) {
         scriptflag = false;
         wait(3);
         while(calflag == true) {
-            motorplus.write(0.5+pwmotor);
-            motormin.write(0.5-pwmotor);
+            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;
@@ -65,45 +135,53 @@
                 n=0;
             }
             move=Move[24]-Move[0];
-            if(Move[24] == 0.0 && Move[0] == 0.0 && abs(pos) > 10) {
+            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);
-                pwmotor = pwmotor*-1;
+                //pwmotor = pwmotor*-1.0;
+                //deliver=0.01*pwmotor;
+                calflag=false;
+                pc.printf("Einde");
+                script.detach();
+                wait(1);
             }
         }
-        script.detach();
     }
-
+    Enc.setPosition(0.0);
     looptimer.attach(setlooptimerflag,ts);
+    pwmotor=0.0;
     while(looptimerflag == false);
     {
-        motorplus.write(0.5+pwmotor);
-        motormin.write(0.5-pwmotor);
+        motorplus.write(0.5+deliver);
+        motormin.write(0.5-deliver);
     }
     looptimerflag = false;
 
-    //Encoder
+//Encoder
     tick=Enc.getPosition();
-    //tickinv=Encinv.getPosition();
     speed=Enc.getSpeed();
-    //speedinv=Encinv.getSpeed();
     pos=tick/1024;
-    //posinv=tickinv/1024;
 
-    t += ts;
     if (t==Tijd[n]) {
         Ref=Referentie[n];
         n +=1;
     }
+    t += ts;
 
-    //Regelaar
+//Regelaar
     Error=Ref-pos;
     Prop=Error*Kp;
     Int=(Int1+ts*Error)*Ki;
     Ctrl=Prop+Int;
+
     pwmotor=Ctrl;
-
-    motorplus.write(0.5+pwmotor);
-    motormin.write(0.5-pwmotor);
-    //pc.printf("RPM %f Y %f\n\r",rpm, y);
+    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);
 }
\ No newline at end of file