Diff: main.cpp

Revision:

18:6c0200364678

Parent:

17:7dd6dc3c7902

Child:

19:09c4b5249cec

--- a/main.cpp	Fri Oct 25 11:04:43 2013 +0000
+++ b/main.cpp	Fri Oct 25 12:36:02 2013 +0000
@@ -1,16 +1,20 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
+//XenY
 AnalogIn emgtr(PTB3);
 AnalogIn emgbr(PTB2);
 //Rechts is x, links is y
 AnalogIn emgtl(PTB1);
 AnalogIn emgbl(PTB0);
-PwmOut pwm_x(PTA12); //Motor A
-PwmOut pwm_y(PTA5);//Motor B
+PwmOut pwm_A(PTA12); //Motor A
+PwmOut pwm_B(PTA5);//Motor B
 MODSERIAL pc(USBTX,USBRX);
 DigitalOut motordirA(PTD3);
 DigitalOut motordirB(PTD1);
 
+void keep_in_range(float * in, float min, float max);
+
+
 volatile bool looptimerflag;
 void setlooptimerflag(void)
 {
@@ -28,18 +32,19 @@
 
 int main()
 {
-pwm_x.period(1.0/22000.0);
-pwm_y.period(1.0/22000.0);
+pwm_A.period(1.0/22000.0);
+pwm_B.period(1.0/22000.0);
     Ticker looptimer;
     Timeout dirtimeout;
-    const double ts=0.001;
+    const float ts=0.001;
     looptimer.attach(setlooptimerflag,ts);
-    double numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3;
-    double xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr;
-    double xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr;
-    double xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
-    double xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
-    double zx,zy;
+    float numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3;
+    float xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr;
+    float xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr;
+    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 gainb, xuit, kp, ki, zx1, yuit, zy1, pwmA, pwmB,rt;
     int xdir;
     int ydir;
 
@@ -49,6 +54,8 @@
     x1bl=0;    y1bl=0;    z1bl=0;    z2bl=0;    yabs1bl=0;    yabs2bl=0;
     zx=0; zy=0;
     xdir=0; ydir=0;
+    gainb=3.0; kp=0; ki=1; zx1=0; zy1=0; rt=33.0/1000.0;
+
 
     //High pass, 35Hz, 1e
     numh1=0.900575535279376;
@@ -60,14 +67,13 @@
     //numl1=0.391302053991682*pow(10.0,-4.0);
     //numl2=0.782604107983365*pow(10.0,-4.0);
     //numl3=0.391302053991682*pow(10.0,-4.0);
-    numl1=0.000944691843840;
-    numl2=0.001889383687680;
-    numl3=0.000944691843840;
+    //Low pass, 5 Hz, 2e orde.
+    numl1=0.241359049041961*pow(10.0,-3.0);
+    numl2=0.482718098083923*pow(10.0,-3.0);
+    numl3=0.241359049041961*pow(10.0,-3.0);
     //denl1=1;
-    //denl2=-1.982228929792529;
-    //denl3=0.982385450614126;
-    denl2=-1.911197067426073;
-    denl3=0.914975834801434;
+    denl2=-1.955578240315036;
+    denl3=0.956543676511203;
     pc.baud(115200);
 
     while(1) {
@@ -103,8 +109,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*4.0);
-        zy=(zbl*4.0);
+        zx=(zbr*gainb);
+        zy=(zbl*gainb);
         
         if (zx>1.0) {
             zx=0.99999;
@@ -113,10 +119,10 @@
             zy=0.99999;
         }
         
-        if (zx<0.20){
+        if (zx<0.30){
         zx=0;
         }
-        if (zy<0.20){
+        if (zy<0.30){
         zy=0;
         }
         
@@ -132,11 +138,32 @@
             zy=0;
             dirtimeout.attach(tricheck,1.5);
         }
+        xuit=(kp*zx+ki*zx1)/(kp+ki);
+        yuit=(kp*zy+ki*zy1)/(kp+ki);
+        //pwmA=(atan(yuit/(xuit+0.0001)))/(2.0*3.14159265359);
+        //pwmB=(sqrt(xuit*xuit+yuit*yuit))/(2.0*3.14159265359*rt);
+        //keep_in_range(&pwmA, -1,1);
+        //keep_in_range(&pwmB, -1,1);
+
+        zx1=zx;
+        zy1=zy;
+        
+        if (pwmA<0.0002)
+        {
+        pwmA=0;
+        }
         motordirA.write(xdir);
         motordirB.write(ydir);
+        pwm_A.write((yuit*3.0));
+        pwm_B.write((yuit*3.0));
         
-        pwm_x.write(abs(zx));
-        pwm_y.write(abs(zy));
-        pc.printf("Bi: %f, Tri: %f, Richting: %d \n\r",zbl*3.0,ztl*3.0,ydir);
+        //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("b %f t %f xu %f yu %f A %f B %f \n\r",zbl,ztl,xuit, yuit, pwmA,pwmB);
     }
+}
+
+void keep_in_range(float * in, float min, float max)
+{
+    *in > min ? *in < max? : *in = max: *in = min;
 }
\ No newline at end of file