Revision 17:7dd6dc3c7902, committed 2013-10-25

Comitter:

Socrates

Date:

Fri Oct 25 11:04:43 2013 +0000

Parent:

16:414a2397c493

Child:

18:6c0200364678

Commit message:

x en y goed ingesteld. In ieder geval y.

Changed in this revision

--- a/main.cpp	Fri Oct 25 09:44:12 2013 +0000
+++ b/main.cpp	Fri Oct 25 11:04:43 2013 +0000
@@ -5,24 +5,25 @@
 //Rechts is x, links is y
 AnalogIn emgtl(PTB1);
 AnalogIn emgbl(PTB0);
-PwmOut pwm_x(PTA5);
-PwmOut pwm_y(PTA12);
+PwmOut pwm_x(PTA12); //Motor A
+PwmOut pwm_y(PTA5);//Motor B
 MODSERIAL pc(USBTX,USBRX);
-DigitalOut motordir1(PTD3);
-DigitalOut motordir2(PTD1);
+DigitalOut motordirA(PTD3);
+DigitalOut motordirB(PTD1);
 
 volatile bool looptimerflag;
-//hoi
-
 void setlooptimerflag(void)
 {
-    looptimerflag = true;
+looptimerflag = true;
 }
 
-volatile bool dirflag=true;
+volatile bool dirflagx=true;
+volatile bool dirflagy=true;
+
 void tricheck(void)
 {
-dirflag=true; 
+dirflagx=true;
+dirflagy=true;
 }
 
 int main()
@@ -56,12 +57,17 @@
     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);
+    //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;
     //denl1=1;
-    denl2=-1.982228929792529;
-    denl3=0.982385450614126;
+    //denl2=-1.982228929792529;
+    //denl3=0.982385450614126;
+    denl2=-1.911197067426073;
+    denl3=0.914975834801434;
     pc.baud(115200);
 
     while(1) {
@@ -97,15 +103,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*3.5);
-
-        //if (ztl>zbl) {
-        //    zy=ztl*5.0;
-        //    ydir=0;
-        //} else {
-        //    zy=zbl*5.0;
-        //    ydir=1;
-        //}
+        zx=(zbr*4.0);
+        zy=(zbl*4.0);
         
         if (zx>1.0) {
             zx=0.99999;
@@ -114,23 +113,30 @@
             zy=0.99999;
         }
         
-        //if (zx<0.20){
-        //zx=0;
-        //}
+        if (zx<0.20){
+        zx=0;
+        }
         if (zy<0.20){
         zy=0;
         }
         
-        if ((ztr>(zbr+0.1)) && dirflag == true) {
-            dirflag = false;
+        if ((ztr>(zbr+0.1)) && dirflagx == true) {
+            dirflagx = false;
             xdir ^= 1;
+            zx=0;
             dirtimeout.attach(tricheck,1.5);
         }
-        //motordir1.write(xdir);
-        //motordir2.write(ydir);
+          if ((ztl>(zbl+0.1)) && dirflagy == true) {
+            dirflagy = false;
+            ydir ^= 1;
+            zy=0;
+            dirtimeout.attach(tricheck,1.5);
+        }
+        motordirA.write(xdir);
+        motordirB.write(ydir);
         
         pwm_x.write(abs(zx));
         pwm_y.write(abs(zy));
-        pc.printf("EMGRechts: %f, EMGLinks: %f, Richting: %d \n\r",zbr*3.0,ztr*3.0,xdir);
+        pc.printf("Bi: %f, Tri: %f, Richting: %d \n\r",zbl*3.0,ztl*3.0,ydir);
     }
 }
\ No newline at end of file