Akashi_2021_Bteam / Mbed 2 deprecated pathtracking

keiro tuiju-

Dependencies:   mbed arrc_mbed

main.cpp

Committer:
hamohamo
Date:
2021-04-14
Revision:
0:fb188a482b84

File content as of revision 0:fb188a482b84:

#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;
}