Diff: shared/Odometry2/Odometry2.h

Revision:

0:ee7e9405e1c7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Odometry2/Odometry2.h	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,48 @@
+
+#ifndef ODOMETRY4_H_
+#define ODOMETRY4_H_
+
+#include <shared/OdometryUnit/OdometryUnit.h>
+#include <Dense.h>
+#include <LU.h>
+#include <shared/EigenCalculation/EigenCalculation.h>
+
+typedef Eigen::Matrix<double, 2, 3> Matrix23d;
+typedef Eigen::Matrix<double, 3, 2> Matrix32d;
+
+class Odometry2
+{
+public:
+	static const int NUM = 2;
+	Odometry2(OdometryUnit *wheel[NUM]);
+	virtual ~Odometry2(){};
+
+	Eigen::Vector3d read_position();
+
+	void write_position(const Eigen::Vector3d& set_position);
+	void reset_position();
+
+	Eigen::Vector3d get_velocity();
+
+	void set_coefficient_matrix();
+
+	enum VectorElements{
+		X,
+		Y,
+		Yaw
+	};
+
+private:
+	Eigen::Vector3d _read_velocity();
+	void _set_coefficient_matrix();
+
+	OdometryUnit *_wheel[NUM];
+	Eigen::Vector3d _coefficient[NUM];
+	Matrix32d _coefficient_matrix;
+	Eigen::Vector3d _position;
+	Eigen::Vector3d _velocity;
+};
+
+
+
+#endif