下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用
Dependents: TOUTEKI_all_mbed mbed_test_program
Diff: Bezier.cpp
- Revision:
- 0:62707e16531a
- Child:
- 1:698586aa0c5a
diff -r 000000000000 -r 62707e16531a Bezier.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Bezier.cpp Sat Sep 01 09:07:35 2018 +0000 @@ -0,0 +1,72 @@ +#include "Bezier.h" + + +BezierCurve::BezierCurve(){ + length = 0; +} + +//指定区間の長さを出す関数 +double BezierCurve::range(double start,double stop){ + + double len = 0; + double _posi[2] = {}; + + for(; start<stop; start+=WIDTH){ + + double posi[2] = {callX(start), callY(start)}; + + len += hypot( posi[0] - _posi[0], posi[1] - _posi[1]); + + _posi[0] = posi[0]; + _posi[1] = posi[1]; + + } + + return len; +} + +//ベジエ曲線のパラメータを設定する関数 +void BezierCurve::set(double (&pX)[4], double (&pY)[4], double (&pT)[4]){ + + for(int i=0; i<4; i++){ + pointX[i] = pX[i]; + pointY[i] = pY[i]; + pointT[i] = pT[i]; + } + + length = range(0,1); +} + + +double BezierCurve::callX(double ratio){ + return (1-ratio)*(1-ratio)*(1-ratio)*pointX[0] + (1-ratio)*(1-ratio)*ratio*pointX[1]+ + (1-ratio)*ratio *ratio *pointX[2] + ratio *ratio *ratio*pointX[3]; +} + +double BezierCurve::callY(double ratio){ + return (1-ratio)*(1-ratio)*(1-ratio)*pointY[0] + (1-ratio)*(1-ratio)*ratio*pointY[1]+ + (1-ratio)*ratio *ratio *pointY[2] + ratio *ratio *ratio*pointY[3]; +} + +double BezierCurve::callT(double ratio){ + return (1-ratio)*(1-ratio)*(1-ratio)*pointT[0] + (1-ratio)*(1-ratio)*ratio*pointT[1]+ + (1-ratio)*ratio *ratio *pointT[2] + ratio *ratio *ratio*pointT[3]; +} +/* +double BezierCurve::aimPoint(double dist, double crrX, double crrY, double (&tgtPosi)[3]){ + + double r = 0; + + do { + r += WIDTH; + if(r >= 1){ break; } + + } while(hypot(callX(r) - crrX, callY(r) - crrY) < dist); + + tgtPosi[0] = callX(r); + tgtPosi[1] = callY(r); + tgtPosi[2] = callT(r); + + return range(r,1); +} +*/ \ No newline at end of file