Akashi_2021_Bteam
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
--- /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_ */
--- /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
--- /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;
+}
+
+
+
--- /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