Diff: main.cpp

Revision:

0:fb188a482b84

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