kazuma yamanaka / 2022_Ateam_MOTORprogram

自己位置推定機能を追加

Dependencies:   SBDBT arrc_mbed BNO055

Diff: Odmetry.cpp

Revision:
7:a0b9b6e71e41
Parent:
2:e189d55ef292
--- a/Odmetry.cpp	Fri Jan 28 15:13:35 2022 +0000
+++ b/Odmetry.cpp	Wed Feb 23 03:03:36 2022 +0000
@@ -57,4 +57,10 @@
     YPos += Yspd * dt;
 }
 
-void Odmetry::print_Pos(){ printf("XPos:%lf YPos:%lf theta:%lf\n",XPos,YPos,Theta); }
+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; }