Akashi_2021_Bteam
/
pathtracking
keiro tuiju-
Revision 0:fb188a482b84, committed 2021-04-14
- Comitter:
- hamohamo
- Date:
- Wed Apr 14 14:17:27 2021 +0000
- Commit message:
- pathtracking;
Changed in this revision
diff -r 000000000000 -r fb188a482b84 PID.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.hpp Wed Apr 14 14:17:27 2021 +0000 @@ -0,0 +1,26 @@ +#ifndef IG_PID_HPP_ +#define IG_PID_HPP_ + +class PID{ +public: + PID(double KP,double KI,double KD,double DT):Kp(KP),Ki(KI),Kd(KD),dt(DT){ + + } + void Update(double e){ + PROP = e; + INT += (curr_e + prev_e)*dt/2.0; + DIFF = (curr_e - prev_e)/dt; + mv = Kp*PROP + Ki*INT + Kd*DIFF; + } + double getmv(){ + return mv; + } +private: + double Kp,Ki,Kd; /* Pゲイン Iゲイン Dゲイン */ + double dt; /* 微小時間 */ + double prev_e,curr_e; /* 前回の誤差 現在の誤差 */ + double PROP,INT,DIFF; /* 比例 積分 微分 */ + double mv; /* manipulating variable */ +}; + +#endif /* IG_PID_HPP_ */
diff -r 000000000000 -r fb188a482b84 arrc_mbed.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/arrc_mbed.lib Wed Apr 14 14:17:27 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/TanakaRobo/code/arrc_mbed/#77c13e86ad12
diff -r 000000000000 -r fb188a482b84 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Apr 14 14:17:27 2021 +0000 @@ -0,0 +1,44 @@ +#include <mbed.h> +#include <cmath> + +struct pos{ + double x; + double y; +} + +double getE(pos a,pos b,pos c); +void hogemove(double x,double y);/* 足回りに速度を渡す関数(仮) */ + +double e; + +pos a,b,c;/* after before current */ +PID mvx(0.1,0.1,0.1,0.001); +PID mvy(0.1,0.1,0.1,0.001); + +double mx,my; +double q3; + +int main(){ + x,yもらう + e = getE(a,b,c); + printf("%lf\n",e); + q = (a.x-b.x)/(a.y-b.y)*-1; + q = atan2(q); + mvx.Update(0-e*cos(q); + mvy.Update(0-e*sin(q); + mx = mvx.getmv(); + my = mvy.getmv(); + hogemove(x+mx,y+my); +} + +double getE(pos a,pos b,pos c){ + double q1,q2,l,e; + q1 = atan2(c.y-b.y,c.x-b.x); + q2 = atan2(a.y-b.y,a.x-b.x); + l = (c.y-b.y)*(c.y-b.y)+(c.x-b.x)*(c.x-b.x) + e = sqrt(l)*sin(q1-q2); + return e; +} + + +
diff -r 000000000000 -r fb188a482b84 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Apr 14 14:17:27 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file