Revision 23:ee89be59ae2f, committed 2013-10-29

Comitter:

Socrates

Date:

Tue Oct 29 14:41:05 2013 +0000

Parent:

22:a658a3e7b3f4

Child:

24:2e5290d8230b

Commit message:

Code werkt grotendeels, maar rare sprongen bij encoder.

Changed in this revision

--- a/main.cpp	Tue Oct 29 13:44:03 2013 +0000
+++ b/main.cpp	Tue Oct 29 14:41:05 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=1;      ki=0.0;   kd=0.0;   rt=0.032805; Ai1=0;        Ad1=0;
+    kp=0.1;    ki=0.0001;   kd=0.0001;   rt=0.032805; Ai1=0; Ad1=0;
     Bi1=0;     Bd1=0;     
 
     //High pass, 35Hz, 1e orde
@@ -156,13 +156,13 @@
         }
         
         ticka=motor1.getPosition(); tickb=motor2.getPosition();
-        vxuit=zx*1.0*pow(1.0,1.0); // 4cm/s
-        vyuit=zy*1.0*pow(1.0,1.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;
         yuit += ts*vyuit;
         
-        refA=floor(4123.0*atan2(yuit,xuit)/(2.0*PI));
-        refB=floor(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt));
+        refA=(4123.0*atan2(yuit,xuit)/(2.0*PI));
+        refB=(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt));
         errA=refA-ticka;
         errB=refB-tickb;     
         
@@ -178,18 +178,19 @@
 }
         //Controllers
         Ap=errA*kp;          Ad=(errA-Ad1)*kd/ts;        Ai=(Ai1+ts*errA)*ki;
-        keep_in_range(&Ai,-0.1,0.1);
-        keep_in_range(&Ad,-0.1,0.1);
+        //keep_in_range(&Ad,-0.1,0.1);
+        //keep_in_range(&Ai,-0.1,0.1);
         Ad1=Ad;           Ai1=Ai;
         ctrlA=(Ai+Ap+Ad);
-        for_A=(ctrlA)/1000.0;
+        for_A=(ctrlA)/100.0;
         
         Bp=errB*kp;          Bd=(errB-Bd1)*kd/ts;        Bi=(Bi1+ts*errB)*ki;
-        keep_in_range(&Bi,-0.1,0.1);
-        keep_in_range(&Bd,-0.1,0.1);
+        //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)/1000.0;
+        for_B=(ctrlB)/100.0;
         //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.  
         
         keep_in_range(&for_A, -1,1);
@@ -200,11 +201,8 @@
         pwm_A.write(abs(for_A));
         pwm_B.write(abs(for_B));
         
-        //Voor EMG
-        pc.printf(" %f \n\r", zy);
-        
-        //Voor motor
-        //pc.printf(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B);
+        //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);
     }
 }