s-shape control
Dependents: wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_blue
Diff: proto01.cpp
- Revision:
- 4:40c5ac00bb5b
- Parent:
- 3:c46f084010aa
- Child:
- 5:8dce5092a719
diff -r c46f084010aa -r 40c5ac00bb5b proto01.cpp --- a/proto01.cpp Thu Aug 22 14:07:24 2019 +0000 +++ b/proto01.cpp Mon Aug 26 06:10:05 2019 +0000 @@ -2,48 +2,80 @@ 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; + 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 /= 2; + accdis = targetDis / 2; + decdis = targetDis / 2; + consdis = 0; + } + /*秘伝のコード*/ /*いじるな*/ + accsec = 2.0 / maxspeed * accdis; + decsec = 2.0 / maxspeed * 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_; + startx = startx_; + starty = starty_; + targetx = targetx_; + targety = targety_; if (targetDis > accdis+decdis) { + maxspeed = maxspeed2; accdis = accdis2; decdis = decdis2; - middledis = targetDis - accdis - decdis; + consdis = targetDis - accdis - decdis; } else { maxspeed /= 2; accdis = targetDis / 2; decdis = targetDis / 2; - middledis = 0; + consdis = 0; } + /*秘伝のコード*/ /*いじるな*/ accsec = 2.0 / maxspeed * accdis; decsec = 2.0 / maxspeed * decdis; } -void Proto1::calculate(int now_x, int now_y) +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)); + 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*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) { + } else if (nowDis >= accdis && nowDis < accdis+consdis) { vector = maxspeed; - } else if (nowDis > (accdis+middledis)) { + } 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 { @@ -51,6 +83,11 @@ } } +double Proto1::getvalue() +{ + return vector; +} + double Proto1::getvalue_x() { return vector * cos(targetRad);