s-shape control
Dependents: wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_blue
proto01.cpp
- Committer:
- piroro4560
- Date:
- 2019-08-22
- Revision:
- 3:c46f084010aa
- Parent:
- prototype01.cpp@ 1:ad711893a3a7
- Child:
- 4:40c5ac00bb5b
File content as of revision 3:c46f084010aa:
#include "proto01.h" Proto1::Proto1(double accdis_, double decdis_, double maxspeed_, double s_vector_) { accdis = accdis_; decdis = decdis_; maxspeed = maxspeed_; s_vector = s_vector_; accdis2 = accdis; decdis2 = decdis; } void Proto1::target_xy(int targetx_, int targety_, int startx_, int starty_) { targetDis = hypot((float)(startx_-targetx_), (float)(starty_-targety_)); startx = startx_; starty = starty_; targetx = targetx_; targety = targety_; if (targetDis > accdis+decdis) { accdis = accdis2; decdis = decdis2; middledis = targetDis - accdis - decdis; } else { maxspeed /= 2; accdis = targetDis / 2; decdis = targetDis / 2; middledis = 0; } accsec = 2.0 / maxspeed * accdis; decsec = 2.0 / maxspeed * decdis; } void Proto1::calculate(int now_x, int now_y) { nowx = now_x; nowy = now_y; nowDis = hypot((startx-nowx),(starty-nowy)); targetRad = atan2((double)(targety-nowy), (double)(targetx-nowx)); if (nowDis < accdis) { counter = sqrt(2.0*accsec/maxspeed*nowDis); vector = s_vector + (-1 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / ((2.0*PI)/(maxspeed-s_vector)); } else if (nowDis >= accdis && nowDis < accdis+middledis) { vector = maxspeed; } else if (nowDis > (accdis+middledis)) { counter = sqrt(2.0*decsec/maxspeed*fabs(targetDis-nowDis)); vector = (-1 * sin((2.0*PI/decsec)*counter) + (2.0*PI/decsec)*counter) / (2.0*PI/maxspeed); } else { vector = 0; } } double Proto1::getvalue_x() { return vector * cos(targetRad); } double Proto1::getvalue_y() { return vector * sin(targetRad); }