自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
Odmetry.cpp
- Committer:
- kazumayamanaka
- Date:
- 2022-03-05
- Revision:
- 8:f2407caf81ba
- Parent:
- 7:a0b9b6e71e41
File content as of revision 8:f2407caf81ba:
#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 + PI / 2); } double Odmetry::obt_CurrentPosX(){ return XPos; } double Odmetry::obt_CurrentPosY(){ return YPos; } double Odmetry::obt_CurrentTheta(){ return Theta + PI / 2; }