Skip to content

Commit

Permalink
Further refactoring of motor_hardware
Browse files Browse the repository at this point in the history
  • Loading branch information
@MatjazBostic
MatjazBostic committed Aug 26, 2024
1 parent 4bda875 commit 9b77691
Show file tree
Hide file tree
Showing 5 changed files with 345 additions and 1,651 deletions.
179 changes: 161 additions & 18 deletions include/ubiquity_motor_ros2/motor_hardware.h
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,51 @@
#ifndef MOTOR_HARDWARE_H
#define MOTOR_HARDWARE_H
/**
Copyright (c) 2016, Ubiquity Robotics
All rights reserved.
#include <hardware_interface/actuator_interface.hpp>
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of ubiquity_motor nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**/

#ifndef MOTORHARDWARE_H
#define MOTORHARDWARE_H

// #include <hardware_interface/actuator_interface.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
// #include <hardware_interface/actuator_handle.hpp>
// #include <hardware_interface/actuator_state_handle.hpp>

// #include <hardware_interface/actuator_interface.hpp>
// #include <hardware_interface/hardware_info.hpp>
// #include <hardware_interface/joint_state_handle.hpp>
// #include <hardware_interface/joint_handle.hpp>

#include <hardware_interface/actuator_interface.hpp>
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/loaned_command_interface.hpp>
#include <hardware_interface/loaned_state_interface.hpp>

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <std_msgs/msg/float32.hpp>
Expand All @@ -19,6 +59,83 @@
#include <ubiquity_motor_ros2/motor_message.h>
#include <ubiquity_motor_ros2/motor_serial.h>

// Mininum hardware versions required for various features
#define MIN_HW_OPTION_SWITCH 50

struct MotorDiagnostics {
MotorDiagnostics()
: odom_update_status(
diagnostic_updater::FrequencyStatusParam(&odom_min_freq, &odom_max_freq)) {}
// Communication Statuses
int firmware_version = 0;
int firmware_date = 0;
int firmware_options = 0;

// These are for diagnostic topic output
int fw_pid_proportional = 0;
int fw_pid_integral = 0;
int fw_pid_derivative = 0;
int fw_pid_control = 0;
int fw_pid_velocity = 0;
int fw_pid_denominator = 0;
int fw_pid_moving_buffer_size = 0;
int fw_max_pwm = 0;

double odom_max_freq = 1000;
double odom_min_freq = 50;
diagnostic_updater::FrequencyStatus odom_update_status;

// Limits
bool left_pwm_limit = false;
bool right_pwm_limit = false;
bool left_integral_limit = false;
bool right_integral_limit = false;
bool left_max_speed_limit = false;
bool right_max_speed_limit = false;
bool param_limit_in_firmware = false;

// Power supply statuses
float battery_voltage = 0.0;
float battery_voltage_low_level = 22.5;
float battery_voltage_critical = 21.0;

// Wheel current states
double motorCurrentLeft = 0.0;
double motorCurrentRight = 0.0;

// ADC count for zero current. We could calibrate this if required.
// Nominally 1024 and goes up from there this lower value is used.
double motorAmpsZeroAdcCount = 1015;

int motorPwmDriveLeft = 0;
int motorPwmDriveRight = 0;

/* For later implementation (firmware support)
bool main_5V_error = false;
bool main_5V_ol = false;
bool main_12V_error = false;
bool main_12V_ol = false;
bool aux_5V_error = false;
bool aux_5V_ol = false;
bool aux_12V_error = false;
bool aux_12V_ol = false;
*/

bool estop_motor_power_off = false; // for Diagnostic reporting of ESTOP switch

void firmware_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void limit_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void battery_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void motor_power_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void motor_pid_p_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void motor_pid_i_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void motor_pid_d_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void motor_pid_v_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void motor_max_pwm_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void firmware_options_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
void firmware_date_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
};

class MotorHardware : public hardware_interface::ActuatorInterface {
public:
MotorHardware(const std::shared_ptr<rclcpp::Node>& n, NodeParams node_params, CommsParams serial_params, FirmwareParams firmware_params);
Expand Down Expand Up @@ -46,54 +163,75 @@ class MotorHardware : public hardware_interface::ActuatorInterface {
void setParams(FirmwareParams firmware_params);
void sendParams();
void forcePidParamUpdates();
float getBatteryVoltage();
float getBatteryVoltage(void);
void setDeadmanTimer(int32_t deadman);
void setDeadzoneEnable(int32_t deadzone_enable);
void setDebugLeds(bool led1, bool led2);
void setHardwareVersion(int32_t hardware_version);
void setEstopPidThreshold(int32_t estop_pid_threshold);
void setEstopDetection(int32_t estop_detection);
bool getEstopState();
bool getEstopState(void);
void setMaxFwdSpeed(int32_t max_speed_fwd);
void setMaxRevSpeed(int32_t max_speed_rev);
void setMaxPwm(int32_t max_pwm);
void setWheelType(int32_t wheel_type);
void setWheelGearRatio(double wheel_gear_ratio);
double getWheelGearRatio();
double getWheelTicksPerRadian();
double getWheelGearRatio(void);
double getWheelTicksPerRadian(void);
void setDriveType(int32_t drive_type);
void setPidControl(int32_t pid_control);
void nullWheelErrors();
void nullWheelErrors(void);
void setWheelDirection(int32_t wheel_direction);
void getMotorCurrents(double &currentLeft, double &currentRight);
int getOptionSwitch();
int getPidControlWord();
int getOptionSwitch(void);
int getPidControlWord(void);
void setOptionSwitchReg(int32_t option_switch);
void requestSystemEvents();
void setSystemEvents(int32_t system_events);
void getWheelJointPositions(double &leftWheelPosition, double &rightWheelPosition);
void setWheelJointVelocities(double leftWheelVelocity, double rightWheelVelocity);
void publishMotorState();
void publishMotorState(void);
int firmware_version;
int firmware_date;
int firmware_options;
int num_fw_params; // This is used for sendParams as modulo count
int hardware_version;
int estop_pid_threshold;
int max_speed_fwd;
int max_speed_rev;
int max_pwm;
int pid_control;
int deadman_enable;
int system_events;
int wheel_type;
double wheel_gear_ratio;
int drive_type;

diagnostic_updater::Updater diag_updater;
private:
void _addOdometryRequest(std::vector<MotorMessage>& commands) const;
void _addVelocityRequest(std::vector<MotorMessage>& commands) const;

int16_t calculateSpeedFromRadians(double radians);
double calculateRadiansFromTicks(int16_t ticks);

// hardware_interface::JointStateInterface joint_state_interface_;
// hardware_interface::VelocityJointInterface velocity_joint_interface_;
std::vector<hardware_interface::StateInterface> state_interfaces_;
std::vector<hardware_interface::CommandInterface> command_interfaces_;

rclcpp::Node::SharedPtr node;

rclcpp::Logger logger;

FirmwareParams fw_params;
FirmwareParams prev_fw_params;

int32_t deadman_timer;
double ticks_per_radian;

double ticks_per_radian; // Odom ticks per radian for wheel encoders in use

int32_t sendPid_count;
bool estop_motor_power_off;

bool estop_motor_power_off; // Motor power inactive, most likely from ESTOP switch

struct Joint {
double position;
Expand All @@ -104,11 +242,13 @@ class MotorHardware : public hardware_interface::ActuatorInterface {
Joint() : position(0), velocity(0), effort(0), velocity_command(0) {}
} joints_[2];

// MessageTypes enum for refering to motor or wheel number
enum MotorOrWheelNumber {
Motor_M1 = 1,
Motor_M2 = 2
};

// MessageTypes enum in class to avoid global namespace pollution
enum WheelJointLocation {
Left = 0,
Right = 1
Expand All @@ -129,9 +269,12 @@ class MotorHardware : public hardware_interface::ActuatorInterface {
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr motor_state;

MotorSerial* motor_serial_;
// MotorDiagnostics motor_diag_;

diagnostic_updater::Updater diag_updater_;
MotorDiagnostics motor_diag_;

FRIEND_TEST(MotorHardwareTests, nonZeroWriteSpeedsOutputs);
FRIEND_TEST(MotorHardwareTests, odomUpdatesPosition);
FRIEND_TEST(MotorHardwareTests, odomUpdatesPositionMax);
};

#endif // MOTOR_HARDWARE_HPP
#endif
Loading

0 comments on commit 9b77691

Please sign in to comment.