diff --git a/CMakeLists.txt b/CMakeLists.txt index 3d31f13..e45a246 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,49 +1,85 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.5) project(ubiquity_motor_ros2) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +# Find dependencies find_package(ament_cmake REQUIRED) -#find_package(message_generation REQUIRED) +find_package(rosidl_default_generators REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclpy REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) -find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(hardware_interface REQUIRED) -find_package(controller_manager REQUIRED) +find_package(ros2_control REQUIRED) find_package(diagnostic_updater REQUIRED) -#find_package(dynamic_reconfigure REQUIRED) - does not exist in ROS2 -#find_package(Boost REQUIRED) -#find_package(thread REQUIRED) -#find_package(atomic REQUIRED) -add_library(ubiquity_motor - src/motor_message.cc src/motor_serial.cc src/motor_hardware.cc + +# ROS 2 message generation +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/MotorState.msg" + DEPENDENCIES std_msgs ) -install(PROGRAMS - scripts/upgrade_firmware.py - scripts/test_motor_board.py - scripts/test_pi_gpio.py - DESTINATION lib/${PROJECT_NAME} +# Include directories +include_directories( + include ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() -ament_package() +# Build the library containing reusable code +# add_library(ubiquity_motor +# src/motor_message.cc +# src/motor_serial.cc +# src/motor_hardware.cc +# ) + +# Link the generated messages to the library +# ament_target_dependencies(ubiquity_motor +# "rclcpp" +# "std_msgs" +# "sensor_msgs" +# "hardware_interface" +# "ros2_control" +# "diagnostic_updater" +# ) + +# Add executable +add_executable(motor_node src/motor_node.cc) + +# add_dependencies(motor_node ${PROJECT_NAME}_interfaces) + +# Link the generated messages and the library to the executable +# target_link_libraries(motor_node ubiquity_motor) + +ament_target_dependencies(motor_node + "rclcpp" + "std_msgs" + "sensor_msgs" + "hardware_interface" + "ros2_control" + "diagnostic_updater" +) + +ament_export_dependencies(rosidl_default_runtime) + +# # Install the library and executable +install(TARGETS + motor_node + #ubiquity_motor + DESTINATION lib/${PROJECT_NAME} +) + +# # Install any Python scripts (if any) +# install(PROGRAMS +# scripts/upgrade_firmware.py +# scripts/test_motor_board.py +# scripts/test_pi_gpio.py +# DESTINATION lib/${PROJECT_NAME} +# ) + +# Finish up the package +ament_package() \ No newline at end of file diff --git a/CMakeLists_old.txt b/CMakeLists_old.txt new file mode 100644 index 0000000..fa1f0d5 --- /dev/null +++ b/CMakeLists_old.txt @@ -0,0 +1,90 @@ +cmake_minimum_required(VERSION 3.5) +project(ubiquity_motor_ros2) + +set(CMAKE_CXX_STANDARD 14) # use C++14 + +# Compiler flags for warnings +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(ros2_control REQUIRED) +find_package(diagnostic_updater REQUIRED) + +# ROS 2 message generation +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/MotorState.msg" +) + +# Add library +add_library(ubiquity_motor + #src/motor_message.cc src/motor_serial.cc src/motor_hardware.cc + src/motor_node.cc +) + +# Include directories for library +target_include_directories(ubiquity_motor + PUBLIC + $ + $ +) + +ament_target_dependencies(ubiquity_motor + "rclcpp" + "std_msgs" + "sensor_msgs" + "hardware_interface" + "ros2_control" + "diagnostic_updater" +) + +# Add executable +add_executable(motor_node src/motor_node.cc) + +# Include directories for executable +target_include_directories(motor_node + PUBLIC + $ + $ +) + +ament_target_dependencies(motor_node + "rclcpp" + "std_msgs" + "sensor_msgs" + "hardware_interface" + "ros2_control" + "diagnostic_updater" +) + +# Install targets +install(TARGETS + motor_node + ubiquity_motor + DESTINATION lib/${PROJECT_NAME} +) + +# Install scripts +install(PROGRAMS + scripts/upgrade_firmware.py + scripts/test_motor_board.py + scripts/test_pi_gpio.py + DESTINATION lib/${PROJECT_NAME} +) + +# Test dependencies +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() \ No newline at end of file diff --git a/include/ubiquity_motor/motor_hardware.h b/include/ubiquity_motor/motor_hardware.h index 0a27909..87990ab 100644 --- a/include/ubiquity_motor/motor_hardware.h +++ b/include/ubiquity_motor/motor_hardware.h @@ -31,22 +31,23 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef MOTORHARDWARE_H #define MOTORHARDWARE_H -#include "hardware_interface/joint_command_interface.h" -#include "hardware_interface/joint_state_interface.h" -#include "hardware_interface/robot_hw.h" -#include "ros/ros.h" -#include "sensor_msgs/JointState.h" - -#include "std_msgs/Int32.h" -#include "std_msgs/UInt32.h" -#include "std_msgs/Float32.h" -#include "std_msgs/Bool.h" -#include "std_msgs/String.h" -#include "sensor_msgs/BatteryState.h" -#include "ubiquity_motor/MotorState.h" - -#include -#include +#include "rclcpp/rclcpp.hpp" +// #include "hardware_interface/joint_command_interface.h" +// #include "hardware_interface/joint_state_interface.hpp" +// #include "hardware_interface/robot_hw.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "sensor_msgs/msg/joint_state.hpp" + +#include "std_msgs/msg/int32.hpp" +#include "std_msgs/msg/u_int32.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/string.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "ubiquity_motor_ros2/msg/motor_state.hpp" + +#include +#include #include #include diff --git a/include/ubiquity_motor/motor_parameters.h b/include/ubiquity_motor/motor_parameters.h index 9191ffe..35b3bbf 100644 --- a/include/ubiquity_motor/motor_parameters.h +++ b/include/ubiquity_motor/motor_parameters.h @@ -31,7 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef UBIQUITY_MOTOR_MOTOR_PARMETERS_H #define UBIQUITY_MOTOR_MOTOR_PARMETERS_H -#include +#include // These defines are for a high level topic for control of the motor node at a system level #define ROS_TOPIC_SYSTEM_CONTROL "system_control" // A topic for system level control commands diff --git a/include/ubiquity_motor/motor_serial.h b/include/ubiquity_motor/motor_serial.h index 98590f3..cb90cb1 100644 --- a/include/ubiquity_motor/motor_serial.h +++ b/include/ubiquity_motor/motor_serial.h @@ -31,8 +31,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef MOTORSERIAL_H #define MOTORSERIAL_H -#include -#include +#include +#include #include #include #include diff --git a/msg/Leds.msg b/msg/Leds.msg new file mode 100644 index 0000000..0d6993b --- /dev/null +++ b/msg/Leds.msg @@ -0,0 +1,2 @@ +bool led1 +bool led2 diff --git a/msg/MotorState.msg b/msg/MotorState.msg new file mode 100644 index 0000000..5573c48 --- /dev/null +++ b/msg/MotorState.msg @@ -0,0 +1,9 @@ +std_msgs/Header header +float64 left_position +float64 right_position +float64 left_rotate_rate +float64 right_rotate_rate +float32 left_current +float32 right_current +int32 left_pwm_drive +int32 right_pwm_drive diff --git a/package.xml b/package.xml index c96c1ef..7b1b367 100644 --- a/package.xml +++ b/package.xml @@ -12,6 +12,13 @@ ament_lint_auto ament_lint_common + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + hardware_interface + + ament_cmake diff --git a/src/motor_hardware.cc b/src/motor_hardware.cc deleted file mode 100644 index 2faaafd..0000000 --- a/src/motor_hardware.cc +++ /dev/null @@ -1,1331 +0,0 @@ -/** -Copyright (c) 2016, Ubiquity Robotics -All rights reserved. - -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. -**/ -#include -#include -#include -#include - -// To access I2C we need some system includes -#include -#include -#include -#include -#define I2C_DEVICE "/dev/i2c-1" // This is specific to default Magni I2C port on host -const static uint8_t I2C_PCF8574_8BIT_ADDR = 0x40; // I2C addresses are 7 bits but often shown as 8-bit - -//#define SENSOR_DISTANCE 0.002478 - - -// For experimental purposes users will see that the wheel encoders are three phases -// of very neaar 43 pulses per revolution or about 43*3 edges so we see very about 129 ticks per rev -// This leads to 129/(2*Pi) or about 20.53 ticks per radian experimentally. -// 60 ticks per revolution of the motor (pre gearbox) and a gear ratio of 4.2941176:1 for 1st rev Magni wheels -// An unexplained constant from the past I leave here till explained is: 17.2328767 - -#define TICKS_PER_RAD_FROM_GEAR_RATIO ((double)(4.774556)*(double)(2.0)) // Used to generate ticks per radian -#define TICKS_PER_RADIAN_DEFAULT 41.004 // For runtime use getWheelGearRatio() * TICKS_PER_RAD_FROM_GEAR_RATIO -#define WHEEL_GEAR_RATIO_DEFAULT WHEEL_GEAR_RATIO_1 - -// TODO: Make HIGH_SPEED_RADIANS, WHEEL_VELOCITY_NEAR_ZERO and ODOM_4WD_ROTATION_SCALE all ROS params -#define HIGH_SPEED_RADIANS (1.8) // threshold to consider wheel turning 'very fast' -#define WHEEL_VELOCITY_NEAR_ZERO ((double)(0.08)) -#define ODOM_4WD_ROTATION_SCALE ((double)(1.65)) // Used to correct for 4WD skid rotation error - -#define MOTOR_AMPS_PER_ADC_COUNT ((double)(0.0238)) // 0.1V/Amp 2.44V=1024 count so 41.97 cnt/amp - -#define VELOCITY_READ_PER_SECOND ((double)(10.0)) // read = ticks / (100 ms), so we scale of 10 for ticks/second -#define LOWEST_FIRMWARE_VERSION 28 - -// Debug verification use only -int32_t g_odomLeft = 0; -int32_t g_odomRight = 0; -int32_t g_odomEvent = 0; - -//lead acid battery percentage levels for a single cell -const static float SLA_AGM[11] = { - 1.800, // 0 - 1.837, // 10 - 1.875, // 20 - 1.912, // 30 - 1.950, // 40 - 2.010, // 50 - 2.025, // 60 - 2.062, // 70 - 2.100, // 80 - 2.137, // 90 - 2.175, // 100 -}; - - -//li-ion battery percentage levels for a single cell -const static float LI_ION[11] = { - 3.3, // 0 - 3.49, // 10 - 3.53, // 20 - 3.55, // 30 - 3.60, // 40 - 3.64, // 50 - 3.70, // 60 - 3.80, // 70 - 3.85, // 80 - 4.05, // 90 - 4.20, // 100 -}; - -// We sometimes need to know if we are rotating in place due to special ways of dealing with -// A 4wd robot must skid to turn. This factor approximates the actual rotation done vs what -// the wheel encoders have indicated. This only applies if in 4WD mode -double g_odom4wdRotationScale = ODOM_4WD_ROTATION_SCALE; - -// 4WD robot chassis that has to use extensive torque to rotate in place and due to wheel slip has odom scale factor -double g_radiansLeft = 0.0; -double g_radiansRight = 0.0; - -// This utility opens and reads 1 or more bytes from a device on an I2C bus -// This method was taken on it's own from a big I2C class we may choose to use later -static int i2c_BufferRead(const char *i2cDevFile, uint8_t i2c8bitAddr, - uint8_t *pBuffer, int16_t chipRegAddr, uint16_t NumBytesToRead); - -MotorHardware::MotorHardware(ros::NodeHandle nh, NodeParams node_params, CommsParams serial_params, - FirmwareParams firmware_params) { - ros::V_string joint_names = - boost::assign::list_of("left_wheel_joint")("right_wheel_joint"); - - for (size_t i = 0; i < joint_names.size(); i++) { - hardware_interface::JointStateHandle joint_state_handle( - joint_names[i], &joints_[i].position, &joints_[i].velocity, - &joints_[i].effort); - joint_state_interface_.registerHandle(joint_state_handle); - - hardware_interface::JointHandle joint_handle( - joint_state_handle, &joints_[i].velocity_command); - velocity_joint_interface_.registerHandle(joint_handle); - } - registerInterface(&joint_state_interface_); - registerInterface(&velocity_joint_interface_); - - // Insert a delay prior to serial port setup to avoid a race defect. - // We see soemtimes the OS sets the port to 115200 baud just after we set it - ROS_INFO("Delay before MCB serial port initialization"); - ros::Duration(5.0).sleep(); - ROS_INFO("Initialize MCB serial port '%s' for %d baud", - serial_params.serial_port.c_str(), serial_params.baud_rate); - - motor_serial_ = - new MotorSerial(serial_params.serial_port, serial_params.baud_rate); - - ROS_INFO("MCB serial port initialized"); - - // For motor tunning and other uses we publish details for each wheel - leftError = nh.advertise("left_error", 1); - rightError = nh.advertise("right_error", 1); - leftTickInterval = nh.advertise("left_tick_interval", 1); - rightTickInterval = nh.advertise("right_tick_interval", 1); - - firmware_state = nh.advertise("firmware_version", 1, true); - battery_state = nh.advertise("battery_state", 1); - motor_power_active = nh.advertise("motor_power_active", 1); - - motor_state = nh.advertise("motor_state", 1); - leftCurrent = nh.advertise("left_current", 1); - rightCurrent = nh.advertise("right_current", 1); - - sendPid_count = 0; - num_fw_params = 8; // number of params sent if any change - - estop_motor_power_off = false; // Keeps state of ESTOP switch where true is in ESTOP state - - // Save default hardware encoder specifics for ticks in one radian of rotation of main wheel - this->ticks_per_radian = TICKS_PER_RADIAN_DEFAULT; - this->wheel_gear_ratio = WHEEL_GEAR_RATIO_DEFAULT; - - fw_params = firmware_params; - - prev_fw_params.pid_proportional = -1; - prev_fw_params.pid_integral = -1; - prev_fw_params.pid_derivative = -1; - prev_fw_params.pid_velocity = -1; - prev_fw_params.pid_denominator = -1; - prev_fw_params.pid_control = -1; - prev_fw_params.pid_moving_buffer_size = -1; - prev_fw_params.max_speed_fwd = -1; - prev_fw_params.max_speed_rev = -1; - prev_fw_params.deadman_timer = -1; - prev_fw_params.deadzone_enable = -1; - prev_fw_params.hw_options = -1; - prev_fw_params.option_switch = -1; - prev_fw_params.system_events = -1; - prev_fw_params.controller_board_version = -1; - prev_fw_params.estop_detection = -1; - prev_fw_params.estop_pid_threshold = -1; - prev_fw_params.max_speed_fwd = -1; - prev_fw_params.max_speed_rev = -1; - prev_fw_params.max_pwm = -1; - - hardware_version = 0; - firmware_version = 0; - firmware_date = 0; - - diag_updater.setHardwareID("Motor Controller"); - diag_updater.add("Firmware", &motor_diag_, &MotorDiagnostics::firmware_status); - diag_updater.add("Limits", &motor_diag_, &MotorDiagnostics::limit_status); - diag_updater.add("Battery", &motor_diag_, &MotorDiagnostics::battery_status); - diag_updater.add("MotorPower", &motor_diag_, &MotorDiagnostics::motor_power_status); - diag_updater.add("PidParamP", &motor_diag_, &MotorDiagnostics::motor_pid_p_status); - diag_updater.add("PidParamI", &motor_diag_, &MotorDiagnostics::motor_pid_i_status); - diag_updater.add("PidParamD", &motor_diag_, &MotorDiagnostics::motor_pid_d_status); - diag_updater.add("PidParamV", &motor_diag_, &MotorDiagnostics::motor_pid_v_status); - diag_updater.add("PidMaxPWM", &motor_diag_, &MotorDiagnostics::motor_max_pwm_status); - diag_updater.add("FirmwareOptions", &motor_diag_, &MotorDiagnostics::firmware_options_status); - diag_updater.add("FirmwareDate", &motor_diag_, &MotorDiagnostics::firmware_date_status); -} - -MotorHardware::~MotorHardware() { delete motor_serial_; } - -// Close of the serial port is used in a special case of suspending the motor controller -// so that another service can load firmware or do direct MCB diagnostics -void MotorHardware::closePort() { - motor_serial_->closePort(); -} - -// After we have given up the MCB we open serial port again using current instance of Serial -bool MotorHardware::openPort() { - return motor_serial_->openPort(); -} - -void MotorHardware::clearCommands() { - for (size_t i = 0; i < sizeof(joints_) / sizeof(joints_[0]); i++) { - joints_[i].velocity_command = 0; - } -} - -// Read the current wheel positions in radians both at once for a snapshot of position -void MotorHardware::getWheelJointPositions(double &leftWheelPosition, double &rightWheelPosition) { - leftWheelPosition = joints_[WheelJointLocation::Left].position; - rightWheelPosition = joints_[WheelJointLocation::Right].position; - return; -} - -// Set the current wheel joing velocities in radians/sec both at once for a snapshot of velocity -// This interface is supplied because MotorHardware does not do a loop on it's own -void MotorHardware::setWheelJointVelocities(double leftWheelVelocity, double rightWheelVelocity) { - joints_[WheelJointLocation::Left].velocity = leftWheelVelocity; - joints_[WheelJointLocation::Right].velocity = rightWheelVelocity; - return; -} - -// Publish motor state conditions -void MotorHardware::publishMotorState(void) { - ubiquity_motor::MotorState mstateMsg; - - mstateMsg.header.frame_id = ""; // Could be base_link. We will use empty till required - mstateMsg.header.stamp = ros::Time::now(); - - mstateMsg.leftPosition = joints_[WheelJointLocation::Left].position; - mstateMsg.rightPosition = joints_[WheelJointLocation::Right].position; - mstateMsg.leftRotateRate = joints_[WheelJointLocation::Left].velocity; - mstateMsg.rightRotateRate = joints_[WheelJointLocation::Right].velocity; - mstateMsg.leftCurrent = motor_diag_.motorCurrentLeft; - mstateMsg.rightCurrent = motor_diag_.motorCurrentRight; - mstateMsg.leftPwmDrive = motor_diag_.motorPwmDriveLeft; - mstateMsg.rightPwmDrive = motor_diag_.motorPwmDriveRight; - motor_state.publish(mstateMsg); - return; -} - -// readInputs() will receive serial and act on the response from motor controller -// -// The motor controller sends unsolicited messages periodically so we must read the -// messages to update status in near realtime -// -void MotorHardware::readInputs(uint32_t index) { - while (motor_serial_->commandAvailable()) { - MotorMessage mm; - mm = motor_serial_->receiveCommand(); - if (mm.getType() == MotorMessage::TYPE_RESPONSE) { - switch (mm.getRegister()) { - - case MotorMessage::REG_SYSTEM_EVENTS: - if ((mm.getData() & MotorMessage::SYS_EVENT_POWERON) != 0) { - ROS_WARN("Firmware System Event for PowerOn transition"); - system_events = mm.getData(); - } - break; - case MotorMessage::REG_FIRMWARE_VERSION: - if (mm.getData() < LOWEST_FIRMWARE_VERSION) { - ROS_FATAL("Firmware version %d, expect %d or above", - mm.getData(), LOWEST_FIRMWARE_VERSION); - throw std::runtime_error("Firmware version too low"); - } else { - ROS_INFO_ONCE("Firmware version %d", mm.getData()); - firmware_version = mm.getData(); - motor_diag_.firmware_version = firmware_version; - } - publishFirmwareInfo(); - break; - - case MotorMessage::REG_FIRMWARE_DATE: - // Firmware date is only supported as of fw version MIN_FW_FIRMWARE_DATE - ROS_INFO_ONCE("Firmware date 0x%x (format 0xYYYYMMDD)", mm.getData()); - firmware_date = mm.getData(); - motor_diag_.firmware_date = firmware_date; - - publishFirmwareInfo(); - break; - - case MotorMessage::REG_BOTH_ODOM: { - /* - * ODOM messages from the MCB tell us how far wheels have rotated - * - * It is here we keep track of wheel joint position - * The odom counts from the MCB are the incremental number of ticks since last report - * WARNING: IF WE LOOSE A MESSAGE WE DRIFT FROM REAL POSITION - */ - int32_t odom = mm.getData(); - // ROS_ERROR("odom signed %d", odom); - int16_t odomLeft = (odom >> 16) & 0xffff; - int16_t odomRight = odom & 0xffff; - - // Debug code to be used for verification - g_odomLeft += odomLeft; - g_odomRight += odomRight; - g_odomEvent += 1; - //if ((g_odomEvent % 50) == 1) { ROS_ERROR("leftOdom %d rightOdom %d", g_odomLeft, g_odomRight); } - - // Due to extreme wheel slip that is required to turn a 4WD robot we are doing a scale factor. - // When doing a rotation on the 4WD robot that is in place where linear velocity is zero - // we will adjust the odom values for wheel joint rotation using the scale factor. - double odom4wdRotationScale = 1.0; - - // Determine if we are rotating then set a scale to account for rotational wheel slip - double near0WheelVel = (double)(WHEEL_VELOCITY_NEAR_ZERO); - double leftWheelVel = g_radiansLeft; // rotational speed of motor - double rightWheelVel = g_radiansRight; // rotational speed of motor - - int leftDir = (leftWheelVel >= (double)(0.0)) ? 1 : -1; - int rightDir = (rightWheelVel >= (double)(0.0)) ? 1 : -1; - int is4wdMode = (fw_params.hw_options & MotorMessage::OPT_DRIVE_TYPE_4WD); - if ( - // Is this in 4wd robot mode - (is4wdMode != 0) - - // Do the joints have Different rotational directions - && ((leftDir + rightDir) == 0) - - // Are Both joints not near joint velocity of 0 - && ((fabs(leftWheelVel) > near0WheelVel) && (fabs(rightWheelVel) > near0WheelVel)) - - // Is the difference of the two absolute values of the joint velocities near zero - && ((fabs(leftWheelVel) - fabs(rightWheelVel)) < near0WheelVel) ) { - - odom4wdRotationScale = g_odom4wdRotationScale; - if ((index % 16) == 1) { // This throttles the messages for rotational torque enhancement - ROS_INFO("ROTATIONAL_SCALING_ACTIVE: odom4wdRotationScale = %4.2f [%4.2f, %4.2f] [%d,%d] opt 0x%x 4wd=%d", - odom4wdRotationScale, leftWheelVel, rightWheelVel, leftDir, rightDir, fw_params.hw_options, is4wdMode); - } - } else { - if (fabs(leftWheelVel) > near0WheelVel) { - ROS_DEBUG("odom4wdRotationScale = %4.2f [%4.2f, %4.2f] [%d,%d] opt 0x%x 4wd=%d", - odom4wdRotationScale, leftWheelVel, rightWheelVel, leftDir, rightDir, fw_params.hw_options, is4wdMode); - } - } - - // Add or subtract from position in radians using the incremental odom value - joints_[WheelJointLocation::Left].position += - ((double)odomLeft / (this->ticks_per_radian * odom4wdRotationScale)); - joints_[WheelJointLocation::Right].position += - ((double)odomRight / (this->ticks_per_radian * odom4wdRotationScale)); - - motor_diag_.odom_update_status.tick(); // Let diag know we got odom - - break; - } - case MotorMessage::REG_BOTH_ERROR: { - std_msgs::Int32 left; - std_msgs::Int32 right; - int32_t speed = mm.getData(); - int16_t leftSpeed = (speed >> 16) & 0xffff; - int16_t rightSpeed = speed & 0xffff; - - left.data = leftSpeed; - right.data = rightSpeed; - leftError.publish(left); - rightError.publish(right); - break; - } - - case MotorMessage::REG_PWM_BOTH_WHLS: { - int32_t bothPwm = mm.getData(); - motor_diag_.motorPwmDriveLeft = (bothPwm >> 16) & 0xffff; - motor_diag_.motorPwmDriveRight = bothPwm & 0xffff; - break; - } - - case MotorMessage::REG_LEFT_CURRENT: { - // Motor current is an absolute value and goes up from a nominal count of near 1024 - // So we subtract a nominal offset then multiply count * scale factor to get amps - int32_t data = mm.getData() & 0xffff; - motor_diag_.motorCurrentLeft = - (double)(data - motor_diag_.motorAmpsZeroAdcCount) * MOTOR_AMPS_PER_ADC_COUNT; - break; - } - case MotorMessage::REG_RIGHT_CURRENT: { - // Motor current is an absolute value and goes up from a nominal count of near 1024 - // So we subtract a nominal offset then multiply count * scale factor to get amps - int32_t data = mm.getData() & 0xffff; - motor_diag_.motorCurrentRight = - (double)(data - motor_diag_.motorAmpsZeroAdcCount) * MOTOR_AMPS_PER_ADC_COUNT; - break; - } - - case MotorMessage::REG_HW_OPTIONS: { - int32_t data = mm.getData(); - - // Enable or disable hardware options reported from firmware - motor_diag_.firmware_options = data; - - // Set radians per encoder tic based on encoder specifics - if (data & MotorMessage::OPT_ENC_6_STATE) { - ROS_WARN_ONCE("Encoder Resolution: 'Enhanced'"); - fw_params.hw_options |= MotorMessage::OPT_ENC_6_STATE; - this->ticks_per_radian = this->getWheelTicksPerRadian(); - } else { - ROS_WARN_ONCE("Encoder Resolution: 'Standard'"); - fw_params.hw_options &= ~MotorMessage::OPT_ENC_6_STATE; - this->ticks_per_radian = this->getWheelTicksPerRadian() / (double)(2.0); - } - - if (data & MotorMessage::OPT_WHEEL_TYPE_THIN) { - ROS_WARN_ONCE("Wheel type is: 'thin'"); - fw_params.hw_options |= MotorMessage::OPT_WHEEL_TYPE_THIN; - } else { - ROS_WARN_ONCE("Wheel type is: 'standard'"); - fw_params.hw_options &= ~MotorMessage::OPT_WHEEL_TYPE_THIN; - } - - if (data & MotorMessage::OPT_DRIVE_TYPE_4WD) { - ROS_WARN_ONCE("Drive type is: '4wd'"); - fw_params.hw_options |= MotorMessage::OPT_DRIVE_TYPE_4WD; - } else { - ROS_WARN_ONCE("Drive type is: '2wd'"); - fw_params.hw_options &= ~MotorMessage::OPT_DRIVE_TYPE_4WD; - } - - if (data & MotorMessage::OPT_WHEEL_DIR_REVERSE) { - ROS_WARN_ONCE("Wheel direction is: 'reverse'"); - fw_params.hw_options |= MotorMessage::OPT_WHEEL_DIR_REVERSE; - } else { - ROS_WARN_ONCE("Wheel direction is: 'standard'"); - fw_params.hw_options &= ~MotorMessage::OPT_WHEEL_DIR_REVERSE; - } - break; - } - - case MotorMessage::REG_LIMIT_REACHED: { - int32_t data = mm.getData(); - - if (data & MotorMessage::LIM_M1_PWM) { - ROS_WARN("left PWM limit reached"); - motor_diag_.left_pwm_limit = true; - } - if (data & MotorMessage::LIM_M2_PWM) { - ROS_WARN("right PWM limit reached"); - motor_diag_.right_pwm_limit = true; - } - if (data & MotorMessage::LIM_M1_INTEGRAL) { - ROS_DEBUG("left Integral limit reached"); - motor_diag_.left_integral_limit = true; - } - if (data & MotorMessage::LIM_M2_INTEGRAL) { - ROS_DEBUG("right Integral limit reached"); - motor_diag_.right_integral_limit = true; - } - if (data & MotorMessage::LIM_M1_MAX_SPD) { - ROS_WARN("left Maximum speed reached"); - motor_diag_.left_max_speed_limit = true; - } - if (data & MotorMessage::LIM_M2_MAX_SPD) { - ROS_WARN("right Maximum speed reached"); - motor_diag_.right_max_speed_limit = true; - } - if (data & MotorMessage::LIM_PARAM_LIMIT) { - ROS_WARN_ONCE("parameter limit in firmware"); - motor_diag_.param_limit_in_firmware = true; - } - break; - } - case MotorMessage::REG_BATTERY_VOLTAGE: { - int32_t data = mm.getData(); - sensor_msgs::BatteryState bstate; - bstate.voltage = (float)data * fw_params.battery_voltage_multiplier + - fw_params.battery_voltage_offset; - bstate.current = std::numeric_limits::quiet_NaN(); - bstate.charge = std::numeric_limits::quiet_NaN(); - bstate.capacity = std::numeric_limits::quiet_NaN(); - bstate.design_capacity = std::numeric_limits::quiet_NaN(); - - // Hardcoded to a sealed lead acid 12S battery, but adjustable for future use - bstate.percentage = calculateBatteryPercentage(bstate.voltage, 12, SLA_AGM); - bstate.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; - bstate.power_supply_health = sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; - bstate.power_supply_technology = sensor_msgs::BatteryState::POWER_SUPPLY_TECHNOLOGY_UNKNOWN; - battery_state.publish(bstate); - - motor_diag_.battery_voltage = bstate.voltage; - motor_diag_.battery_voltage_low_level = MotorHardware::fw_params.battery_voltage_low_level; - motor_diag_.battery_voltage_critical = MotorHardware::fw_params.battery_voltage_critical; - break; - } - case MotorMessage::REG_MOT_PWR_ACTIVE: { // Starting with rev 5.0 board we can see power state - int32_t data = mm.getData(); - - if (data & MotorMessage::MOT_POW_ACTIVE) { - if (estop_motor_power_off == true) { - ROS_WARN("Motor power has gone from inactive to active. Most likely from ESTOP switch"); - } - estop_motor_power_off = false; - } else { - if (estop_motor_power_off == false) { - ROS_WARN("Motor power has gone inactive. Most likely from ESTOP switch active"); - } - estop_motor_power_off = true; - } - motor_diag_.estop_motor_power_off = estop_motor_power_off; // A copy for diagnostics topic - - std_msgs::Bool estop_message; - estop_message.data = !estop_motor_power_off; - motor_power_active.publish(estop_message); - } - - case MotorMessage::REG_TINT_BOTH_WHLS: { // As of v41 show time between wheel enc edges - int32_t data = mm.getData(); - uint16_t leftTickSpacing = (data >> 16) & 0xffff; - uint16_t rightTickSpacing = data & 0xffff; - uint16_t tickCap = 0; // We can cap the max value if desired - - if ((tickCap > 0) && (leftTickSpacing > tickCap)) { leftTickSpacing = tickCap; } - if ((tickCap > 0) && (rightTickSpacing > tickCap)) { rightTickSpacing = tickCap; } - - // Publish the two wheel tic intervals - std_msgs::Int32 leftInterval; - std_msgs::Int32 rightInterval; - - leftInterval.data = leftTickSpacing; - rightInterval.data = rightTickSpacing; - - // Only publish the tic intervals when wheels are moving - if (data > 1) { // Optionally show the intervals for debug - leftTickInterval.publish(leftInterval); - rightTickInterval.publish(rightInterval); - - ROS_DEBUG("Tic Ints M1 %d [0x%x] M2 %d [0x%x]", - leftTickSpacing, leftTickSpacing, rightTickSpacing, rightTickSpacing); - } - } - default: - break; - } - } - } -} - - -void MotorHardware::publishFirmwareInfo(){ - //publish the firmware version and optional date to ROS - if(firmware_version > 0){ - - std_msgs::String fstate; - fstate.data = "v"+std::to_string(firmware_version); - - if(firmware_date > 0){ - std::stringstream stream; - stream << std::hex << firmware_date; - std::string daycode(stream.str()); - - fstate.data +=" "+daycode; - } - - firmware_state.publish(fstate); - } -} - -// calculateBatteryPercentage() takes in battery voltage, number of cells, and type; returns approximate percentage -// -// A battery type is defined by an array of 11 values, each corresponding to one 10% sized step from 0% to 100%. -// If the values fall between the steps, they are linearly interpolated to give a more accurate reading. -// -float MotorHardware::calculateBatteryPercentage(float voltage, int cells, const float* type) { - float onecell = voltage / (float)cells; - - if(onecell >= type[10]) - return 1.0; - else if(onecell <= type[0]) - return 0.0; - - int upper = 0; - int lower = 0; - - for(int i = 0; i < 11; i++){ - if(onecell > type[i]){ - lower = i; - }else{ - upper = i; - break; - } - } - - float deltavoltage = type[upper] - type[lower]; - float between_percent = (onecell - type[lower]) / deltavoltage; - - return (float)lower * 0.1 + between_percent * 0.1; -} - -// writeSpeedsInRadians() Take in radians per sec for wheels and send in message to controller -// -// A direct write speeds that allows caller setting speeds in radians -// This interface allows maintaining of system speed in state but override to zero -// which is of value for such a case as ESTOP implementation -// -void MotorHardware::writeSpeedsInRadians(double left_radians, double right_radians) { - MotorMessage both; - both.setRegister(MotorMessage::REG_BOTH_SPEED_SET); - both.setType(MotorMessage::TYPE_WRITE); - - g_radiansLeft = left_radians; - g_radiansRight = right_radians; - - // We are going to implement a message when robot is moving very fast or rotating very fast - if (((left_radians / VELOCITY_READ_PER_SECOND) > HIGH_SPEED_RADIANS) || - ((right_radians / VELOCITY_READ_PER_SECOND) > HIGH_SPEED_RADIANS)) { - ROS_WARN("Wheel rotation at high radians per sec. Left %f rad/s Right %f rad/s", - left_radians, right_radians); - } - - int16_t left_speed = calculateSpeedFromRadians(left_radians); - int16_t right_speed = calculateSpeedFromRadians(right_radians); - - // The masking with 0x0000ffff is necessary for handling -ve numbers - int32_t data = (left_speed << 16) | (right_speed & 0x0000ffff); - both.setData(data); - - std_msgs::Int32 smsg; - smsg.data = left_speed; - - motor_serial_->transmitCommand(both); - - // ROS_ERROR("velocity_command %f rad/s %f rad/s", - // joints_[WheelJointLocation::Left].velocity_command, joints_[WheelJointLocation::Right].velocity_command); - // joints_[LEFT_WHEEL_JOINT].velocity_command, joints_[RIGHT_WHEEL_JOINT].velocity_command); - // ROS_ERROR("SPEEDS %d %d", left.getData(), right.getData()); -} - -// writeSpeeds() Take in radians per sec for wheels and send in message to controller -// -// Legacy interface where no speed overrides are supported -// -void MotorHardware::writeSpeeds() { - // This call pulls in speeds from the joints array maintained by other layers - - double left_radians = joints_[WheelJointLocation::Left].velocity_command; - double right_radians = joints_[WheelJointLocation::Right].velocity_command; - - writeSpeedsInRadians(left_radians, right_radians); -} - -// areWheelSpeedsLower() Determine if all wheel joint speeds are below given threshold -// -int MotorHardware::areWheelSpeedsLower(double wheelSpeedRadPerSec) { - int retCode = 0; - - // This call pulls in speeds from the joints array maintained by other layers - - double left_radians = joints_[WheelJointLocation::Left].velocity_command; - double right_radians = joints_[WheelJointLocation::Right].velocity_command; - - if ((std::abs(left_radians) < wheelSpeedRadPerSec) && - (std::abs(right_radians) < wheelSpeedRadPerSec)) { - retCode = 1; - } - - return retCode; -} - -void MotorHardware::requestFirmwareVersion() { - MotorMessage fw_version_msg; - fw_version_msg.setRegister(MotorMessage::REG_FIRMWARE_VERSION); - fw_version_msg.setType(MotorMessage::TYPE_READ); - fw_version_msg.setData(0); - motor_serial_->transmitCommand(fw_version_msg); -} - -// Firmware date register implemented as of MIN_FW_FIRMWARE_DATE -void MotorHardware::requestFirmwareDate() { - MotorMessage fw_date_msg; - fw_date_msg.setRegister(MotorMessage::REG_FIRMWARE_DATE); - fw_date_msg.setType(MotorMessage::TYPE_READ); - fw_date_msg.setData(0); - motor_serial_->transmitCommand(fw_date_msg); -} - -// Request the MCB system event register -void MotorHardware::requestSystemEvents() { - MotorMessage sys_event_msg; - sys_event_msg.setRegister(MotorMessage::REG_SYSTEM_EVENTS); - sys_event_msg.setType(MotorMessage::TYPE_READ); - sys_event_msg.setData(0); - motor_serial_->transmitCommand(sys_event_msg); -} - -// Read the wheel currents in amps -void MotorHardware::getMotorCurrents(double ¤tLeft, double ¤tRight) { - currentLeft = motor_diag_.motorCurrentLeft; - currentRight = motor_diag_.motorCurrentRight; - return; -} - - -// Due to greatly limited pins on the firmware processor the host figures out the hardware rev and sends it to fw -// The hardware version is 0x0000MMmm where MM is major rev like 4 and mm is minor rev like 9 for first units. -// The 1st firmware version this is set for is 32, before it was always 1 -void MotorHardware::setHardwareVersion(int32_t hardware_version) { - ROS_INFO("setting hardware_version to %x", (int)hardware_version); - this->hardware_version = hardware_version; - MotorMessage mm; - mm.setRegister(MotorMessage::REG_HARDWARE_VERSION); - mm.setType(MotorMessage::TYPE_WRITE); - mm.setData(hardware_version); - motor_serial_->transmitCommand(mm); -} - -// Setup the controller board threshold to put into force estop protection on boards prior to rev 5.0 with hardware support -void MotorHardware::setEstopPidThreshold(int32_t estop_pid_threshold) { - ROS_INFO("setting Estop PID threshold to %d", (int)estop_pid_threshold); - MotorMessage mm; - mm.setRegister(MotorMessage::REG_PID_MAX_ERROR); - mm.setType(MotorMessage::TYPE_WRITE); - mm.setData(estop_pid_threshold); - motor_serial_->transmitCommand(mm); -} - -// Setup the controller board to have estop button state detection feature enabled or not -void MotorHardware::setEstopDetection(int32_t estop_detection) { - ROS_INFO("setting estop button detection to %x", (int)estop_detection); - MotorMessage mm; - mm.setRegister(MotorMessage::REG_ESTOP_ENABLE); - mm.setType(MotorMessage::TYPE_WRITE); - mm.setData(estop_detection); - motor_serial_->transmitCommand(mm); -} - -// Returns true if estop switch is active OR if motor power is off somehow off -bool MotorHardware::getEstopState(void) { - return estop_motor_power_off; -} - -// Setup the controller board maximum settable motor forward speed -void MotorHardware::setMaxFwdSpeed(int32_t max_speed_fwd) { - ROS_INFO("setting max motor forward speed to %d", (int)max_speed_fwd); - MotorMessage mm; - mm.setRegister(MotorMessage::REG_MAX_SPEED_FWD); - mm.setType(MotorMessage::TYPE_WRITE); - mm.setData(max_speed_fwd); - motor_serial_->transmitCommand(mm); -} - -// Setup the Wheel Type. Overrides mode in use on hardware -// This used to only be standard but THIN_WHEELS were added in Jun 2020 -void MotorHardware::setWheelType(int32_t new_wheel_type) { - - MotorMessage ho; - switch(new_wheel_type) { - case MotorMessage::OPT_WHEEL_TYPE_STANDARD: - case MotorMessage::OPT_WHEEL_TYPE_THIN: - ROS_INFO_ONCE("setting MCB wheel type %d", (int)new_wheel_type); - wheel_type = new_wheel_type; - ho.setRegister(MotorMessage::REG_WHEEL_TYPE); - ho.setType(MotorMessage::TYPE_WRITE); - ho.setData(wheel_type); - motor_serial_->transmitCommand(ho); - break; - default: - ROS_ERROR("Illegal MCB wheel type 0x%x will not be set!", (int)new_wheel_type); - } -} - -// A simple fetch of the wheel_gear_ratio -double MotorHardware::getWheelGearRatio(void) { - return this->wheel_gear_ratio; -} - -// A simple fetch of the encoder ticks per radian of wheel rotation -double MotorHardware::getWheelTicksPerRadian(void) { - double result = this->getWheelGearRatio() * TICKS_PER_RAD_FROM_GEAR_RATIO; - return result; -} - -// Setup the local Wheel gear ratio so the motor hardware layer can adjust wheel odom reporting -// This gear ratio was introduced for a new version of the standard wheels in late 2021 production -// This is slightly more complex in that it is compounded with the now default 6 state encoder hw option -void MotorHardware::setWheelGearRatio(double new_wheel_gear_ratio) { - // This gear ratio is not used by the firmware so it is a simple state element in this module - this->wheel_gear_ratio = new_wheel_gear_ratio; - this->ticks_per_radian = this->getWheelTicksPerRadian(); // Need to also reset ticks_per_radian - if ((fw_params.hw_options & MotorMessage::OPT_ENC_6_STATE) == 0) { - this->ticks_per_radian = this->ticks_per_radian / (double)(2.0); // 3 state was half - } - ROS_INFO("Setting Wheel gear ratio to %6.4f and tics_per_radian to %6.4f", - this->wheel_gear_ratio, this->ticks_per_radian); -} - -// Setup the Drive Type. Overrides mode in use on hardware -// This used to only be 2WD and use of THIN_WHEELS set 4WD -// We are not trying to decouple wheel type from drive type -// This register always existed but was a do nothing till firmware v42 -void MotorHardware::setDriveType(int32_t drive_type) { - ROS_INFO_ONCE("setting MCB drive type %d", (int)drive_type); - MotorMessage mm; - mm.setRegister(MotorMessage::REG_DRIVE_TYPE); - mm.setType(MotorMessage::TYPE_WRITE); - mm.setData(drive_type); - motor_serial_->transmitCommand(mm); -} - -// Setup the PID control options. Overrides modes in use on hardware -void MotorHardware::setPidControl(int32_t pid_control_word) { - ROS_INFO_ONCE("setting MCB pid control word to 0x%x", (int)pid_control_word); - MotorMessage mm; - mm.setRegister(MotorMessage::REG_PID_CONTROL); - mm.setType(MotorMessage::TYPE_WRITE); - mm.setData(pid_control_word); - motor_serial_->transmitCommand(mm); -} - -// Do a one time NULL of the wheel setpoint based on current position error -// This allows to relieve stress in static situation where wheels cannot slip to match setpoint -void MotorHardware::nullWheelErrors(void) { - ROS_DEBUG("Nulling MCB wheel errors using current wheel positions"); - MotorMessage mm; - mm.setRegister(MotorMessage::REG_WHEEL_NULL_ERR); - mm.setType(MotorMessage::TYPE_WRITE); - mm.setData(MotorOrWheelNumber::Motor_M1|MotorOrWheelNumber::Motor_M2); - motor_serial_->transmitCommand(mm); -} - -// Setup the Wheel direction. Overrides mode in use on hardware -// This allows for customer to install wheels on cutom robots as they like -void MotorHardware::setWheelDirection(int32_t wheel_direction) { - ROS_INFO("setting MCB wheel direction to %d", (int)wheel_direction); - MotorMessage ho; - ho.setRegister(MotorMessage::REG_WHEEL_DIR); - ho.setType(MotorMessage::TYPE_WRITE); - ho.setData(wheel_direction); - motor_serial_->transmitCommand(ho); -} - -// A simple fetch of the pid_control word from firmware params -int MotorHardware::getPidControlWord(void) { - int pidControlWord; - pidControlWord = motor_diag_.fw_pid_control; - return pidControlWord; -} - -// Read the controller board option switch itself that resides on the I2C bus but is on the MCB -// This call inverts the bits because a shorted option switch is a 0 where we want it as 1 -// If return is negative something went wrong -int MotorHardware::getOptionSwitch(void) { - uint8_t buf[16]; - int retBits = 0; - ROS_INFO("reading MCB option switch on the I2C bus"); - int retCount = i2c_BufferRead(I2C_DEVICE, I2C_PCF8574_8BIT_ADDR, &buf[0], -1, 1); - if (retCount < 0) { - ROS_ERROR("Error %d in reading MCB option switch at 8bit Addr 0x%x", - retCount, I2C_PCF8574_8BIT_ADDR); - retBits = retCount; - } else if (retCount != 1) { - ROS_ERROR("Cannot read byte from MCB option switch at 8bit Addr 0x%x", I2C_PCF8574_8BIT_ADDR); - retBits = -1; - } else { - retBits = (0xff) & ~buf[0]; - } - - return retBits; -} - -// Setup the controller board option switch register which comes from the I2C 8-bit IO chip on MCB -void MotorHardware::setOptionSwitchReg(int32_t option_switch_bits) { - ROS_INFO("setting MCB option switch register to 0x%x", (int)option_switch_bits); - MotorMessage os; - os.setRegister(MotorMessage::REG_OPTION_SWITCH); - os.setType(MotorMessage::TYPE_WRITE); - os.setData(option_switch_bits); - motor_serial_->transmitCommand(os); -} - -// Setup the controller board system event register or clear bits in the register -void MotorHardware::setSystemEvents(int32_t system_events) { - ROS_INFO("setting MCB system event register to %d", (int)system_events); - MotorMessage se; - se.setRegister(MotorMessage::REG_SYSTEM_EVENTS); - se.setType(MotorMessage::TYPE_WRITE); - se.setData(system_events); - motor_serial_->transmitCommand(se); -} - -// Setup the controller board maximum settable motor reverse speed -void MotorHardware::setMaxRevSpeed(int32_t max_speed_rev) { - ROS_INFO("setting max motor reverse speed to %d", (int)max_speed_rev); - MotorMessage mm; - mm.setRegister(MotorMessage::REG_MAX_SPEED_REV); - mm.setType(MotorMessage::TYPE_WRITE); - mm.setData(max_speed_rev); - motor_serial_->transmitCommand(mm); -} - -// Setup the controller board maximum PWM level allowed for a motor -void MotorHardware::setMaxPwm(int32_t max_pwm) { - ROS_INFO("setting max motor PWM to %x", (int)max_pwm); - MotorMessage mm; - mm.setRegister(MotorMessage::REG_MAX_PWM); - mm.setType(MotorMessage::TYPE_WRITE); - mm.setData(max_pwm); - motor_serial_->transmitCommand(mm); -} - -void MotorHardware::setDeadmanTimer(int32_t deadman_timer) { - ROS_ERROR("setting deadman to %d", (int)deadman_timer); - MotorMessage mm; - mm.setRegister(MotorMessage::REG_DEADMAN); - mm.setType(MotorMessage::TYPE_WRITE); - mm.setData(deadman_timer); - motor_serial_->transmitCommand(mm); -} - -void MotorHardware::setDeadzoneEnable(int32_t deadzone_enable) { - ROS_ERROR("setting deadzone enable to %d", (int)deadzone_enable); - MotorMessage mm; - mm.setRegister(MotorMessage::REG_DEADZONE); - mm.setType(MotorMessage::TYPE_WRITE); - mm.setData(deadman_timer); - motor_serial_->transmitCommand(mm); -} - -void MotorHardware::setParams(FirmwareParams fp) { - fw_params.pid_proportional = fp.pid_proportional; - fw_params.pid_integral = fp.pid_integral; - fw_params.pid_derivative = fp.pid_derivative; - fw_params.pid_velocity = fp.pid_velocity; - fw_params.pid_denominator = fp.pid_denominator; - fw_params.pid_moving_buffer_size = fp.pid_moving_buffer_size; - fw_params.pid_denominator = fp.pid_denominator; - fw_params.pid_control = fp.pid_control; - fw_params.max_pwm = fp.max_pwm; - fw_params.estop_pid_threshold = fp.estop_pid_threshold; -} - -// Forces next calls to sendParams() to always update each parameter. -// KEEP THIS IN SYNC WITH CHANGES TO sendParams() -void MotorHardware::forcePidParamUpdates() { - - // Reset each of the flags that causes parameters to be sent to MCB by sendParams() - prev_fw_params.pid_proportional = -1; - prev_fw_params.pid_integral = -1; - prev_fw_params.pid_derivative = -1; - prev_fw_params.pid_velocity = -1; - prev_fw_params.pid_denominator = -1; - prev_fw_params.pid_moving_buffer_size = -1; - prev_fw_params.max_pwm = -1; - prev_fw_params.pid_control = 1; -} - -void MotorHardware::sendParams() { - std::vector commands; - - // ROS_ERROR("sending PID %d %d %d %d", - //(int)p_value, (int)i_value, (int)d_value, (int)denominator_value); - - // Only send one register at a time to avoid overwhelming serial comms - // SUPPORT NOTE! Adjust modulo for total parameters in the cycle - // and be sure no duplicate modulos are used! - int cycle = (sendPid_count++) % num_fw_params; // MUST BE THE TOTAL NUMBER IN THIS HANDLING - - if (cycle == 0 && - fw_params.pid_proportional != prev_fw_params.pid_proportional) { - ROS_WARN("Setting PidParam P to %d", fw_params.pid_proportional); - prev_fw_params.pid_proportional = fw_params.pid_proportional; - motor_diag_.fw_pid_proportional = fw_params.pid_proportional; - MotorMessage p; - p.setRegister(MotorMessage::REG_PARAM_P); - p.setType(MotorMessage::TYPE_WRITE); - p.setData(fw_params.pid_proportional); - commands.push_back(p); - } - - if (cycle == 1 && fw_params.pid_integral != prev_fw_params.pid_integral) { - ROS_WARN("Setting PidParam I to %d", fw_params.pid_integral); - prev_fw_params.pid_integral = fw_params.pid_integral; - motor_diag_.fw_pid_integral = fw_params.pid_integral; - MotorMessage i; - i.setRegister(MotorMessage::REG_PARAM_I); - i.setType(MotorMessage::TYPE_WRITE); - i.setData(fw_params.pid_integral); - commands.push_back(i); - } - - if (cycle == 2 && - fw_params.pid_derivative != prev_fw_params.pid_derivative) { - ROS_WARN("Setting PidParam D to %d", fw_params.pid_derivative); - prev_fw_params.pid_derivative = fw_params.pid_derivative; - motor_diag_.fw_pid_derivative = fw_params.pid_derivative; - MotorMessage d; - d.setRegister(MotorMessage::REG_PARAM_D); - d.setType(MotorMessage::TYPE_WRITE); - d.setData(fw_params.pid_derivative); - commands.push_back(d); - } - - if (cycle == 3 && (motor_diag_.firmware_version >= MIN_FW_PID_V_TERM) && - fw_params.pid_velocity != prev_fw_params.pid_velocity) { - ROS_WARN("Setting PidParam V to %d", fw_params.pid_velocity); - prev_fw_params.pid_velocity = fw_params.pid_velocity; - motor_diag_.fw_pid_velocity = fw_params.pid_velocity; - MotorMessage v; - v.setRegister(MotorMessage::REG_PARAM_V); - v.setType(MotorMessage::TYPE_WRITE); - v.setData(fw_params.pid_velocity); - commands.push_back(v); - } - - if (cycle == 4 && - fw_params.pid_denominator != prev_fw_params.pid_denominator) { - ROS_WARN("Setting PidParam Denominator to %d", fw_params.pid_denominator); - prev_fw_params.pid_denominator = fw_params.pid_denominator; - motor_diag_.fw_pid_denominator = fw_params.pid_denominator; - MotorMessage denominator; - denominator.setRegister(MotorMessage::REG_PARAM_C); - denominator.setType(MotorMessage::TYPE_WRITE); - denominator.setData(fw_params.pid_denominator); - commands.push_back(denominator); - } - - if (cycle == 5 && - fw_params.pid_moving_buffer_size != - prev_fw_params.pid_moving_buffer_size) { - ROS_WARN("Setting PidParam D window to %d", fw_params.pid_moving_buffer_size); - prev_fw_params.pid_moving_buffer_size = - fw_params.pid_moving_buffer_size; - motor_diag_.fw_pid_moving_buffer_size = fw_params.pid_moving_buffer_size; - MotorMessage winsize; - winsize.setRegister(MotorMessage::REG_MOVING_BUF_SIZE); - winsize.setType(MotorMessage::TYPE_WRITE); - winsize.setData(fw_params.pid_moving_buffer_size); - commands.push_back(winsize); - } - - if (cycle == 6 && - fw_params.max_pwm != prev_fw_params.max_pwm) { - ROS_WARN("Setting PidParam max_pwm to %d", fw_params.max_pwm); - prev_fw_params.max_pwm = fw_params.max_pwm; - motor_diag_.fw_max_pwm = fw_params.max_pwm; - MotorMessage maxpwm; - maxpwm.setRegister(MotorMessage::REG_MAX_PWM); - maxpwm.setType(MotorMessage::TYPE_WRITE); - maxpwm.setData(fw_params.max_pwm); - commands.push_back(maxpwm); - } - - if (cycle == 7 && - fw_params.pid_control != prev_fw_params.pid_control) { - ROS_WARN("Setting PidParam pid_control to %d", fw_params.pid_control); - prev_fw_params.pid_control = fw_params.pid_control; - motor_diag_.fw_pid_control = fw_params.pid_control; - MotorMessage mmsg; - mmsg.setRegister(MotorMessage::REG_PID_CONTROL); - mmsg.setType(MotorMessage::TYPE_WRITE); - mmsg.setData(fw_params.pid_control); - commands.push_back(mmsg); - } - - // SUPPORT NOTE! Adjust max modulo for total parameters in the cycle, be sure no duplicates used! - - if (commands.size() != 0) { - motor_serial_->transmitCommands(commands); - } -} - -// Get current battery voltage -float MotorHardware::getBatteryVoltage(void) { - return motor_diag_.battery_voltage; // We keep battery_voltage in diagnostic context -} - -void MotorHardware::setDebugLeds(bool led_1, bool led_2) { - std::vector commands; - - MotorMessage led1; - led1.setRegister(MotorMessage::REG_LED_1); - led1.setType(MotorMessage::TYPE_WRITE); - if (led_1) { - led1.setData(0x00000001); - } else { - led1.setData(0x00000000); - } - commands.push_back(led1); - - MotorMessage led2; - led2.setRegister(MotorMessage::REG_LED_2); - led2.setType(MotorMessage::TYPE_WRITE); - if (led_2) { - led2.setData(0x00000001); - } else { - led2.setData(0x00000000); - } - commands.push_back(led2); - - motor_serial_->transmitCommands(commands); -} - -// calculate the binary speed value sent to motor controller board -// using an input expressed in radians. -// The firmware uses the same speed value no matter what type of encoder is used -int16_t MotorHardware::calculateSpeedFromRadians(double radians) { - int16_t speed; - double encoderFactor = 1.0; - double speedFloat; - - // The firmware accepts same units for speed value - // and will deal with it properly depending on encoder handling in use - if (fw_params.hw_options & MotorMessage::OPT_ENC_6_STATE) { - encoderFactor = (double)(0.5); - } - - speedFloat = encoderFactor * radians * ((getWheelTicksPerRadian() * (double)(4.0)) / VELOCITY_READ_PER_SECOND); - speed = boost::math::iround(speedFloat); - - return speed; -} - -double MotorHardware::calculateRadiansFromTicks(int16_t ticks) { - double result; - - result = ((double)ticks * VELOCITY_READ_PER_SECOND) / (getWheelTicksPerRadian() * (double)(4.0)); - return result; -} - -// Diagnostics Status Updater Functions -using diagnostic_updater::DiagnosticStatusWrapper; -using diagnostic_msgs::DiagnosticStatus; - -void MotorDiagnostics::firmware_status(DiagnosticStatusWrapper &stat) { - stat.add("Firmware Version", firmware_version); - if (firmware_version == 0) { - stat.summary(DiagnosticStatus::ERROR, "No firmware version reported. Power may be off."); - } - else if (firmware_version < MIN_FW_RECOMMENDED) { - stat.summary(DiagnosticStatus::WARN, "Firmware is older than recommended! You must update firmware!"); - } - else { - stat.summary(DiagnosticStatus::OK, "Firmware version is OK"); - } -} - -void MotorDiagnostics::firmware_date_status(DiagnosticStatusWrapper &stat) { - - // Only output status if the firmware daycode is supported - if (firmware_version >= MIN_FW_FIRMWARE_DATE) { - std::stringstream stream; - stream << std::hex << firmware_date; - std::string daycode(stream.str()); - - stat.add("Firmware Date", daycode); - stat.summary(DiagnosticStatus::OK, "Firmware daycode format is YYYYMMDD"); - } -} - -// When a firmware limit condition is reported the diagnostic topic reports it. -// Once the report is made the condition is cleared till next time firmware reports that limit -void MotorDiagnostics::limit_status(DiagnosticStatusWrapper &stat) { - stat.summary(DiagnosticStatus::OK, "Limits reached:"); - if (left_pwm_limit) { - stat.mergeSummary(DiagnosticStatusWrapper::ERROR, " left pwm,"); - left_pwm_limit = false; - } - if (right_pwm_limit) { - stat.mergeSummary(DiagnosticStatusWrapper::ERROR, " right pwm,"); - right_pwm_limit = false; - } - if (left_integral_limit) { - stat.mergeSummary(DiagnosticStatusWrapper::WARN, " left integral,"); - left_integral_limit = false; - } - if (right_integral_limit) { - stat.mergeSummary(DiagnosticStatusWrapper::WARN, " right integral,"); - right_integral_limit = false; - } - if (left_max_speed_limit) { - stat.mergeSummary(DiagnosticStatusWrapper::WARN, " left speed,"); - left_max_speed_limit = false; - } - if (right_max_speed_limit) { - stat.mergeSummary(DiagnosticStatusWrapper::WARN, " right speed,"); - right_max_speed_limit = false; - } - if (param_limit_in_firmware) { - // A parameter was sent to firmware that was out of limits for the firmware register - stat.mergeSummary(DiagnosticStatusWrapper::WARN, " firmware limit,"); - param_limit_in_firmware = false; - } -} - -void MotorDiagnostics::battery_status(DiagnosticStatusWrapper &stat) { - stat.add("Battery Voltage", battery_voltage); - if (battery_voltage < battery_voltage_low_level) { - stat.summary(DiagnosticStatusWrapper::WARN, "Battery low"); - } - else if (battery_voltage < battery_voltage_critical) { - stat.summary(DiagnosticStatusWrapper::ERROR, "Battery critical"); - } - else { - stat.summary(DiagnosticStatusWrapper::OK, "Battery OK"); - } -} - -// PID parameters for motor control -void MotorDiagnostics::motor_pid_p_status(DiagnosticStatusWrapper &stat) { - stat.add("PidParam P", fw_pid_proportional); - stat.summary(DiagnosticStatus::OK, "PID Parameter P"); -} -void MotorDiagnostics::motor_pid_i_status(DiagnosticStatusWrapper &stat) { - stat.add("PidParam I", fw_pid_integral); - stat.summary(DiagnosticStatus::OK, "PID Parameter I"); -} -void MotorDiagnostics::motor_pid_d_status(DiagnosticStatusWrapper &stat) { - stat.add("PidParam D", fw_pid_derivative); - stat.summary(DiagnosticStatus::OK, "PID Parameter D"); -} -void MotorDiagnostics::motor_pid_v_status(DiagnosticStatusWrapper &stat) { - stat.add("PidParam V", fw_pid_velocity); - stat.summary(DiagnosticStatus::OK, "PID Parameter V"); -} -void MotorDiagnostics::motor_max_pwm_status(DiagnosticStatusWrapper &stat) { - stat.add("PidParam MaxPWM", fw_max_pwm); - stat.summary(DiagnosticStatus::OK, "PID Max PWM"); -} - - -void MotorDiagnostics::motor_power_status(DiagnosticStatusWrapper &stat) { - stat.add("Motor Power", !estop_motor_power_off); - if (estop_motor_power_off == false) { - stat.summary(DiagnosticStatusWrapper::OK, "Motor power on"); - } - else { - stat.summary(DiagnosticStatusWrapper::WARN, "Motor power off"); - } -} - - -// Show firmware options and give readable decoding of the meaning of the bits -void MotorDiagnostics::firmware_options_status(DiagnosticStatusWrapper &stat) { - stat.add("Firmware Options", firmware_options); - std::string option_descriptions(""); - if (firmware_options & MotorMessage::OPT_ENC_6_STATE) { - option_descriptions += "High resolution encoders"; - } else { - option_descriptions += "Standard resolution encoders"; - } - if (firmware_options & MotorMessage::OPT_WHEEL_TYPE_THIN) { - option_descriptions += ", Thin gearless wheels"; - } else { - option_descriptions += ", Standard wheels"; - } - if (firmware_options & MotorMessage::OPT_DRIVE_TYPE_4WD) { - option_descriptions += ", 4 wheel drive"; - } else { - option_descriptions += ", 2 wheel drive"; - } - if (firmware_options & MotorMessage::OPT_WHEEL_DIR_REVERSE) { - // Only indicate wheel reversal if that has been set as it is non-standard - option_descriptions += ", Reverse polarity wheels"; - } - stat.summary(DiagnosticStatusWrapper::OK, option_descriptions); -} - -// i2c_BufferRead() A host OS system specific utility to open, read, close from an I2C device -// -// The I2C address is the 8-bit address which is the 7-bit addr shifted left in some code -// If chipRegAddr is greater than 1 we write this out for the internal chip address for the following read(s) -// -// Returns number of bytes read where 0 or less implies some form of failure -// -// NOTE: The i2c8bitAddr will be shifted right one bit to use as 7-bit I2C addr -// -static int i2c_BufferRead(const char *i2cDevFile, uint8_t i2c8bitAddr, - uint8_t *pBuffer, int16_t chipRegAddr, uint16_t NumBytesToRead) -{ - int bytesRead = 0; - int retCode = 0; - - int fd; // File descrition - int address = i2c8bitAddr >> 1; // Address of the I2C device - uint8_t buf[8]; // Buffer for data being written to the i2c device - - if ((fd = open(i2cDevFile, O_RDWR)) < 0) { // Open port for reading and writing - retCode = -2; - ROS_ERROR("Cannot open I2C def of %s with error %s", i2cDevFile, strerror(errno)); - goto exitWithNoClose; - } - - // The ioctl here will address the I2C slave device making it ready for 1 or more other bytes - if (ioctl(fd, I2C_SLAVE, address) != 0) { // Set the port options and addr of the dev - retCode = -3; - ROS_ERROR("Failed to get bus access to I2C device %s! ERROR: %s", i2cDevFile, strerror(errno)); - goto exitWithFileClose; - } - - if (chipRegAddr < 0) { // Suppress reg address if negative value was used - buf[0] = (uint8_t)(chipRegAddr); // Internal chip register address - if ((write(fd, buf, 1)) != 1) { // Write both bytes to the i2c port - retCode = -4; - goto exitWithFileClose; - } - } - - bytesRead = read(fd, pBuffer, NumBytesToRead); - if (bytesRead != NumBytesToRead) { // verify the number of bytes we requested were read - retCode = -9; - goto exitWithFileClose; - } - retCode = bytesRead; - - exitWithFileClose: - close(fd); - - exitWithNoClose: - - return retCode; -} diff --git a/src/motor_message.cc b/src/motor_message.cc deleted file mode 100644 index 74134dd..0000000 --- a/src/motor_message.cc +++ /dev/null @@ -1,211 +0,0 @@ -/** -Copyright (c) 2016, Ubiquity Robotics -All rights reserved. - -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. -**/ - -#include -// #include -#include -#include - -uint8_t const MotorMessage::valid_types[] = {TYPE_READ, TYPE_WRITE, - TYPE_RESPONSE, TYPE_ERROR}; - -uint8_t const MotorMessage::valid_registers[] = {REG_STOP_START, - REG_BRAKE_STOP, - REG_SYSTEM_EVENTS, - REG_LEFT_PWM, - REG_RIGHT_PWM, - REG_PWM_BOTH_WHLS, - REG_TINT_BOTH_WHLS, - REG_09, - REG_DRIVE_TYPE, - REG_WHEEL_NULL_ERR, - REG_WHEEL_DIR, - REG_DEADMAN, - REG_LEFT_CURRENT, - REG_RIGHT_CURRENT, - REG_WHEEL_TYPE, - REG_PID_ERROR_CAP, - REG_OPTION_SWITCH, - REG_PWM_OVERRIDE, - REG_PID_CONTROL, - REG_PARAM_V_RDY, - REG_PARAM_P_RDY, - REG_PARAM_I_RDY, - REG_PARAM_D_RDY, - REG_PARAM_C_RDY, - REG_PARAM_V, - REG_PARAM_P, - REG_PARAM_I, - REG_PARAM_D, - REG_PARAM_C, - REG_LED_1, - REG_LED_2, - REG_HARDWARE_VERSION, - REG_FIRMWARE_VERSION, - REG_BATTERY_VOLTAGE, - REG_5V_MAIN_CURRENT, - REG_MAINV_TPOINT, - REG_12V_MAIN_CURRENT, - REG_AUXV_TPOINT, - REG_BATT_VOL_LOW, - REG_VBUF_SIZ, - REG_BOTH_SPEED_SET, - REG_MOVING_BUF_SIZE, - REG_LIMIT_REACHED, - REG_BOTH_ERROR, - REG_BOTH_ODOM, - REG_ROBOT_ID, - REG_MOT_PWR_ACTIVE, - REG_ESTOP_ENABLE, - REG_PID_MAX_ERROR, - REG_MAX_SPEED_FWD, - REG_MAX_SPEED_REV, - REG_MAX_PWM, - REG_HW_OPTIONS, - REG_DEADZONE, - REG_FIRMWARE_DATE, - REG_STEST_REQUEST, - REG_STEST_RESULTS, - DEBUG_50, - DEBUG_51, - DEBUG_52, - DEBUG_53, - DEBUG_54, - DEBUG_55, - DEBUG_56, - DEBUG_57, - DEBUG_58}; - -void MotorMessage::setType(MotorMessage::MessageTypes type) { - if (verifyType(type)) { - this->type = type; - } -} - -MotorMessage::MessageTypes MotorMessage::getType() const { - return static_cast(this->type); -} - -void MotorMessage::setRegister(MotorMessage::Registers reg) { - if (verifyRegister(reg)) { - this->register_addr = reg; - } -} - -MotorMessage::Registers MotorMessage::getRegister() const { - return static_cast(this->register_addr); -} - -void MotorMessage::setData(int32_t data) { - // Spilt 32 bit data (system byte order) into 4 8bit elements in big endian - // (network byte order) - this->data[3] = (data >> 0) & 0xFF; - this->data[2] = (data >> 8) & 0xFF; - this->data[1] = (data >> 16) & 0xFF; - this->data[0] = (data >> 24) & 0xFF; -} - -int32_t MotorMessage::getData() const { - // Take big endian (network byte order) elements and return 32 bit int - return (this->data[0] << 24) | (this->data[1] << 16) | - (this->data[2] << 8) | (this->data[3] << 0); -} - -RawMotorMessage MotorMessage::serialize() const { - RawMotorMessage out; - out[0] = delimeter; - out[1] = (protocol_version << 4) | type; - out[2] = register_addr; - std::copy(data.begin(), data.end(), out.begin() + 3); - out[7] = generateChecksum(out); - return out; -} - -MotorMessage::ErrorCodes MotorMessage::deserialize(const RawMotorMessage &serialized) { - if (serialized[0] == delimeter) { - if ((serialized[1] & 0xF0) == (protocol_version << 4)) { - if (generateChecksum(serialized) == serialized[7]) { - if (verifyType(serialized[1] & 0x0F)) { - if (verifyRegister(serialized[2])) { - this->type = serialized[1] & 0x0F; - this->register_addr = serialized[2]; - std::copy(serialized.begin() + 3, - serialized.begin() + 7, data.begin()); - return MotorMessage::ERR_NONE; - } else - return MotorMessage::ERR_UNKNOWN_REGISTER; - } else - return MotorMessage::ERR_BAD_TYPE;; - } else - return MotorMessage::ERR_BAD_CHECKSUM; - } else - return MotorMessage::ERR_WRONG_PROTOCOL; - } else - return MotorMessage::ERR_DELIMITER; - - // ERROR codes returned are defined in MotorMessage class - // First char not delimiter - // wrong protocol version - // bad checksum - // bad type - // bad register -} - -int MotorMessage::verifyType(uint8_t t) { - // Return 1 good - // Return 0 for bad - for (size_t i = 0; i < sizeof(valid_types) / sizeof(valid_types[0]); ++i) { - if (t == valid_types[i]) return 1; - } - return 0; -} - -int MotorMessage::verifyRegister(uint8_t r) { - // Return 1 good - // Return 0 for bad - for (size_t i = 0; i < sizeof(valid_registers) / sizeof(valid_registers[0]); - ++i) { - if (r == valid_registers[i]) return 1; - } - return 0; -} - -uint8_t MotorMessage::generateChecksum(const RawMotorMessage &data) { - int sum = std::accumulate(data.begin() + 1, data.end() - 1, 0); - - if (sum > 0xFF) { - int tmp; - tmp = sum >> 8; - tmp = tmp << 8; - return 0xFF - (sum - tmp); - } else { - return 0xFF - sum; - } -} diff --git a/src/motor_node.cc b/src/motor_node.cc new file mode 100644 index 0000000..eb96cdd --- /dev/null +++ b/src/motor_node.cc @@ -0,0 +1,591 @@ +/** +Copyright (c) 2016, Ubiquity Robotics +All rights reserved. + +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. +**/ + +//#include +#include +#include "std_msgs/msg/string.hpp" +#include +//#include +#include +//#include +//#include +//#include +#include +#include +//#include "controller_manager/controller_manager.h" + +static const double BILLION = 1000000000.0; + +static FirmwareParams g_firmware_params; +static CommsParams g_serial_params; +static NodeParams g_node_params; + +// TODO: Enhancement - Make WHEEL_SLIP_THRESHOLD be a ROS param +#define WHEEL_SLIP_THRESHOLD (0.08) // Rotation below which we null excess wheel torque in 4wd drive_type +int g_wheel_slip_nulling = 0; + +// Until we have a holdoff for MCB message overruns we do this delay to be cautious +// Twice the period for status reports from MCB +ros::Duration mcbStatusPeriodSec(0.02); + +// Dynamic reconfiguration callback for setting ROS parameters dynamically +void PID_update_callback(const ubiquity_motor::PIDConfig& config, + uint32_t level) { + if (level == 0xFFFFFFFF) { + return; + } + + if (level == 1) { + g_firmware_params.pid_proportional = config.PID_P; + } else if (level == 2) { + g_firmware_params.pid_integral = config.PID_I; + } else if (level == 4) { + g_firmware_params.pid_derivative = config.PID_D; + } else if (level == 8) { + g_firmware_params.pid_denominator = config.PID_C; + } else if (level == 16) { + g_firmware_params.pid_moving_buffer_size = config.PID_W; + } else if (level == 32) { + g_firmware_params.pid_velocity = config.PID_V; + } else if (level == 64) { + g_firmware_params.max_pwm = config.MAX_PWM; + } else { + ROS_ERROR("Unsupported dynamic_reconfigure level %u", level); + } +} + +// The system_control topic is used to be able to stop or start communications from the MCB +// and thus allow live firmware updates or other direct MCB serial communications to happen +// without the main control code trying to talk to the MCB +void SystemControlCallback(const std_msgs::String::ConstPtr& msg) { + ROS_DEBUG("System control msg with content: '%s']", msg->data.c_str()); + + // Manage the complete cut-off of all control to the MCB + // Typically used for firmware upgrade, but could be used for other diagnostics + if (msg->data.find(MOTOR_CONTROL_CMD) != std::string::npos) { + if (msg->data.find(MOTOR_CONTROL_ENABLE) != std::string::npos) {; + if (g_node_params.mcbControlEnabled == 0) { // Only show message if state changes + ROS_INFO("Received System control msg to ENABLE control of the MCB"); + } + g_node_params.mcbControlEnabled = 1; + } else if (msg->data.find(MOTOR_CONTROL_DISABLE) != std::string::npos) { + if (g_node_params.mcbControlEnabled != 0) { // Only show message if state changes + ROS_INFO("Received System control msg to DISABLE control of the MCB"); + } + g_node_params.mcbControlEnabled = 0; + } + } + + // Manage a motor speed override used to allow collision detect motor stopping + if (msg->data.find(MOTOR_SPEED_CONTROL_CMD) != std::string::npos) { + if (msg->data.find(MOTOR_CONTROL_ENABLE) != std::string::npos) {; + if (g_node_params.mcbSpeedEnabled == 0) { // Only show message if state changes + ROS_INFO("Received System control msg to ENABLE control of motor speed"); + } + g_node_params.mcbSpeedEnabled = 1; + } else if (msg->data.find(MOTOR_CONTROL_DISABLE) != std::string::npos) { + if (g_node_params.mcbSpeedEnabled != 0) { // Only show message if state changes + ROS_INFO("Received System control msg to DISABLE control of motor speed"); + } + g_node_params.mcbSpeedEnabled = 0; + } + } +} + +// initMcbParameters() +// +// Setup MCB parameters that are from host level options or settings +// +void initMcbParameters(std::unique_ptr &robot ) +{ + // A full mcb initialization requires high level system overrides to be disabled + g_node_params.mcbControlEnabled = 1; + g_node_params.mcbSpeedEnabled = 1; + + // Force future calls to sendParams() to update current pid parametes on the MCB + robot->forcePidParamUpdates(); + + // Determine the wheel type to be used by the robot base + int32_t wheel_type = MotorMessage::OPT_WHEEL_TYPE_STANDARD; + if (g_node_params.wheel_type == "firmware_default") { + // Here there is no specification so the firmware default will be used + ROS_INFO("Default wheel_type of 'standard' will be used."); + wheel_type = MotorMessage::OPT_WHEEL_TYPE_STANDARD; + } else { + // Any other setting leads to host setting the wheel type + if (g_node_params.wheel_type == "standard") { + wheel_type = MotorMessage::OPT_WHEEL_TYPE_STANDARD; + ROS_INFO("Host is specifying wheel_type of '%s'", "standard"); + } else if (g_node_params.wheel_type == "thin"){ + wheel_type = MotorMessage::OPT_WHEEL_TYPE_THIN; + ROS_INFO("Host is specifying wheel_type of '%s'", "thin"); + + // If thin wheels and no drive_type is in yaml file we will use 4wd + if (g_node_params.drive_type == "firmware_default") { + ROS_INFO("Default to drive_type of 4wd when THIN wheels unless option drive_type is set"); + g_node_params.drive_type = "4wd"; + } + } else { + ROS_WARN("Invalid wheel_type of '%s' specified! Using wheel type of standard", + g_node_params.wheel_type.c_str()); + g_node_params.wheel_type = "standard"; + wheel_type = MotorMessage::OPT_WHEEL_TYPE_STANDARD; + } + } + // Write out the wheel type setting to hardware layer + robot->setWheelType(wheel_type); + robot->wheel_type = wheel_type; + mcbStatusPeriodSec.sleep(); + + // Determine the wheel gear ratio to be used by the robot base + // Firmware does not use this setting so no message to firmware is required + // This gear ratio is contained in the hardware layer so if this node got new setting update hardware layer + robot->setWheelGearRatio(g_node_params.wheel_gear_ratio); + ROS_INFO("Wheel gear ratio of %5.3f will be used.", g_node_params.wheel_gear_ratio); + + // Determine the drive type to be used by the robot base + int32_t drive_type = MotorMessage::OPT_DRIVE_TYPE_STANDARD; + if (g_node_params.drive_type == "firmware_default") { + // Here there is no specification so the firmware default will be used + ROS_INFO("Default drive_type of 'standard' will be used."); + drive_type = MotorMessage::OPT_DRIVE_TYPE_STANDARD; + } else { + // Any other setting leads to host setting the drive type + if (g_node_params.drive_type == "standard") { + drive_type = MotorMessage::OPT_DRIVE_TYPE_STANDARD; + ROS_INFO("Host is specifying drive_type of '%s'", "standard"); + } else if (g_node_params.drive_type == "4wd"){ + drive_type = MotorMessage::OPT_DRIVE_TYPE_4WD; + ROS_INFO("Host is specifying drive_type of '%s'", "4wd"); + } else { + ROS_WARN("Invalid drive_type of '%s' specified! Using drive type of standard", + g_node_params.drive_type.c_str()); + g_node_params.drive_type = "standard"; + drive_type = MotorMessage::OPT_DRIVE_TYPE_STANDARD; + } + } + // Write out the drive type setting to hardware layer + robot->setDriveType(drive_type); + robot->drive_type = drive_type; + mcbStatusPeriodSec.sleep(); + + int32_t wheel_direction = 0; + if (g_node_params.wheel_direction == "firmware_default") { + // Here there is no specification so the firmware default will be used + ROS_INFO("Firmware default wheel_direction will be used."); + } else { + // Any other setting leads to host setting the wheel type + if (g_node_params.wheel_direction == "standard") { + wheel_direction = MotorMessage::OPT_WHEEL_DIR_STANDARD; + ROS_INFO("Host is specifying wheel_direction of '%s'", "standard"); + } else if (g_node_params.wheel_direction == "reverse"){ + wheel_direction = MotorMessage::OPT_WHEEL_DIR_REVERSE; + ROS_INFO("Host is specifying wheel_direction of '%s'", "reverse"); + } else { + ROS_WARN("Invalid wheel_direction of '%s' specified! Using wheel direction of standard", + g_node_params.wheel_direction.c_str()); + g_node_params.wheel_direction = "standard"; + wheel_direction = MotorMessage::OPT_WHEEL_DIR_STANDARD; + } + // Write out the wheel direction setting + robot->setWheelDirection(wheel_direction); + mcbStatusPeriodSec.sleep(); + } + + // Tell the controller board firmware what version the hardware is at this time. + // TODO: Read from I2C. At this time we only allow setting the version from ros parameters + if (robot->firmware_version >= MIN_FW_HW_VERSION_SET) { + ROS_INFO_ONCE("Firmware is version %d. Setting Controller board version to %d", + robot->firmware_version, g_firmware_params.controller_board_version); + robot->setHardwareVersion(g_firmware_params.controller_board_version); + ROS_DEBUG("Controller board version has been set to %d", + g_firmware_params.controller_board_version); + mcbStatusPeriodSec.sleep(); + } + + // Suggest to customer to have current firmware version + if (robot->firmware_version < MIN_FW_SUGGESTED) { + ROS_ERROR_ONCE("Firmware is version V%d. We strongly recommend minimum firmware version of at least V%d", + robot->firmware_version, MIN_FW_SUGGESTED); + mcbStatusPeriodSec.sleep(); + } else { + ROS_INFO_ONCE("Firmware is version V%d. This meets the recommend minimum firmware versionof V%d", + robot->firmware_version, MIN_FW_SUGGESTED); + mcbStatusPeriodSec.sleep(); + } + + // Certain 4WD robots rely on wheels to skid to reach final positions. + // For such robots when loaded down the wheels can get in a state where they cannot skid. + // This leads to motor overheating. This code below sacrifices accurate odometry which + // is not achievable in such robots anyway to relieve high wattage drive power when zero velocity. + g_wheel_slip_nulling = 0; + if ((robot->firmware_version >= MIN_FW_WHEEL_NULL_ERROR) && (g_node_params.drive_type == "4wd")) { + g_wheel_slip_nulling = 1; + ROS_INFO("Wheel slip nulling will be enabled for this 4wd system when velocity remains at zero."); + } + + // Tell the MCB board what the port that is on the Pi I2c says on it (the mcb cannot read it's own switchs!) + // We could re-read periodically but perhaps only every 5-10 sec but should do it from main loop + if (robot->firmware_version >= MIN_FW_OPTION_SWITCH && robot->hardware_version >= MIN_HW_OPTION_SWITCH) { + g_firmware_params.option_switch = robot->getOptionSwitch(); + ROS_INFO("Setting firmware option register to 0x%x.", g_firmware_params.option_switch); + robot->setOptionSwitchReg(g_firmware_params.option_switch); + mcbStatusPeriodSec.sleep(); + } + + if (robot->firmware_version >= MIN_FW_SYSTEM_EVENTS) { + // Start out with zero for system events + robot->setSystemEvents(0); // Clear entire system events register + robot->system_events = 0; + mcbStatusPeriodSec.sleep(); + } + + // Setup other firmware parameters that could come from ROS parameters + if (robot->firmware_version >= MIN_FW_ESTOP_SUPPORT) { + robot->setEstopPidThreshold(g_firmware_params.estop_pid_threshold); + mcbStatusPeriodSec.sleep(); + robot->setEstopDetection(g_firmware_params.estop_detection); + mcbStatusPeriodSec.sleep(); + } + + if (robot->firmware_version >= MIN_FW_MAX_SPEED_AND_PWM) { + robot->setMaxFwdSpeed(g_firmware_params.max_speed_fwd); + mcbStatusPeriodSec.sleep(); + robot->setMaxRevSpeed(g_firmware_params.max_speed_rev); + mcbStatusPeriodSec.sleep(); + } + + return; +} + +int main(int argc, char* argv[]) { + ros::init(argc, argv, "motor_node"); + ros::NodeHandle nh; + + g_firmware_params = FirmwareParams(nh); + g_serial_params = CommsParams(nh); + g_node_params = NodeParams(nh); + + ros::Rate ctrlLoopDelay(g_node_params.controller_loop_rate); + + int lastMcbEnabled = 1; + + // Until we have a holdoff for MCB message overruns we do this delay to be cautious + // Twice the period for status reports from MCB + ros::Duration mcbStatusPeriodSec(0.02); + + std::unique_ptr robot = nullptr; + // Keep trying to open serial + { + int times = 0; + while (ros::ok() && robot.get() == nullptr) { + try { + robot.reset(new MotorHardware(nh, g_node_params, g_serial_params, g_firmware_params)); + } + catch (const serial::IOException& e) { + if (times % 30 == 0) + ROS_FATAL("Error opening serial port %s, trying again", g_serial_params.serial_port.c_str()); + } + ctrlLoopDelay.sleep(); + times++; + } + } + + controller_manager::ControllerManager cm(robot.get(), nh); + + // Subscribe to the topic with overall system control ability + ros::Subscriber sub = nh.subscribe(ROS_TOPIC_SYSTEM_CONTROL, 1000, SystemControlCallback); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&PID_update_callback, _1, _2); + server.setCallback(f); + + robot->setParams(g_firmware_params); + robot->requestFirmwareVersion(); + + // wait for reply then we know firmware version + mcbStatusPeriodSec.sleep(); + + // Make sure firmware is listening + { + robot->diag_updater.broadcast(0, "Establishing communication with motors"); + // Start times counter at 1 to prevent false error print (0 % n = 0) + int times = 1; + while (ros::ok() && robot->firmware_version == 0) { + if (times % 30 == 0) + ROS_ERROR("The Firmware not reporting its version"); + robot->requestFirmwareVersion(); + robot->readInputs(0); + mcbStatusPeriodSec.sleep(); + times++; + } + } + + if (robot->firmware_version >= MIN_FW_FIRMWARE_DATE) { + // If supported by firmware also request date code for this version + ROS_INFO("Requesting Firmware daycode "); + robot->requestFirmwareDate(); + } + + // Setup MCB parameters that are defined by host parameters in most cases + ROS_INFO("Initializing MCB"); + initMcbParameters(robot); + ROS_INFO("Initialization of MCB completed."); + + if (robot->firmware_version >= MIN_FW_SYSTEM_EVENTS) { + // Start out with zero for system events + robot->setSystemEvents(0); // Clear entire system events register + robot->system_events = 0; + mcbStatusPeriodSec.sleep(); + } + + // Send out the refreshable firmware parameters, most are the PID terms + // We must be sure num_fw_params is set to the modulo used in sendParams() + for (int i = 0; i < robot->num_fw_params; i++) { + mcbStatusPeriodSec.sleep(); + robot->sendParams(); + } + + float expectedCycleTime = ctrlLoopDelay.expectedCycleTime().toSec(); + ros::Duration minCycleTime = ros::Duration(0.75 * expectedCycleTime); + ros::Duration maxCycleTime = ros::Duration(1.25 * expectedCycleTime); + + // Clear any commands the robot has at this time + robot->clearCommands(); + + double leftLastWheelPos = 0.0; + double rightLastWheelPos = 0.0; + double leftWheelPos = 0.0; + double rightWheelPos = 0.0; + robot-> getWheelJointPositions(leftLastWheelPos, rightWheelPos); + ros::Duration zeroVelocityTime(0.0); + + // Define a period where if wheels are under stress for this long we back off stress + ros::Duration wheelSlipNullingPeriod(2.0); + int wheelSlipEvents = 0; + + ROS_INFO("Starting motor control node now"); + + // Implement a speed reset while ESTOP is active and a delay after release + double estopReleaseDeadtime = 0.8; + double estopReleaseDelay = 0.0; + + // Setup to be able to do periodic operations based on elapsed times + ros::Time current_time; + ros::Duration sysMaintPeriod(60.0); // A periodic MCB maintenance operation + ros::Duration jointUpdatePeriod(0.25); // A periodic time to update joint velocity + + ros::Time last_loop_time = ros::Time::now(); + ros::Duration elapsed_loop_time; + ros::Time last_sys_maint_time = last_loop_time; + ros::Time last_joint_time = last_loop_time; + ctrlLoopDelay.sleep(); // Do delay to setup periodic loop delays + uint32_t loopIdx = 0; + + while (ros::ok()) { + current_time = ros::Time::now(); + elapsed_loop_time = current_time - last_loop_time; + last_loop_time = current_time; + loopIdx += 1; + + // Speical handling if motor control is disabled. skip the entire loop + if (g_node_params.mcbControlEnabled == 0) { + // Check for if we are just starting to go into idle mode and release MCB + if (lastMcbEnabled == 1) { + ROS_WARN("Motor controller going offline and closing MCB serial port"); + robot->closePort(); + } + lastMcbEnabled = 0; + ctrlLoopDelay.sleep(); // Allow controller to process command + continue; + } + + if (lastMcbEnabled == 0) { // Were disabled but re-enabled so re-setup mcb + bool portOpenStatus; + lastMcbEnabled = 1; + ROS_WARN("Motor controller went from offline to online!"); + portOpenStatus = robot->openPort(); // Must re-open serial port + if (portOpenStatus == true) { + robot->setSystemEvents(0); // Clear entire system events register + robot->system_events = 0; + mcbStatusPeriodSec.sleep(); + + // Setup MCB parameters that are defined by host parameters in most cases + initMcbParameters(robot); + ROS_WARN("Motor controller has been re-initialized as we go back online"); + } else { + // We do not have recovery from this situation and it seems not possible + ROS_ERROR("ERROR in re-opening of the Motor controller!"); + } + } + + // Determine and set wheel velocities in rad/sec from hardware positions in rads + ros::Duration elapsed_time = current_time - last_joint_time; + if (elapsed_time > jointUpdatePeriod) { + last_joint_time = ros::Time::now(); + double leftWheelVel = 0.0; + double rightWheelVel = 0.0; + robot-> getWheelJointPositions(leftWheelPos, rightWheelPos); + leftWheelVel = (leftWheelPos - leftLastWheelPos) / elapsed_time.toSec(); + rightWheelVel = (rightWheelPos - rightLastWheelPos) / elapsed_time.toSec(); + robot->setWheelJointVelocities(leftWheelVel, rightWheelVel); // rad/sec + leftLastWheelPos = leftWheelPos; + rightLastWheelPos = rightWheelPos; + + // Publish motor state at this time + robot->publishMotorState(); + + // Implement static wheel slippage relief + // Deal with auto-null of MCB wheel setpoints if wheel slip nulling is enabled + // We null wheel torque if wheel speed has been very low for a long time + // This would be even better if we only did this when over a certain current is heating the wheel + if (g_wheel_slip_nulling != 0) { + if (robot->areWheelSpeedsLower(WHEEL_SLIP_THRESHOLD) != 0) { + zeroVelocityTime += jointUpdatePeriod; // add to time at zero velocity + if (zeroVelocityTime > wheelSlipNullingPeriod) { + // null wheel error if at zero velocity for the nulling check period + // OPTION: We could also just null wheels at high wheel power + ROS_DEBUG("Applying wheel slip relief now with slip period of %4.1f sec ", + wheelSlipNullingPeriod.toSec()); + wheelSlipEvents += 1; + robot->nullWheelErrors(); + zeroVelocityTime = ros::Duration(0.0); // reset time we have been at zero velocity + } + } else { + zeroVelocityTime = ros::Duration(0.0); // reset time we have been at zero velocity + } + } + } + + // Read in and process all serial packets that came from the MCB. + // The MotorSerial class has been receiving and queing packets from the MCB + robot->readInputs(loopIdx); + + // Here is the update call that ties our motor node to the standard ROS classes. + // We will supply a brief overview of how this ROS robot interfaces to our hardware. + // At the top is the controller_manager and just below it is hardware_interface + // The ROS DiffDriveController works with one 'joint' for each wheel. + // to know and publish /odom and generate the odom to base_footprint in /tf topic + // + // Refer to motor_hardware.cc code and study any interfaces to the joints_ array + // where there is one 'joint' for each wheel. + // The joints_ used by ROS DiffDriveController have these 3 key members: + // position - We incrementally update wheel position (from encoders) + // velocity - We set desired wheel velocity in Rad/Sec + // velocity_command - We are told what speeds to set our MCB hardware with from this + + if ((minCycleTime < elapsed_loop_time) && (elapsed_loop_time < maxCycleTime)) { + cm.update(current_time, elapsed_loop_time); // Key update to controller_manager + } + else { + ROS_WARN("Resetting controller due to time jump %f seconds", + elapsed_loop_time.toSec()); + cm.update(current_time, ctrlLoopDelay.expectedCycleTime(), true); + robot->clearCommands(); + } + robot->setParams(g_firmware_params); + robot->sendParams(); + + // Periodically watch for MCB board having been reset which is an MCB system event + // This is also a good place to refresh or show status that may have changed + elapsed_time = current_time - last_sys_maint_time; + if ((robot->firmware_version >= MIN_FW_SYSTEM_EVENTS) && (elapsed_time > sysMaintPeriod)) { + robot->requestSystemEvents(); + mcbStatusPeriodSec.sleep(); + last_sys_maint_time = ros::Time::now(); + + // See if we are in a low battery voltage state + std::string batStatus = "OK"; + if (robot->getBatteryVoltage() < g_firmware_params.battery_voltage_low_level) { + batStatus = "LOW!"; + } + + // Post a status message for MCB state periodically. This may be nice to do more on as required + ROS_INFO("Battery = %5.2f volts [%s], MCB system events 0x%x, PidCtrl 0x%x, WheelType '%s' DriveType '%s' GearRatio %6.3f", + robot->getBatteryVoltage(), batStatus.c_str(), robot->system_events, robot->getPidControlWord(), + (robot->wheel_type == MotorMessage::OPT_WHEEL_TYPE_THIN) ? "thin" : "standard", + g_node_params.drive_type.c_str(), robot->getWheelGearRatio()); + + // If we detect a power-on of MCB we should re-initialize MCB + if ((robot->system_events & MotorMessage::SYS_EVENT_POWERON) != 0) { + ROS_WARN("Detected Motor controller PowerOn event!"); + robot->setSystemEvents(0); // Clear entire system events register + robot->system_events = 0; + mcbStatusPeriodSec.sleep(); + + // Setup MCB parameters that are defined by host parameters in most cases + initMcbParameters(robot); + ROS_WARN("Motor controller has been re-initialized"); + } + + // a periodic refresh of wheel type which is a safety net due to it's importance. + // This can be removed when a solid message protocol is developed + if (robot->firmware_version >= MIN_FW_WHEEL_TYPE_THIN) { + // Refresh the wheel type setting + robot->setWheelType(robot->wheel_type); + mcbStatusPeriodSec.sleep(); + } + // a periodic refresh of drive type which is a safety net due to it's importance. + if (robot->firmware_version >= MIN_FW_DRIVE_TYPE) { + // Refresh the drive type setting + robot->setDriveType(robot->drive_type); + mcbStatusPeriodSec.sleep(); + } + } + + // Update motor controller speeds unless global disable is set, perhaps for colision safety + if (robot->getEstopState()) { + robot->writeSpeedsInRadians(0.0, 0.0); // We send zero velocity when estop is active + estopReleaseDelay = estopReleaseDeadtime; + } else { + if (estopReleaseDelay > 0.0) { + // Implement a delay after estop release where velocity remains zero + estopReleaseDelay -= (1.0/g_node_params.controller_loop_rate); + robot->writeSpeedsInRadians(0.0, 0.0); + } else { + if (g_node_params.mcbSpeedEnabled != 0) { // A global disable used for safety at node level + robot->writeSpeeds(); // Normal operation using current system speeds + } else { + robot->writeSpeedsInRadians(0.0, 0.0); + } + } + } + + robot->diag_updater.update(); + ctrlLoopDelay.sleep(); // Allow controller to process command + } + + return 0; +} diff --git a/src/motor_serial.cc b/src/motor_serial.cc deleted file mode 100644 index 1de2a21..0000000 --- a/src/motor_serial.cc +++ /dev/null @@ -1,164 +0,0 @@ -/** -Copyright (c) 2016, Ubiquity Robotics -All rights reserved. - -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. -**/ - -#include -#include -#include - -MotorSerial::MotorSerial(const std::string& port, uint32_t baud_rate) - : motors(port, baud_rate, serial::Timeout::simpleTimeout(100)), - serial_errors(0), error_threshold(20) { - serial_thread = boost::thread(&MotorSerial::SerialThread, this); -} - -MotorSerial::~MotorSerial() { - serial_thread.interrupt(); - serial_thread.join(); - motors.close(); -} - -int MotorSerial::transmitCommand(MotorMessage command) { - RawMotorMessage out = command.serialize(); - ROS_DEBUG("out %02x %02x %02x %02x %02x %02x %02x %02x", out[0], out[1], - out[2], out[3], out[4], out[5], out[6], out[7]); - motors.write(out.c_array(), out.size()); - return 0; -} - -int MotorSerial::transmitCommands(const std::vector& commands) { - for (auto& command : commands) { - RawMotorMessage out = command.serialize(); - ROS_DEBUG("out %02x %02x %02x %02x %02x %02x %02x %02x", out[0], out[1], - out[2], out[3], out[4], out[5], out[6], out[7]); - motors.write(out.c_array(), out.size()); - boost::this_thread::sleep(boost::posix_time::milliseconds(2)); - } - return 0; -} - -MotorMessage MotorSerial::receiveCommand() { - MotorMessage mc; - if (!this->output.empty()) { - mc = output.front_pop(); - } - return mc; -} - -int MotorSerial::commandAvailable() { return !output.fast_empty(); } - -void MotorSerial::appendOutput(MotorMessage command) { output.push(command); } - -void MotorSerial::closePort() { return motors.close(); } - -// After we have been offine this is called to re-open serial port -// This returns true if port was open or if port opened with success -bool MotorSerial::openPort() { - bool retCode = true; - - if (motors.isOpen() == true) { - return true; - } - - // Port was closed so must open it using info from prior open() - try { - motors.open(); - } catch (const serial::IOException& e) { - ROS_ERROR("%s", e.what()); - retCode = false; - } catch (const std::invalid_argument) { - ROS_ERROR("MotorSerial::openPort Invalid argument"); - retCode = false; - } catch (const serial::SerialException& e) { - ROS_ERROR("%s", e.what()); - retCode = false; - } catch (...) { - ROS_ERROR("Unknown Error"); - retCode = false; - } - - return retCode; -} - -void MotorSerial::SerialThread() { - try { - while (motors.isOpen()) { - boost::this_thread::interruption_point(); - if (motors.waitReadable()) { - RawMotorMessage innew = {0, 0, 0, 0, 0, 0, 0, 0}; - - motors.read(innew.c_array(), 1); - if (innew[0] != MotorMessage::delimeter) { - // The first byte was not the delimiter, so re-loop - if (++serial_errors > error_threshold) { - ROS_WARN("REJECT %02x", innew[0]); - } - continue; - } - - // This will wait for the transmission time of 8 bytes - motors.waitByteTimes(innew.size()); - - // Read in next 7 bytes - motors.read(&innew.c_array()[1], 7); - ROS_DEBUG("Got message %x %x %x %x %x %x %x %x", innew[0], - innew[1], innew[2], innew[3], innew[4], innew[5], - innew[6], innew[7]); - - MotorMessage mc; - int error_code = mc.deserialize(innew); - if (error_code == 0) { - appendOutput(mc); - if (mc.getType() == MotorMessage::TYPE_ERROR) { - ROS_ERROR("GOT error from Firm 0x%02x", - mc.getRegister()); - } - } else { - if (++serial_errors > error_threshold) { - if (error_code == MotorMessage::ERR_UNKNOWN_REGISTER) { - ROS_WARN_ONCE("Message deserialize found an unrecognized firmware register"); - } else { - ROS_ERROR("DESERIALIZATION ERROR! - %d", error_code); - } - } - } - } - } - - } catch (const boost::thread_interrupted& e) { - motors.close(); - } catch (const serial::IOException& e) { - ROS_ERROR("%s", e.what()); - } catch (const serial::PortNotOpenedException& e) { - ROS_ERROR("%s", e.what()); - } catch (...) { - ROS_ERROR("Unknown Error"); - throw; - } -}