Revision 32:5ae627e1bce8, committed 2013-11-04

Comitter:

Socrates

Date:

Mon Nov 04 09:27:22 2013 +0000

Parent:

31:5c90e931dbfe

Child:

33:c495e9d8ea1f

Commit message:

Deze doet wel iets.

Changed in this revision

--- a/main.cpp	Mon Nov 04 08:15:19 2013 +0000
+++ b/main.cpp	Mon Nov 04 09:27:22 2013 +0000
@@ -100,13 +100,13 @@
     kpc=1.0*0.1545;
     kdc=0.0*2.8*pow(10.0,-3.0);
     kic=0.1*1.0;
-    kp=0.1*0.1545;
-    kd=0.1*2.8*pow(10.0,-3.0);
-    ki=0.1*1.0;
+    kp=1*0.1545;
+    kd=1.0*2.8*pow(10.0,-3.0);
+    ki=1.0*1.0;
     rt=0.032805;
     gain=4.0;
     emggrens=0.35;
-    friction=0.4;
+    friction=0.45;
     Ai1=0;
     Ad1=0;
     Bi1=0;
@@ -185,7 +185,7 @@
         }
 
         while(calA==true) {
-            ticka=-motor1.getPosition();  //NOTE: deze moet volgens mij gewoon weer positief zijn.
+            ticka=-1*motor1.getPosition();  //NOTE: deze moet volgens mij gewoon weer positief zijn.
             errA=refA-ticka;
             Ap=errA*kpc;
             Ad=(errA-Ad1)*kdc/ts;
@@ -295,13 +295,13 @@
         zy=zy*(1.0/(1.0-emggrens));
 
 //Richting omdraaien met triceps.
-        if (ztr>0.1 && dirflagx == true) {
+        if (ztr>0.15 && dirflagx == true) {
             dirflagx = false;
             xdir ^= 1;
             //zx=0;  //NOTE: deze weghalen kan schelen?
             dirtimeout.attach(tricheck,1.0);
         }
-        if (ztl>0.1 && dirflagy == true) {
+        if (ztl>0.15 && dirflagy == true) {
             dirflagy = false;
             ydir ^= 1;
             //zy=0;
@@ -312,7 +312,7 @@
         if (ydir==0) {
             zy=-1.0*zy;
         }
-        if (xdir==1) { //NOTE: moet dit geen 0 zijn?
+        if (xdir==0) { //NOTE: moet dit geen 0 zijn?
             zx=-1.0*zx;
         }
 
@@ -320,13 +320,15 @@
         tickb=motor2.getPosition();
 
         //Begrenzing.
-        vxuit=zx*4.0*pow(10.0,-1.0); // 4cm/s //NOTE: deze shizzle kan er misschien uit. In ieder geval de vier. En anders moet het /1000 zijn.
-        vyuit=zy*4.0*pow(10.0,-2.0); // 4cm/s
+        vxuit=zx*20.0/500.0; //NOTE: deze shizzle kan er misschien uit. In ieder geval de vier. En anders moet het /1000 zijn.
+        vyuit=zy*20.0/500.0; 
         xuit += ts*vxuit;
         yuit += ts*vyuit;
 
-        keep_in_range(&xuit,-4.5,2.75); //NOTE: Iets met de grenzen. En de richtingpins.
-        keep_in_range(&yuit,-4.5,2.8); //NOTE: als bovenstaande weer klopt moeten de grenzen voor x zijn: .125 en 0.422. Voor y is dat 0.125 en 0.335.
+        //keep_in_range(&xuit,-4.5,2.75); //NOTE: Iets met de grenzen. En de richtingpins.
+        //keep_in_range(&yuit,-4.5,2.8); //NOTE: als bovenstaande weer klopt moeten de grenzen voor x zijn: .125 en 0.422. Voor y is dat 0.125 en 0.335.
+        keep_in_range(&xuit,0.125,0.422);
+        keep_in_range(&yuit,0.125,0.335);
 
         refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
         refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt);
@@ -376,8 +378,8 @@
             for_A=abs(for_A)+friction;
             for_B=abs(for_B)+friction;
 
-            keep_in_range(&for_A, 0,1.0); //NOTE: misschien valt hier nog wat te begrenzen, maar dan wel op ondergrens friction.
-            keep_in_range(&for_B, 0,1.0);
+            keep_in_range(&for_A, friction,1.0); //NOTE: misschien valt hier nog wat te begrenzen, maar dan wel op ondergrens friction.
+            keep_in_range(&for_B, friction,1.0);
         }
         motordirA.write(Adir);
         motordirB.write(Bdir);
@@ -386,8 +388,9 @@
 
         if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) {
             //pc.printf(" %f %f \n\r",zx, zy);
+            //pc.printf(" %f %f %f %f %f \n\r",zbl,ztl,zbr,ztr,kbl);
             //pc.printf(" %f %i %i %f %i %i\n\r",for_A,ticka,refA,for_B,tickb, refB);
-            pc.printf(" %i %i %i %i %f %f \n\r", ticka,tickb,refA,refB,xuit,yuit);
+            pc.printf(" %i %i %i %i %f %f \n\r", ticka,tickb,refA,refB,xuit*1000.0,yuit*1000.0);
             //pc.printf("A %i %i B %i %i\n\r",Adir, ticka,Bdir, tickb);
             //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit);
             //pc.printf(" %i %i %i %f\n\r",tickb,refB,errB,for_B);