s-shape control
Dependents: wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_blue
proto01.cpp
- Committer:
- ec30109b
- Date:
- 2019-09-23
- Revision:
- 6:16a24200ab68
- Parent:
- 5:8dce5092a719
- Child:
- 7:c9707baaf007
File content as of revision 6:16a24200ab68:
#include "proto01.h" Proto1::Proto1(double accdis_, double decdis_, double maxspeed_, double s_vector_) { accdis = accdis_; accdis2 = accdis_; decdis = decdis_; decdis2 = decdis_; maxspeed = maxspeed_; maxspeed2 = maxspeed_; s_vector = s_vector_; } void Proto1::target(double target_, double start_) { targetDis = target_; start = start_; if (targetDis > accdis+decdis) { maxspeed = maxspeed2; accdis = accdis2; decdis = decdis2; consdis = targetDis - accdis - decdis; } else { maxspeed = maxspeed2 / 2.0; accdis = targetDis / 2.0; decdis = targetDis / 2.0; consdis = 0.0; } /*秘伝のコード*/ /*いじるな*/ accsec = 2.0 / maxspeed * accdis; decsec = 2.0 / maxspeed * decdis; } void Proto1::targetXY(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) { maxspeed = maxspeed2; accdis = accdis2; decdis = decdis2; consdis = targetDis - accdis - decdis; } else { maxspeed = maxspeed2 / 2.0; accdis = targetDis / 2.0; decdis = targetDis / 2.0; consdis = 0.0; } /*秘伝のコード*/ /*いじるな*/ accsec = 2.0 / maxspeed * accdis; decsec = 2.0 / maxspeed * decdis; } void Proto1::Input_now(double now_) { now = now_; nowDis = now - start; } void Proto1::Input_nowXY(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)); } void Proto1::calculate() { if (nowDis < accdis) { counter = sqrt(2.0*accsec/maxspeed*fabs(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+consdis) { vector = maxspeed; } else if (nowDis > (accdis+consdis)) { 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() { return vector; } double Proto1::getvalue_x() { return vector * cos(targetRad); } double Proto1::getvalue_y() { return vector * sin(targetRad); }