s-shape control
Dependents: wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_blue
Diff: proto01.cpp
- Revision:
- 3:c46f084010aa
- Parent:
- 1:ad711893a3a7
- Child:
- 4:40c5ac00bb5b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/proto01.cpp Thu Aug 22 14:07:24 2019 +0000 @@ -0,0 +1,62 @@ +#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); +} \ No newline at end of file