s-shape control
Dependents: wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_blue
Diff: proto01.cpp
- Revision:
- 6:16a24200ab68
- Parent:
- 5:8dce5092a719
- Child:
- 7:c9707baaf007
diff -r 8dce5092a719 -r 16a24200ab68 proto01.cpp --- a/proto01.cpp Wed Sep 04 10:01:50 2019 +0000 +++ b/proto01.cpp Mon Sep 23 12:21:36 2019 +0000 @@ -70,25 +70,17 @@ void Proto1::calculate() { -// if(targetDis <= (accdis+decdis)){ -// if(nowDis >= (accdis+decdis)){ -// vector = 0; -// }else{ -// vector = maxspeed - s_vector; -// } -// }else{ - 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; - } -// } + 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()