Revision 20:013e9c00e058, committed 2013-10-29

Comitter:

Socrates

Date:

Tue Oct 29 11:11:19 2013 +0000

Parent:

19:09c4b5249cec

Child:

21:659f7c8ed328

Commit message:

90 %;

Changed in this revision

--- a/main.cpp	Mon Oct 28 16:09:13 2013 +0000
+++ b/main.cpp	Tue Oct 29 11:11:19 2013 +0000
@@ -2,26 +2,24 @@
 #include "MODSERIAL.h"
 #include "encoder.h"
 #define PI 3.14159265358979323
-
+//XenY
 //Een pwm van 0.05 is net genoeg om de heugel te bewegen. linksom bewegen is negatief voor de encoder. getposition gaat in ticks. 
 //4123 ticks is een rondje. 
-
-//XenY
+//Rechts is x, links is y
 AnalogIn emgtr(PTB3);
 AnalogIn emgbr(PTB2);
-//Rechts is x, links is y
 AnalogIn emgtl(PTB1);
 AnalogIn emgbl(PTB0);
-PwmOut pwm_A(PTA12); //Motor A
-PwmOut pwm_B(PTA5);//Motor B
+PwmOut pwm_A(PTA12);
+PwmOut pwm_B(PTA5);
 MODSERIAL pc(USBTX,USBRX);
 DigitalOut motordirA(PTD3);
 DigitalOut motordirB(PTD1);
 Encoder motor1(PTD0,PTC9);
 Encoder motor2(PTD5,PTC8);
+
 void keep_in_range(float * in, float min, float max);
 
-
 volatile bool looptimerflag;
 void setlooptimerflag(void)
 {
@@ -39,8 +37,8 @@
 
 int main()
 {
-pwm_A.period(1.0/22000.0);
-pwm_B.period(1.0/22000.0);
+    pwm_A.period(1.0/22000.0);
+    pwm_B.period(1.0/22000.0);
     Ticker looptimer;
     Timeout dirtimeout;
     const float ts=0.001;
@@ -51,31 +49,30 @@
     float xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
     float xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
     float zx,zy;
-    float gain, xuit, kp, ki, zx1, yuit, zy1, pwmA, pwmB,rt;
-    int xdir;
-    int ydir;
+    float xuit,yuit, rt; 
+    //float zx1, zy1, gain;
+    //float pwmA, pwmB;
+    int xdir, ydir, Adir, Bdir;
     float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
-    float for_A, for_B;
-    int ticka, tickb, refA,refB, errA, errB,
-
+    float for_A, for_B, yuit1, xuit1;
+    int ticka, tickb, refA,refB, errA, errB;
+    float ctrlA;
+    float ctrlB;
+    //Startwaarden
     x1tr=0;    y1tr=0;    z1tr=0;    z2tr=0;    yabs1tr=0;    yabs2tr=0;
     x1br=0;    y1br=0;    z1br=0;    z2br=0;    yabs1br=0;    yabs2br=0;
     x1tl=0;    y1tl=0;    z1tl=0;    z2tl=0;    yabs1tl=0;    yabs2tl=0;
     x1bl=0;    y1bl=0;    z1bl=0;    z2bl=0;    yabs1bl=0;    yabs2bl=0;
-    zx=0;      zy=0;      xdir=0;    ydir=0;
-    kp=0;      ki=1;      zx1=0;     zy1=0;     rt=32.805/1000.0;
+    zx=0;      zy=0;      xdir=0;    ydir=0;    xuit=0;       yuit=0;
+    kp=0.01;    ki=0.01;   kd=0.01;   rt=0.032805;
+    Bi1=0;     Bd1=0;     yuit1=0;   xuit1=0;
 
-
-    //High pass, 35Hz, 1e
+    //High pass, 35Hz, 1e orde
     numh1=0.900575535279376;
     numh2=-0.900575535279376;
     //denh1=1;
     denh2=-0.801151070558751;
-
-    //Low pass, 2 Hz, 2e orde
-    //numl1=0.391302053991682*pow(10.0,-4.0);
-    //numl2=0.782604107983365*pow(10.0,-4.0);
-    //numl3=0.391302053991682*pow(10.0,-4.0);
+    
     //Low pass, 5 Hz, 2e orde.
     numl1=0.241359049041961*pow(10.0,-3.0);
     numl2=0.482718098083923*pow(10.0,-3.0);
@@ -119,8 +116,8 @@
         zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3;
         x1bl=xbl;        y1bl=ybl;        z2bl=z1bl;        z1bl=zbl;        yabs2bl=yabs1bl;        yabs1bl=yabsbl;
 
-        zx=(zbr);
-        zy=(zbl);
+        zx=(zbr*5.0);
+        zy=(zbl*5.0);
         
         //Grenzen.
         if (zx>1.0) {
@@ -129,10 +126,10 @@
         if (zy>1.0) {
             zy=0.99999;
         }
-        if (zx<0.30){
+        if (zx<0.20){
         zx=0;
         }
-        if (zy<0.30){
+        if (zy<0.20){
         zy=0;
         }
         
@@ -140,11 +137,13 @@
         if ((ztr>(zbr+0.1)) && dirflagx == true) {
             dirflagx = false;
             xdir ^= 1;
+            zx=0;
             dirtimeout.attach(tricheck,1.5);
         }
           if ((ztl>(zbl+0.1)) && dirflagy == true) {
             dirflagy = false;
             ydir ^= 1;
+            zy=0;
             dirtimeout.attach(tricheck,1.5);
         }
         
@@ -159,35 +158,49 @@
         }
         
         ticka=motor1.getPosition(); tickb=motor2.getPosition();
-        vxuit=zx*4.0*pow(10.0,-5.0); // 4cm/s
-        vyuit=zy*4.0*pow(10.0,-5.0); // 4cm/s
+        vxuit=zx*1.0*pow(1.0,-2.0); // 4cm/s
+        vyuit=zy*1.0*pow(1.0,-2.0); // 4cm/s
         xuit += ts*vxuit;
-        yuiy += ts*vyuit;
+        yuit += ts*vyuit;
+        yuit1=yuit;
+        
+        refA=floor(4123.0*atan2(yuit,xuit)/(2.0*PI));
+        refB=floor(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt));
+        errA=refA-ticka;
+        errB=refB-tickb;     
         
-        refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
-        refB=4123.0*sqrt(xuit*xuit+yuit*yuit)*(2.0*PI*rt);//nog keer omtrek
-        errA=refA-ticka;
-        errB=refB-tickb;
-
+if(errA<0){
+Adir=0;}
+else{
+Adir=1;
+}
+if(errB<0){
+Bdir=0;}
+else{
+Bdir=1;
+}
         //Controllers
         Ap=errA*kp;          Ad=(errA-Ad1)*kd/ts;        Ai=(Ai1+ts*errA)*ki;
         Ad1=Ad;           Ai1=Ai;
-        for_a=Ai+Ap+Ad;
+        ctrlA=float(Ai+Ap+Ad);
+        for_A=(ctrlA)/1000.0;
+        
         Bp=errB*kp;          Bd=(errB-Bd1)*kd/ts;        Bi=(Bi1+ts*errB)*ki;
         Bd1=Bd;           Bi1=Bi;
-        for_B=Bi+Bp+Bd;
+        ctrlB=float(Bi+Bp+Bd);
+        for_B=(ctrlB)/1000.0;
         //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.  
         
         keep_in_range(&for_A, -1,1);
         keep_in_range(&for_B, -1,1);
 
-        motordirA.write(xdir);
-        motordirB.write(ydir);
+        motordirA.write(Adir);
+        motordirB.write(Bdir);
         pwm_A.write(for_A);
         pwm_B.write(for_B);
         
         //pc.printf("Bi: %f, Tri: %f, Richting: %d \n\r",zbl*3.0,ztl*3.0,ydir);
-        pc.printf(" %f\n\r",zy);
+        pc.printf(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B);
         //pc.printf("b %f t %f xu %f yu %f A %f B %f \n\r",zbl,ztl,xuit, yuit, pwmA,pwmB);
     }
 }