自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
Diff: Odmetry.cpp
- Revision:
- 2:e189d55ef292
- Child:
- 7:a0b9b6e71e41
diff -r ea880e226e5a -r e189d55ef292 Odmetry.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Odmetry.cpp Fri Jan 28 15:12:04 2022 +0000 @@ -0,0 +1,60 @@ +#include "Odmetry.hpp" + +Odmetry::Odmetry(){ + dt = 0.05; + Radius = 395; + Wradius = 25; + XPos = 0; + YPos = 0; +} + +void Odmetry::pass_pulse(double PULSE1,double PULSE2,double PULSE3,double PULSE4){ + pulse_a[0] = PULSE1; + pulse_a[1] = PULSE2; + pulse_a[2] = PULSE3; + pulse_a[3] = PULSE4; + + if(PULSE1 == PULSE2 == PULSE3 == PULSE4){ + return; + } + else{ + CalcSpd(); + Angular(); + transform(); + CalcPosition(); + } +} + +void Odmetry::pass_angle(double THETA){ + Theta = THETA - PI / 4; +} + + +void Odmetry::CalcSpd(){ + for(int i = 0;i < 4;i ++){ + diff[i] = pulse_a[i] - pulse_b[i]; + spd[i] = Co * diff[i] / dt; + pulse_b[i] = pulse_a[i]; + } +} + +void Odmetry::Angular(){ + for(int i = 0;i < 4;i ++){ + Omega[i] = spd[i] / Radius; + } +} + +void Odmetry::transform(){ + Xpower = Wradius * (Omega[1] - Omega[3]); + Ypower = Wradius * (Omega[0] - Omega[2]); +} + +void Odmetry::CalcPosition(){ + Xspd = cos(Theta) * Xpower - sin(Theta) * Ypower; + Yspd = sin(Theta) * Xpower + cos(Theta) * Ypower; + + XPos += Xspd * dt; + YPos += Yspd * dt; +} + +void Odmetry::print_Pos(){ printf("XPos:%lf YPos:%lf theta:%lf\n",XPos,YPos,Theta); }