1年生講習
/
PID_Sample
1年生の講習用プログラム(坂とピンポン玉)
Fork of PID_Sample by
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
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 639c3c990c9b -r c21f68307740 main.cpp --- 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); }