Revision 24:2e5290d8230b, committed 2013-10-30

Comitter:

Socrates

Date:

Wed Oct 30 13:02:45 2013 +0000

Parent:

23:ee89be59ae2f

Child:

25:bfe7c49e76cd

Commit message:

voor victor

Changed in this revision

--- a/main.cpp	Tue Oct 29 14:41:05 2013 +0000
+++ b/main.cpp	Wed Oct 30 13:02:45 2013 +0000
@@ -62,7 +62,7 @@
     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;    xuit=0;       yuit=0;
-    kp=0.1;    ki=0.0001;   kd=0.0001;   rt=0.032805; Ai1=0; Ad1=0;
+    kp=0.1;    ki=0.0000;   kd=0.0000;   rt=0.032805; Ai1=0; Ad1=0;
     Bi1=0;     Bd1=0;     
 
     //High pass, 35Hz, 1e orde
@@ -78,6 +78,14 @@
     //denl1=1;
     denl2=-1.955578240315036;
     denl3=0.956543676511203;
+    //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);
+    //denl1=1;
+    //denl2=-1.982228929792529;
+    //denl3=0.982385450614126;
+
     pc.baud(115200);
 
     while(1) {
@@ -114,8 +122,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.0);
-        zy=(zbl*3.0);
+        zx=(zbr*5.0);
+        zy=(zbl*5.0);
         
         //Grenzen.
         if (zx>1.0) {
@@ -124,25 +132,28 @@
         if (zy>1.0) {
             zy=0.99999;
         }
-        if (zx<0.30){
-        zx=0;
+        if (zx<0.3){
+        zx=0.3;
         }
-        if (zy<0.30){
-        zy=0;
+        if (zy<0.3){
+        zy=0.3;
         }
-        
+        zx=zx-0.3;
+        zx=zx*(1.0/0.7);
+        zy=zy-0.3;
+        zy=zy*(1.0/0.7);
         //Richting omdraaien met triceps. 
         if ((ztr>(zbr+0.1)) && dirflagx == true) {
             dirflagx = false;
             xdir ^= 1;
             zx=0;
-            dirtimeout.attach(tricheck,1.5);
+            dirtimeout.attach(tricheck,1.0);
         }
           if ((ztl>(zbl+0.1)) && dirflagy == true) {
             dirflagy = false;
             ydir ^= 1;
             zy=0;
-            dirtimeout.attach(tricheck,1.5);
+            dirtimeout.attach(tricheck,1.0);
         }
         
         //Motoraansturing.
@@ -156,8 +167,8 @@
         }
         
         ticka=motor1.getPosition(); tickb=motor2.getPosition();
-        vxuit=zx*1.0*pow(1.0,-2.0); // 4cm/s
-        vyuit=zy*1.0*pow(1.0,-2.0); // 4cm/s
+        vxuit=zx*4.0*pow(10.0,-1.0); // 4cm/s
+        vyuit=zy*4.0*pow(10.0,-1.0); // 4cm/s
         xuit += ts*vxuit;
         yuit += ts*vyuit;
         
@@ -182,15 +193,14 @@
         //keep_in_range(&Ai,-0.1,0.1);
         Ad1=Ad;           Ai1=Ai;
         ctrlA=(Ai+Ap+Ad);
-        for_A=(ctrlA)/100.0;
+        for_A=(ctrlA)/10.0;
         
         Bp=errB*kp;          Bd=(errB-Bd1)*kd/ts;        Bi=(Bi1+ts*errB)*ki;
         //keep_in_range(&Bd,-0.1,0.1);
         //keep_in_range(&Bi,-0.1,0.1);
-
         Bd1=Bd;           Bi1=Bi;
         ctrlB=(Bi+Bp+Bd);
-        for_B=(ctrlB)/100.0;
+        for_B=(ctrlB)/10.0;
         //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.  
         
         keep_in_range(&for_A, -1,1);
@@ -201,8 +211,14 @@
         pwm_A.write(abs(for_A));
         pwm_B.write(abs(for_B));
         
+        //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit);
+        //pc.printf(" %i %i %i\n\r",tickb,refB,errB);
         //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit, yuit);
-        pc.printf(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B);
+        //pc.printf("A %i %i %i %i %f %f\n\r",ticka,refA,errA,ctrlA,for_A);
+        pc.printf("B %i %i %i %i %i %f %f\n\r",Bdir,tickb,refB,errB,ctrlB,for_B);
+//Conclusies: tot en met yuit gaat het goed. Kabel A lijkt het beter te doen dan kabel B. Als de EMG niet aangesloten zit is er nog steeds ruis van de
+//encoder.
+
     }
 }