Akashi_2021_Bteam / Mbed 2 deprecated pathtracking

keiro tuiju-

Dependencies:   mbed arrc_mbed

main.cpp@0:fb188a482b84, 2021-04-14 (annotated)

Committer:
hamohamo
Date:
Wed Apr 14 14:17:27 2021 +0000
Revision:
0:fb188a482b84
pathtracking;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hamohamo 0:fb188a482b84 1 #include <mbed.h>
hamohamo 0:fb188a482b84 2 #include <cmath>
hamohamo 0:fb188a482b84 3
hamohamo 0:fb188a482b84 4 struct pos{
hamohamo 0:fb188a482b84 5 double x;
hamohamo 0:fb188a482b84 6 double y;
hamohamo 0:fb188a482b84 7 }
hamohamo 0:fb188a482b84 8
hamohamo 0:fb188a482b84 9 double getE(pos a,pos b,pos c);
hamohamo 0:fb188a482b84 10 void hogemove(double x,double y);/* 足回りに速度を渡す関数(仮) */
hamohamo 0:fb188a482b84 11
hamohamo 0:fb188a482b84 12 double e;
hamohamo 0:fb188a482b84 13
hamohamo 0:fb188a482b84 14 pos a,b,c;/* after before current */
hamohamo 0:fb188a482b84 15 PID mvx(0.1,0.1,0.1,0.001);
hamohamo 0:fb188a482b84 16 PID mvy(0.1,0.1,0.1,0.001);
hamohamo 0:fb188a482b84 17
hamohamo 0:fb188a482b84 18 double mx,my;
hamohamo 0:fb188a482b84 19 double q3;
hamohamo 0:fb188a482b84 20
hamohamo 0:fb188a482b84 21 int main(){
hamohamo 0:fb188a482b84 22 x,yもらう
hamohamo 0:fb188a482b84 23 e = getE(a,b,c);
hamohamo 0:fb188a482b84 24 printf("%lf\n",e);
hamohamo 0:fb188a482b84 25 q = (a.x-b.x)/(a.y-b.y)*-1;
hamohamo 0:fb188a482b84 26 q = atan2(q);
hamohamo 0:fb188a482b84 27 mvx.Update(0-e*cos(q);
hamohamo 0:fb188a482b84 28 mvy.Update(0-e*sin(q);
hamohamo 0:fb188a482b84 29 mx = mvx.getmv();
hamohamo 0:fb188a482b84 30 my = mvy.getmv();
hamohamo 0:fb188a482b84 31 hogemove(x+mx,y+my);
hamohamo 0:fb188a482b84 32 }
hamohamo 0:fb188a482b84 33
hamohamo 0:fb188a482b84 34 double getE(pos a,pos b,pos c){
hamohamo 0:fb188a482b84 35 double q1,q2,l,e;
hamohamo 0:fb188a482b84 36 q1 = atan2(c.y-b.y,c.x-b.x);
hamohamo 0:fb188a482b84 37 q2 = atan2(a.y-b.y,a.x-b.x);
hamohamo 0:fb188a482b84 38 l = (c.y-b.y)*(c.y-b.y)+(c.x-b.x)*(c.x-b.x)
hamohamo 0:fb188a482b84 39 e = sqrt(l)*sin(q1-q2);
hamohamo 0:fb188a482b84 40 return e;
hamohamo 0:fb188a482b84 41 }
hamohamo 0:fb188a482b84 42
hamohamo 0:fb188a482b84 43
hamohamo 0:fb188a482b84 44