Revision 4:c21f68307740, committed 2015-12-15

Comitter:

DeguNaoto

Date:

Tue Dec 15 08:57:37 2015 +0000

Parent:

3:639c3c990c9b

Commit message:

?????????

Changed in this revision

--- a/main.cpp	Tue Dec 15 08:15:58 2015 +0000
+++ b/main.cpp	Tue Dec 15 08:57:37 2015 +0000
@@ -11,22 +11,23 @@
 #define Adjust 0.2
 //"s" is sita ,"d" is dist
 #define RATE 0.01
-#define Kpd 0.5
-#define Kid 0.05
+#define Kpd 0.01
+#define Kid 0.015
 #define Kdd 0.01
 #define Kps 1.0
-#define Kis 0.0
-#define Kds 0.0
-float L,sita,targ_sita,sita_diff,dist,targ_dist,dist_diff,Ps,Is,Ds,Us,Pd,Id,Dd,Ud;
+#define Kis 0.001
+#define Kds 0.1
+float L,sita,targ_sita,sita_diff,sita_prefdiff,dist,targ_dist,dist_diff,dist_prefdiff,Ps,Is,Ds,Us,Pd,Id,Dd,Ud;
 
 void compute_d(){
     dist_diff = targ_dist - dist;
     Pd        = Kpd * dist_diff;          //proportionaly
     Id       += Kid * dist_diff * RATE;   //integral
-    Dd        = Kdd * (dist_diff / RATE); //differential
+    Dd        = Kdd * ((dist_diff - dist_prefdiff) / RATE); //differential
     Ud        = Pd + Id + Dd;
     if( Ud > 1.0) Ud = 1.0;
     else if( Ud < -1.0) Ud = -1.0;
+    dist_prefdiff = dist_diff;
 }
 
 void compute_s(){
@@ -36,10 +37,11 @@
     sita_diff = targ_sita - sita;
     Ps        = Kps * sita_diff;          //proportionaly
     Is       += Kis * sita_diff * RATE;   //integral
-    Ds        = Kds * (sita_diff / RATE); //differential
+    Ds        = Kds * ((sita_diff - sita_prefdiff) / RATE); //differential
     Us        = Ps + Is + Ds;
     if( Us > 1.0) Us = 1.0;
     else if( Us < -1.0) Us = -1.0;
+    sita_prefdiff = sita_diff;
     serve.speed(Us);
 }
 
@@ -48,11 +50,8 @@
     Com_s.attach(&compute_s,RATE);
     Com_d.attach(&compute_d,RATE);
     while(1) {
-        /*if(arg / 0.4 - 0.5 > 1.0) sita = 1.0;
-        else if(arg / 0.4 - 0.5 < -1.0) sita = -1.0;
-        else sita = arg / 0.4 - 0.5;*/
-        targ_sita = Ud;
-//        targ_sita = pot;
+//        targ_sita = Ud;
+        targ_sita = pot;
         dist = Adjust * (1.0 / distant) < 1.0 ? Adjust * (1.0 / distant) : 1.0;
         pc.printf("dist:%f, sita:%f\r\n",dist,sita);
     }