[Neuron][Servo][Inialize] introduce a new variable to check the connn…

…ection between spinal and neuron
jsk-ros-pkg · Nov 17, 2024 · feb5e2a · feb5e2a
1 parent 71d746f
commit feb5e2a
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 0 deletions.
diff --git a/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp b/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp
@@ -62,6 +62,7 @@ void Initializer::receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t*
 	case CAN::MESSAGEID_RECEIVE_INITIAL_CONFIG_REQUEST:
 	{
 		sendBoardConfig();
+		servo_.setConnect(true);
 	}
 	break;
 	case CAN::MESSAGEID_RECEIVE_BOARD_CONFIG_REQUEST:

diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp
@@ -11,6 +11,7 @@
 void Servo::init(UART_HandleTypeDef* huart, I2C_HandleTypeDef* hi2c, osMutexId* mutex = NULL)
 {
   servo_handler_.init(huart, hi2c, mutex);
+  connect_ = false;
 }
 
 void Servo::update()
@@ -35,6 +36,8 @@ void Servo::sendData()
 
 void Servo::receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* data)
 {
+	if (!connect_) return;
+
 	switch (message_id) {
 		case CAN::MESSAGEID_RECEIVE_SERVO_ANGLE:
 		{

diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h
@@ -24,6 +24,9 @@ class Servo : public CANDevice
 	void sendData() override;
 	void receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* data) override;
 
+	bool getConnect() {return connect_;}
+	void setConnect(bool connect) {connect_ = connect;}
+
 private:
 	struct CANServoData{
 		int16_t angle;
@@ -35,6 +38,7 @@ class Servo : public CANDevice
 		:angle(angle), temperature(temperature), moving(moving), current(current), error(error){}
 	};
 
+	bool connect_;
 	DynamixelSerial servo_handler_;
 	friend class Initializer;
 };