yuki honma / UnderBody

下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用

Dependents:   TOUTEKI_all_mbed mbed_test_program

Bezier.cpp@3:9c3f2662974e, 2018-09-04 (annotated)

Committer:
yuki17100
Date:
Tue Sep 04 14:22:13 2018 +0000
Revision:
3:9c3f2662974e
Parent:
1:698586aa0c5a 
Mecanum????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki17100 0:62707e16531a 1 #include "Bezier.h"
yuki17100 0:62707e16531a 2
yuki17100 0:62707e16531a 3
yuki17100 0:62707e16531a 4 BezierCurve::BezierCurve(){
yuki17100 0:62707e16531a 5 length = 0;
yuki17100 0:62707e16531a 6 }
yuki17100 0:62707e16531a 7
yuki17100 0:62707e16531a 8 //指定区間の長さを出す関数
yuki17100 0:62707e16531a 9 double BezierCurve::range(double start,double stop){
yuki17100 0:62707e16531a 10
yuki17100 0:62707e16531a 11 double len = 0;
yuki17100 0:62707e16531a 12 double _posi[2] = {};
yuki17100 0:62707e16531a 13
yuki17100 0:62707e16531a 14 for(; start<stop; start+=WIDTH){
yuki17100 0:62707e16531a 15
yuki17100 0:62707e16531a 16 double posi[2] = {callX(start), callY(start)};
yuki17100 0:62707e16531a 17
yuki17100 0:62707e16531a 18 len += hypot( posi[0] - _posi[0], posi[1] - _posi[1]);
yuki17100 0:62707e16531a 19
yuki17100 0:62707e16531a 20 _posi[0] = posi[0];
yuki17100 0:62707e16531a 21 _posi[1] = posi[1];
yuki17100 0:62707e16531a 22
yuki17100 0:62707e16531a 23 }
yuki17100 0:62707e16531a 24
yuki17100 0:62707e16531a 25 return len;
yuki17100 0:62707e16531a 26 }
yuki17100 0:62707e16531a 27
yuki17100 0:62707e16531a 28 //ベジエ曲線のパラメータを設定する関数
yuki17100 0:62707e16531a 29 void BezierCurve::set(double (&pX)[4], double (&pY)[4], double (&pT)[4]){
yuki17100 0:62707e16531a 30
yuki17100 0:62707e16531a 31 for(int i=0; i<4; i++){
yuki17100 0:62707e16531a 32 pointX[i] = pX[i];
yuki17100 0:62707e16531a 33 pointY[i] = pY[i];
yuki17100 0:62707e16531a 34 pointT[i] = pT[i];
yuki17100 0:62707e16531a 35 }
yuki17100 0:62707e16531a 36
yuki17100 0:62707e16531a 37 length = range(0,1);
yuki17100 0:62707e16531a 38 }
yuki17100 0:62707e16531a 39
yuki17100 0:62707e16531a 40
yuki17100 0:62707e16531a 41 double BezierCurve::callX(double ratio){
yuki17100 0:62707e16531a 42 return (1-ratio)*(1-ratio)*(1-ratio)*pointX[0] + (1-ratio)*(1-ratio)*ratio*pointX[1]+
yuki17100 0:62707e16531a 43 (1-ratio)*ratio *ratio *pointX[2] + ratio *ratio *ratio*pointX[3];
yuki17100 0:62707e16531a 44 }
yuki17100 0:62707e16531a 45
yuki17100 0:62707e16531a 46 double BezierCurve::callY(double ratio){
yuki17100 0:62707e16531a 47 return (1-ratio)*(1-ratio)*(1-ratio)*pointY[0] + (1-ratio)*(1-ratio)*ratio*pointY[1]+
yuki17100 0:62707e16531a 48 (1-ratio)*ratio *ratio *pointY[2] + ratio *ratio *ratio*pointY[3];
yuki17100 0:62707e16531a 49 }
yuki17100 0:62707e16531a 50
yuki17100 0:62707e16531a 51 double BezierCurve::callT(double ratio){
yuki17100 0:62707e16531a 52 return (1-ratio)*(1-ratio)*(1-ratio)*pointT[0] + (1-ratio)*(1-ratio)*ratio*pointT[1]+
yuki17100 0:62707e16531a 53 (1-ratio)*ratio *ratio *pointT[2] + ratio *ratio *ratio*pointT[3];
yuki17100 0:62707e16531a 54 }
yuki17100 0:62707e16531a 55 /*
yuki17100 1:698586aa0c5a 56 *
yuki17100 1:698586aa0c5a 57 * 指定位置から一定距離の点を算出する関数を作ろうとした。
yuki17100 1:698586aa0c5a 58 * 参考のために残しておく
yuki17100 1:698586aa0c5a 59
yuki17100 0:62707e16531a 60 double BezierCurve::aimPoint(double dist, double crrX, double crrY, double (&tgtPosi)[3]){
yuki17100 0:62707e16531a 61
yuki17100 1:698586aa0c5a 62 double r = 0; //これ0だといけない static かクラス変数つくるべき
yuki17100 0:62707e16531a 63
yuki17100 0:62707e16531a 64 do {
yuki17100 0:62707e16531a 65 r += WIDTH;
yuki17100 0:62707e16531a 66 if(r >= 1){ break; }
yuki17100 0:62707e16531a 67
yuki17100 0:62707e16531a 68 } while(hypot(callX(r) - crrX, callY(r) - crrY) < dist);
yuki17100 0:62707e16531a 69
yuki17100 0:62707e16531a 70 tgtPosi[0] = callX(r);
yuki17100 0:62707e16531a 71 tgtPosi[1] = callY(r);
yuki17100 0:62707e16531a 72 tgtPosi[2] = callT(r);
yuki17100 0:62707e16531a 73
yuki17100 0:62707e16531a 74 return range(r,1);
yuki17100 0:62707e16531a 75 }
yuki17100 1:698586aa0c5a 76
yuki17100 1:698586aa0c5a 77
yuki17100 0:62707e16531a 78 */