Diff: main.cpp

Revision:

0:095ff84c3ee9

Child:

1:95dd5c626960

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 11 08:22:59 2014 +0000
@@ -0,0 +1,109 @@
+#include "mbed.h"
+#include "encoder.h"
+
+Serial pc(USBTX,USBRX);
+Encoder Enc(PTA1,PTA2);
+Encoder Encinv(PTA0,PTD0);
+PwmOut motorplus(PTD3);
+PwmOut motormin(PTC3);
+
+double rpm, rpmmin,pos,pos1,posinv;
+volatile double pwmotor;
+double tickinv, tick, speed,move, speedinv;
+volatile bool looptimerflag, calflag, scriptflag;
+double Move[25];
+int n;
+
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
+void setscriptflag(void)
+{
+    scriptflag = true;
+}
+void printdata(void)
+{
+    pc.printf("Speed %f Pos %f\n\r",speed, pos);
+}
+
+int main()
+{
+    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.
+    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;
+    pos1=0;
+    n=0;
+    Move[24]=-10;
+    calflag = true;
+
+    printer.attach(printdata,tsd);
+    wait(tsd);
+
+    script.attach(setscriptflag, ts);
+    while(scripttflag == true) {
+        scriptflag = false;
+        wait(3);
+        while(calflag == true) {
+            motorplus.write(0.5+pwmotor);
+            motormin.write(0.5-pwmotor);
+            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];
+            if(Move[24] == 0.0 && Move[0] == 0.0 && abs(pos) > 10) {
+                motorplus.write(0.0);
+                motormin.write(0.0);
+                pwmotor = pwmotor*-1;
+            }
+        }
+        script.detach();
+    }
+
+    looptimer.attach(setlooptimerflag,ts);
+    while(looptimerflag == false);
+    {
+        motorplus.write(0.5+pwmotor);
+        motormin.write(0.5-pwmotor);
+    }
+    looptimerflag = false;
+
+    //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;
+    }
+
+    //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);
+}
\ No newline at end of file