⚡ (rc): Move reboot to RC

leka · Jun 8, 2022 · 7308c6c · 7308c6c
1 parent a6bd955
commit 7308c6c
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 8 deletions.
diff --git a/app/os/main.cpp b/app/os/main.cpp
@@ -351,14 +351,8 @@ namespace robot {
 
 	void emergencyStop(const MagicCard &card)
 	{
-		static auto emergency_stop_iteration = 0;
 		if (card == MagicCard::emergency_stop) {
-			++emergency_stop_iteration;
 			controller.raiseEmergencyStop();
-
-			if (emergency_stop_iteration == 7) {
-				system_reset();
-			}
 		}
 	}
 

diff --git a/libs/RobotKit/include/RobotController.h b/libs/RobotKit/include/RobotController.h
@@ -209,6 +209,8 @@ class RobotController : public interface::RobotController
 		stopActuators();
 	}
 
+	void resetEmergencyStopIteration() final { _emergency_stop_iteration = 0; }
+
 	void raise(auto event)
 	{
 		_event_queue.call([this, &event] { state_machine.process_event(event); });
@@ -247,7 +249,15 @@ class RobotController : public interface::RobotController
 		_service_update.onFactoryResetNotification(on_factory_reset_requested);
 	}
 
-	void raiseEmergencyStop() { raise(system::robot::sm::event::emergency_stop {}); }
+	void raiseEmergencyStop()
+	{
+		++_emergency_stop_iteration;
+		raise(system::robot::sm::event::emergency_stop {});
+		log_debug("emergency_stop_iteration : %i", _emergency_stop_iteration);
+		if (_emergency_stop_iteration == 7) {
+			system_reset();
+		}
+	}
 
 	void registerEvents()
 	{
@@ -379,6 +389,8 @@ class RobotController : public interface::RobotController
 		&_service_battery,	  &_service_commands,		&_service_device_information,
 		&_service_monitoring, &_service_file_reception, &_service_update,
 	};
+
+	uint8_t _emergency_stop_iteration {0};
 };
 
 }	// namespace leka
diff --git a/libs/RobotKit/include/StateMachine.h b/libs/RobotKit/include/StateMachine.h
@@ -143,6 +143,10 @@ namespace sm::action {
 		auto operator()(irc &rc) const { rc.turnOffActuators(); }
 	};
 
+	struct reset_emergency_stop_iteration {
+		auto operator()(irc &rc) const { rc.resetEmergencyStopIteration(); }
+	};
+
 }	// namespace sm::action
 
 struct StateMachine {
@@ -195,6 +199,7 @@ struct StateMachine {
 			, sm::state::updating + boost::sml::on_entry<_> / sm::action::apply_update {}
 
 			, sm::state::emergency_stopped + boost::sml::on_entry<_> / sm::action::turn_off_actuators {}
+			, sm::state::emergency_stopped + boost::sml::on_exit <_> / sm::action::reset_emergency_stop_iteration {}
 
 			, sm::state::emergency_stopped + event<sm::event::command_received> [sm::guard::is_not_charging {} && sm::guard::is_connected {}] = sm::state::working
 			, sm::state::emergency_stopped + event<sm::event::ble_connection>   [sm::guard::is_not_charging {}]                               = sm::state::working

diff --git a/libs/RobotKit/include/interface/RobotController.h b/libs/RobotKit/include/interface/RobotController.h
@@ -41,7 +41,8 @@ class RobotController
 	virtual auto isReadyToUpdate() -> bool = 0;
 	virtual void applyUpdate()			   = 0;
 
-	virtual void turnOffActuators() = 0;
+	virtual void turnOffActuators()			   = 0;
+	virtual void resetEmergencyStopIteration() = 0;
 };
 
 }	// namespace leka::interface