Merge pull request #119 from YoujianWu/master

Add chassis_calibration to calibrate chassis motor.
rm-controls · Sep 29, 2024 · 8509fdb · 8509fdb
2 parents b1e92b4 + 77493d5
commit 8509fdb
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 0 deletions.
diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h
@@ -119,6 +119,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   rm_common::JointPositionBinaryCommandSender* image_transmission_cmd_sender_{};
   rm_common::SwitchDetectionCaller* switch_detection_srv_{};
   rm_common::SwitchDetectionCaller* switch_armor_target_srv_{};
+  rm_common::CalibrationQueue* chassis_calibration_;
   rm_common::CalibrationQueue* shooter_calibration_;
   rm_common::CalibrationQueue* gimbal_calibration_;
 

diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
@@ -36,6 +36,8 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros:
   shooter_calibration_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_);
   nh.getParam("gimbal_calibration", rpc_value);
   gimbal_calibration_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_);
+  nh.getParam("chassis_calibration", rpc_value);
+  chassis_calibration_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_);
   shooter_power_on_event_.setRising(boost::bind(&ChassisGimbalShooterManual::shooterOutputOn, this));
   self_inspection_event_.setRising(boost::bind(&ChassisGimbalShooterManual::selfInspectionStart, this));
   game_start_event_.setRising(boost::bind(&ChassisGimbalShooterManual::gameStart, this));
@@ -71,6 +73,7 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros:
 void ChassisGimbalShooterManual::run()
 {
   ChassisGimbalManual::run();
+  chassis_calibration_->update(ros::Time::now());
   shooter_calibration_->update(ros::Time::now());
   gimbal_calibration_->update(ros::Time::now());
 }
@@ -197,6 +200,7 @@ void ChassisGimbalShooterManual::remoteControlTurnOff()
   shooter_cmd_sender_->setZero();
   shooter_calibration_->stop();
   gimbal_calibration_->stop();
+  chassis_calibration_->stop();
   use_scope_ = false;
   adjust_image_transmission_ = false;
 }
@@ -206,6 +210,7 @@ void ChassisGimbalShooterManual::remoteControlTurnOn()
   ChassisGimbalManual::remoteControlTurnOn();
   shooter_calibration_->stopController();
   gimbal_calibration_->stopController();
+  chassis_calibration_->stopController();
   std::string robot_color = robot_id_ >= 100 ? "blue" : "red";
   switch_detection_srv_->setEnemyColor(robot_id_, robot_color);
 }
@@ -222,6 +227,7 @@ void ChassisGimbalShooterManual::chassisOutputOn()
 {
   ChassisGimbalManual::chassisOutputOn();
   chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
+  chassis_calibration_->reset();
 }
 
 void ChassisGimbalShooterManual::shooterOutputOn()

diff --git a/src/manual_base.cpp b/src/manual_base.cpp
@@ -79,6 +79,7 @@ void ManualBase::run()
 
 void ManualBase::checkReferee()
 {
+  chassis_power_on_event_.update(chassis_output_on_);
   gimbal_power_on_event_.update(gimbal_output_on_);
   shooter_power_on_event_.update(shooter_output_on_);
   referee_is_online_ = (ros::Time::now() - referee_last_get_stamp_ < ros::Duration(0.3));
@@ -110,6 +111,7 @@ void ManualBase::jointStateCallback(const sensor_msgs::JointState::ConstPtr& dat
 
 void ManualBase::actuatorStateCallback(const rm_msgs::ActuatorState::ConstPtr& data)
 {
+  updateActuatorStamp(data, chassis_mount_motor_, chassis_actuator_last_get_stamp_);
   updateActuatorStamp(data, gimbal_mount_motor_, gimbal_actuator_last_get_stamp_);
   updateActuatorStamp(data, shooter_mount_motor_, shooter_actuator_last_get_stamp_);
 }