Diff: shared/OnOffMovement/OnOffMovement.cpp

Revision:

0:ee7e9405e1c7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/OnOffMovement/OnOffMovement.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,35 @@
+#include "OnOffMovement.h"
+
+OnOffMovement::OnOffMovement(Movement *movement) : AutomaticMovement(movement)
+{}
+
+Eigen::Vector3d OnOffMovement::move_automatic(Eigen::Vector3d current_position)
+{
+	Eigen::Vector3d position_difference, speed_move;
+	position_difference = _target_position - current_position;
+
+	for(int i = 0; i < 3; i++){
+		if(fabs(position_difference(i)) < _near_distance(i))
+			speed_move(i) = 0;
+		else
+			speed_move(i) = position_difference(i)
+							/ fabs(position_difference(i))
+							* _speed(i);
+	}
+
+	EigenCalculation::rotate_coordinate(&speed_move, -current_position(Movement::Yaw));
+
+	_movement->move(speed_move);
+
+	return speed_move;
+}
+
+void OnOffMovement::set_near_distance(Eigen::Vector3d near_distance)
+{
+	_near_distance = near_distance;
+}
+
+void OnOffMovement::set_speed(Eigen::Vector3d speed)
+{
+	_speed = speed;
+}