s-shape control
Dependents: wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_blue
Revision 9:4a72ba990137, committed 2019-10-04
- Comitter:
- ec30109b
- Date:
- Fri Oct 04 08:21:13 2019 +0000
- Parent:
- 7:c9707baaf007
- Commit message:
- a
Changed in this revision
proto01.cpp | Show annotated file Show diff for this revision Revisions of this file |
proto01.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r c9707baaf007 -r 4a72ba990137 proto01.cpp --- a/proto01.cpp Mon Sep 23 23:28:45 2019 +0000 +++ b/proto01.cpp Fri Oct 04 08:21:13 2019 +0000 @@ -1,16 +1,16 @@ #include "proto01.h" - + Proto1::Proto1(double accdis_, double decdis_, double maxspeed_, double s_vector_) { accdis = accdis_; - accdis2 = accdis_; + accdis2 = accdis; decdis = decdis_; - decdis2 = decdis_; + decdis2 = decdis; maxspeed = maxspeed_; - maxspeed2 = maxspeed_; + maxspeed2 = maxspeed; s_vector = s_vector_; } - + void Proto1::target(double target_, double start_) { targetDis = target_; @@ -22,15 +22,15 @@ consdis = targetDis - accdis - decdis; } else { maxspeed = maxspeed2 / 2.0; - accdis = targetDis / 2.0; - decdis = targetDis / 2.0; - consdis = 0.0; + accdis = targetDis / 2; + decdis = targetDis / 2; + consdis = 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_)); @@ -44,22 +44,22 @@ decdis = decdis2; consdis = targetDis - accdis - decdis; } else { - maxspeed = maxspeed2 / 2.0; - accdis = targetDis / 2.0; - decdis = targetDis / 2.0; - consdis = 0.0; + maxspeed = maxspeed / 2.0; + accdis = targetDis / 2; + decdis = targetDis / 2; + consdis = 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; @@ -67,11 +67,11 @@ 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)); + 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+consdis) { vector = maxspeed; @@ -82,23 +82,18 @@ vector = 0; } } - + double Proto1::getvalue() { return vector; } - + double Proto1::getvalue_x() { return vector * cos(targetRad); } - + double Proto1::getvalue_y() { return vector * sin(targetRad); -} - -double Proto1::getTheta() -{ - return targetRad; } \ No newline at end of file
diff -r c9707baaf007 -r 4a72ba990137 proto01.h --- a/proto01.h Mon Sep 23 23:28:45 2019 +0000 +++ b/proto01.h Fri Oct 04 08:21:13 2019 +0000 @@ -1,15 +1,15 @@ #ifndef PROTO01_H #define PROTO01_H #define PI 3.141592653589793238462643383219502884 - + #include "mbed.h" - + /** * S字制御クラス */ class Proto1 { public: - + /** * コンストラクタ * @param accdis_ 加速距離 @@ -17,14 +17,14 @@ * @param maxspeed_ 最大速度(?) */ Proto1(double accdis_, double decdis_, double maxspeed_, double s_vector_); - + /** * 目標点を設定する * @param target_ 目標点 * @param start_ 開始地点 */ void target(double target_, double start_); - + /** * xy座標で目標点を設定する * @param targetx_ 目標点のx座標 @@ -33,47 +33,40 @@ * @param starty_ 開始地点のy座標 */ void targetXY(int targetx_, int targety_, int startx_, int starty_); - + /** * 現在地点を入力 * @param now_ 現在地点 */ void Input_now(double now_); - + /** * 現在地点を入力 * @param now_x 現在地点のx座標 * @param now_y 現在地点のy座標 */ void Input_nowXY(int now_x, int now_y); - + /** * S字に変化する速度を計算 */ void calculate(); - + /** * x方向のベクトルを返す */ double getvalue_x(); - + /** * y方向のベクトルを返す */ double getvalue_y(); - + /** * そのまま返す */ double getvalue(); - - /** - * 角度を返す - */ - double getTheta(); - - - + double accdis; double decdis; double accdis2; @@ -82,25 +75,26 @@ double decsec; double maxspeed; double maxspeed2; - + double s_vector; double vector; - + double startx; double starty; double start; double nowx; double nowy; double now; - + double targetDis; double targetRad; double targetx; double targety; double nowDis; + //consdis : 一定速度で走るところ double consdis; double counter; - + }; #endif \ No newline at end of file