✨ (MotionKit): Add on_rotation_ended_callback to rotate function

leka · Dec 8, 2022 · 4abb309 · 4abb309
1 parent c8b1a9b
commit 4abb309
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 3 deletions.
diff --git a/libs/MotionKit/include/MotionKit.h b/libs/MotionKit/include/MotionKit.h
@@ -21,7 +21,8 @@ class MotionKit
 
 	void init();
 
-	void rotate(uint8_t number_of_rotations, Rotation direction);
+	void rotate(uint8_t number_of_rotations, Rotation direction,
+				const std::function<void()> &on_rotation_ended_callback = {});
 	void startStabilisation();
 
 	void stop();
@@ -39,6 +40,7 @@ class MotionKit
 	PID _pid;
 
 	uint8_t _rotations_to_execute = 0;
+	std::function<void()> _on_rotation_ended_callback {};
 
 	bool _target_not_reached	   = false;
 	bool _stabilisation_requested  = false;

diff --git a/libs/MotionKit/source/MotionKit.cpp b/libs/MotionKit/source/MotionKit.cpp
@@ -26,7 +26,8 @@ void MotionKit::stop()
 	_rotate_x_turns_requested = false;
 }
 
-void MotionKit::rotate(uint8_t number_of_rotations, Rotation direction)
+void MotionKit::rotate(uint8_t number_of_rotations, Rotation direction,
+					   const std::function<void()> &on_rotation_ended_callback)
 {
 	stop();
 
@@ -43,6 +44,8 @@ void MotionKit::rotate(uint8_t number_of_rotations, Rotation direction)
 	_motor_right.spin(direction, kPwmMaxValue);
 
 	_event_loop.start();
+
+	_on_rotation_ended_callback = on_rotation_ended_callback;
 }
 
 void MotionKit::startStabilisation()
@@ -93,6 +96,11 @@ void MotionKit::run()
 
 		rtos::ThisThread::sleep_for(70ms);
 	}
+
+	if (_on_rotation_ended_callback != nullptr) {
+		_on_rotation_ended_callback();
+	}
+
 	_imukit.stop();
 }
 

diff --git a/spikes/lk_motion_kit/main.cpp b/spikes/lk_motion_kit/main.cpp
@@ -96,7 +96,7 @@ void onMagicCardAvailable(const MagicCard &card)
 {
 	switch (card.getId()) {
 		case (MagicCard::number_1.getId()):
-			motionkit.rotate(1, Rotation::counterClockwise);
+			motionkit.rotate(1, Rotation::counterClockwise, [&] { log_debug("Callback end of rotation"); });
 			break;
 		case (MagicCard::number_2.getId()):
 			motionkit.rotate(2, Rotation::clockwise);