Changed logic for enable/disable armIK, and added helper functions

huskyroboticsteam · Dec 1, 2023 · 305b851 · 305b851
1 parent 587a681
commit 305b851
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 25 deletions.
diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp
@@ -86,10 +86,7 @@ void MissionControlProtocol::handleEmergencyStopRequest(const json& j) {
 	}
 	// TODO: reinit motors
 	Globals::E_STOP = stop;
-	Globals::armIKEnabled = false;
-
-	// inform MC that Ik has been disabled
-	sendArmIKEnabledReport();
+	this->setArmIKEnabled();
 }
 
 static bool validateOperationModeRequest(const json& j) {
@@ -136,29 +133,29 @@ void MissionControlProtocol::handleDriveRequest(const json& j) {
 void MissionControlProtocol::handleRequestArmIKEnabled(const json& j) {
 	bool enabled = j["enabled"];
 	if (enabled) {
-		Globals::armIKEnabled = false;
-		if (_arm_ik_repeat_thread.joinable()) {
-			_arm_ik_repeat_thread.join();
-		}
-		DataPoint<navtypes::Vectord<Constants::arm::IK_MOTORS.size()>> armJointPositions =
-			robot::getMotorPositionsRad(Constants::arm::IK_MOTORS);
-		// Rover responds with armIKEnabledReport after requestArmIKEnable is processed
-		if (armJointPositions.isValid()) {
-			Globals::planarArmController.set_setpoint(armJointPositions.getData());
-			Globals::armIKEnabled = true;
-			_arm_ik_repeat_thread =
-				std::thread(&MissionControlProtocol::updateArmIKRepeatTask, this);
-		} else {
-			// unable to enable IK
-			log(LOG_WARN, "Unable to enable IK");
+		if (!Globals::armIKEnabled) {
+			if (_arm_ik_repeat_thread.joinable()) {
+				_arm_ik_repeat_thread.join();
+			}
+			DataPoint<navtypes::Vectord<Constants::arm::IK_MOTORS.size()>> armJointPositions =
+				robot::getMotorPositionsRad(Constants::arm::IK_MOTORS);
+			// Rover responds with armIKEnabledReport after requestArmIKEnable is processed
+			if (armJointPositions.isValid()) {
+				Globals::planarArmController.set_setpoint(armJointPositions.getData());
+				this->setArmIKEnabled(true);
+				_arm_ik_repeat_thread =
+					std::thread(&MissionControlProtocol::updateArmIKRepeatTask, this);
+			} else {
+				// unable to enable IK
+				log(LOG_WARN, "Unable to enable IK");
+			}
 		}
 	} else {
-		Globals::armIKEnabled = false;
+		this->setArmIKEnabled(false);
 		if (_arm_ik_repeat_thread.joinable()) {
 			_arm_ik_repeat_thread.join();
 		}
 	}
-	sendArmIKEnabledReport();
 }
 
 void MissionControlProtocol::setRequestedCmdVel(double dtheta, double dx) {
@@ -283,7 +280,7 @@ void MissionControlProtocol::sendCameraStreamReport(
 
 void MissionControlProtocol::handleConnection() {
 	// Turn off inverse kinematics on connection
-	Globals::armIKEnabled = false;
+	this->setArmIKEnabled(false);
 
 	// TODO: send the actual mounted peripheral, as specified by the command-line parameter
 	json j = {{"type", MOUNTED_PERIPHERAL_REP_TYPE}};
@@ -295,7 +292,6 @@ void MissionControlProtocol::handleConnection() {
 	}
 
 	this->_server.sendJSON(Constants::MC_PROTOCOL_NAME, j);
-	sendArmIKEnabledReport();
 
 	if (!Globals::AUTONOMOUS) {
 		// start power repeat thread (if not already running)
@@ -308,7 +304,6 @@ void MissionControlProtocol::handleHeartbeatTimedOut() {
 	robot::emergencyStop();
 	LOG_F(ERROR, "Heartbeat timed out! Emergency stopping.");
 	Globals::E_STOP = true;
-	Globals::armIKEnabled = false;
 }
 
 void MissionControlProtocol::startPowerRepeat() {
@@ -351,7 +346,7 @@ void MissionControlProtocol::stopAndShutdownPowerRepeat() {
 		}
 	}
 	// Turn off inverse kinematics so that IK state will be in sync with mission control
-	Globals::armIKEnabled = false;
+	this->setArmIKEnabled(false);
 }
 
 MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server)
@@ -423,6 +418,11 @@ MissionControlProtocol::~MissionControlProtocol() {
 	}
 }
 
+void MissionControlProtocol::setArmIKEnabled(const bool enabled) {
+	Globals::armIKEnabled = enabled;
+	this->sendArmIKEnabledReport();
+}
+
 void MissionControlProtocol::setRequestedJointPower(jointid_t joint, double power) {
 	std::lock_guard<std::mutex> joint_lock(this->_joint_power_mutex);
 	this->_last_joint_power[joint] = power;

diff --git a/src/network/MissionControlProtocol.h b/src/network/MissionControlProtocol.h
@@ -73,6 +73,7 @@ class MissionControlProtocol : public WebSocketProtocol { // TODO: add documenta
 	void handleHeartbeatTimedOut();
 	void startPowerRepeat();
 	void stopAndShutdownPowerRepeat();
+	void setArmIKEnabled(const bool enabled);
 	void setRequestedJointPower(jointid_t joint, double power);
 	void setRequestedCmdVel(double dtheta, double dx);
 };