自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
Diff: TARGETco.cpp
- Revision:
- 0:a1238c4cd105
- Child:
- 1:ea880e226e5a
diff -r 000000000000 -r a1238c4cd105 TARGETco.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TARGETco.cpp Wed Jan 19 12:43:26 2022 +0000 @@ -0,0 +1,32 @@ +#include "TARGETco.hpp" + +void TARGETco::pass_val(double XPOWER,double YPOWER,double THETA){ + Xpower = XPOWER; + Ypower = YPOWER; + theta = THETA; + + cal_angular(); +} + +void TARGETco::pass_target(double TARGET){ + for(int i = 0;i < 4;i ++){ + target[i] = TARGET; + } +} + +void TARGETco::cal_angular(){ + Xvelocity = Xpower * cos(theta) - Ypower * sin(theta); + Yvelocity = Xpower * sin(theta) + Ypower * cos(theta); + target[0] = -Xvelocity; + target[1] = -Yvelocity; + target[2] = Xvelocity; + target[3] = Yvelocity; +} + +double TARGETco::obt_target1(){ return target[0]; } + +double TARGETco::obt_target2(){ return target[1]; } + +double TARGETco::obt_target3(){ return target[2]; } + +double TARGETco::obt_target4(){ return target[3]; } \ No newline at end of file