From 0714ca661af0ceb8d1c91119f7070ca1d6af48f7 Mon Sep 17 00:00:00 2001 From: Ben Scholar Date: Thu, 11 Feb 2021 17:13:56 -0500 Subject: [PATCH 1/6] cleaned up motor implementation, added input topic parameter --- hwctrl/CMakeLists.txt | 2 +- hwctrl/compile_commands.json | 133 +----------------------- hwctrl/include/hardware/bmc.h | 7 +- hwctrl/include/hardware/motor.h | 154 +++++++++++++++------------- hwctrl/include/hardware/vesc.h | 8 +- hwctrl/src/hardware/bmc.cpp | 9 +- hwctrl/src/hardware/motor.cpp | 34 ++---- hwctrl/src/hardware/vesc.cpp | 21 ++-- hwctrl/src/threads/motor_thread.cpp | 57 +++++----- 9 files changed, 127 insertions(+), 298 deletions(-) mode change 100644 => 120000 hwctrl/compile_commands.json diff --git a/hwctrl/CMakeLists.txt b/hwctrl/CMakeLists.txt index ba2122b1..770d25cf 100644 --- a/hwctrl/CMakeLists.txt +++ b/hwctrl/CMakeLists.txt @@ -14,7 +14,7 @@ endif() if(CMAKE_BUILD_TYPE STREQUAL "Release") add_compile_options(-std=c++14 -O2) else() - add_compile_options(-std=c++14 -Wall -Wextra -pedantic -g) + add_compile_options(-std=c++14 -Wall) # -Wextra -pedantic -g) endif() set(CMAKE_EXPORT_COMPILE_COMMANDS TRUE) diff --git a/hwctrl/compile_commands.json b/hwctrl/compile_commands.json deleted file mode 100644 index 3e80ca9d..00000000 --- a/hwctrl/compile_commands.json +++ /dev/null @@ -1,132 +0,0 @@ -[ -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/buffer.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/buffer.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/buffer.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/canbus.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/canbus.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/canbus.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/crc.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/crc.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/crc.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/gpio.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/gpio.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/gpio.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/hwctrl_node.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/hwctrl_node.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/hwctrl_node.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/hwctrl.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/hwctrl.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/hwctrl.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/lsm6ds3.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/lsm6ds3.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/lsm6ds3.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/motors.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/motors.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/motors.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/parse_csv.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/parse_csv.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/parse_csv.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/sensors.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/sensors.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/sensors.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/spi.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/spi.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/spi.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/uwb.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/uwb.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/uwb.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl.dir/src/vesc.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/vesc.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/vesc.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/buffer.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/buffer.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/buffer.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/canbus.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/canbus.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/canbus.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/crc.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/crc.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/crc.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/gpio.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/gpio.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/gpio.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/hwctrl_cal.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/hwctrl_cal.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/hwctrl_cal.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/hwctrl.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/hwctrl.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/hwctrl.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/lsm6ds3.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/lsm6ds3.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/lsm6ds3.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/motors.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/motors.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/motors.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/parse_csv.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/parse_csv.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/parse_csv.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/sensors.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/sensors.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/sensors.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/spi.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/spi.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/spi.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/uwb.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/uwb.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/uwb.cpp" -}, -{ - "directory": "/home/bscholar/catkin_ws/build/NASA-RMC-2020/hwctrl", - "command": "/usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\\\"hwctrl\\\" -I/home/bscholar/catkin_ws/devel/include -I/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/include -I/opt/ros/noetic/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -std=c++11 -o CMakeFiles/hwctrl_cal.dir/src/vesc.cpp.o -c /home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/vesc.cpp", - "file": "/home/bscholar/catkin_ws/src/NASA-RMC-2020/hwctrl/src/vesc.cpp" -} -] \ No newline at end of file diff --git a/hwctrl/compile_commands.json b/hwctrl/compile_commands.json new file mode 120000 index 00000000..e81d89c3 --- /dev/null +++ b/hwctrl/compile_commands.json @@ -0,0 +1 @@ +../../../build/hwctrl/compile_commands.json \ No newline at end of file diff --git a/hwctrl/include/hardware/bmc.h b/hwctrl/include/hardware/bmc.h index 2b9ff785..6be60b4f 100644 --- a/hwctrl/include/hardware/bmc.h +++ b/hwctrl/include/hardware/bmc.h @@ -6,12 +6,7 @@ class BMCMotor : CanMotor { public: - BMCMotor(ros::NodeHandle nh, const std::string& name, uint32_t id, - uint32_t can_id, - ros::Duration update_pd = ros::Duration(MOTOR_LOOP_PERIOD), - float accel_setpoint = DEFAULT_MAX_ACCEL, - float max_accel = DEFAULT_MAX_ACCEL, float max_rpm = DEFAULT_MAX_RPM, - float gear_reduc = 1.0f, ros::Duration timeout = ros::Duration(2.0)); + BMCMotor(ros::NodeHandle nh, MotorConfig const& config); virtual ~BMCMotor() = default; virtual void setup() override final; diff --git a/hwctrl/include/hardware/motor.h b/hwctrl/include/hardware/motor.h index 8c5589d0..e76d7641 100644 --- a/hwctrl/include/hardware/motor.h +++ b/hwctrl/include/hardware/motor.h @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -13,102 +14,107 @@ #include "sensor.h" -#define DEFAULT_MAX_ACCEL 30.0 -#define DEFAULT_MAX_RPM 50.0 -#define MOTOR_LOOP_PERIOD 0.005 +#define DEFAULT_MAX_ACCEL 30.0 +#define DEFAULT_MAX_RPM 50.0 +#define MOTOR_LOOP_PERIOD 0.005 -enum class ControlType { - None, RPM, Position -}; +enum class ControlType { None, RPM, Position }; -enum class MotorType { - None, Vesc, Sabertooth, BMC -}; +enum class MotorType { None, Vesc, Sabertooth, BMC }; MotorType get_motor_type(boost::string_view); +struct MotorConfig { + std::string name; + std::string cmd_topic = "hwctrl/motor_cmd"; + + uint32_t id; + + MotorType m_type; + ControlType c_type; + + ros::Duration update_pd = ros::Duration(MOTOR_LOOP_PERIOD); + ros::Duration timeout = ros::Duration(2.0); + + float accel_setpoint = DEFAULT_MAX_ACCEL; + float max_accel = DEFAULT_MAX_ACCEL; + float max_rpm = DEFAULT_MAX_RPM; + float gear_reduc = 1.0f; +}; + class Motor { public: - Motor( - ros::NodeHandle nh, const std::string& name, uint32_t id, MotorType m_type, - ControlType c_type, ros::Duration update_pd = ros::Duration(MOTOR_LOOP_PERIOD), float accel_setpoint = DEFAULT_MAX_ACCEL, float max_accel = DEFAULT_MAX_ACCEL, - float max_rpm = DEFAULT_MAX_RPM, float gear_reduc = 1.0f, ros::Duration timeout = ros::Duration(2.0) - ); - virtual ~Motor() = default; + Motor(ros::NodeHandle nh, MotorConfig const &config); + virtual ~Motor() = default; + + // allow moves but not copies + Motor(Motor const &) = delete; + void operator=(Motor const &) = delete; - // allow moves but not copies - Motor(Motor const&) = delete; - void operator=(Motor const&) = delete; + virtual void setup() = 0; + virtual void update(ros::Time time) = 0; + virtual void stop() = 0; + virtual void limit_stop(LimitSwitch::Direction direction){}; - virtual void setup() = 0; - virtual void update(ros::Time time) = 0; // send the setpoint and return sensor data - virtual void stop() = 0; - virtual void limit_stop(LimitSwitch::Direction direction) {}; + void set_setpoint(ros::Time time, float setpoint = 0.0f, + float acceleration = 0.0f); - void set_setpoint(ros::Time time, float setpoint = 0.0f, float acceleration = 0.0f); + inline const std::string &get_name() const { return m_config.name; } + inline uint32_t get_id() const { return m_config.id; } + inline MotorType get_motor_type() const { return m_config.m_type; } + inline ControlType get_control_type() const { return m_config.c_type; } - inline const std::string& get_name() const { return m_name; } - inline uint32_t get_id() const { return m_id; } - inline MotorType get_motor_type() const { return m_type; } - inline ControlType get_control_type() const { return m_control_type; } + inline bool is_online() const { return m_online; } + inline bool ready_to_update() const { return m_update; } - inline bool is_online() const { return m_online; } - inline bool ready_to_update() const { return m_update; } + inline void disable_update_timer() { + m_update_timer.stop(); + } + + inline void enable_update_timer() { + m_update_timer.start(); + } private: - void set_update_flag(const ros::TimerEvent&) { - m_update = true; - } + void setpoint_callback(boost::shared_ptr cmd) { + set_setpoint(ros::Time::now(), cmd->setpoint, cmd->acceleration); + } + void set_update_flag(const ros::TimerEvent &) { m_update = true; } protected: - ros::NodeHandle m_nh; - ros::Publisher m_motor_data_pub; - ros::Subscriber m_motor_cmd_sub; - - std::string m_name; - uint32_t m_id; - MotorType m_type; - ControlType m_control_type; - - float m_setpoint = 0.0; - float m_last_setpoint = 0.0; - float m_accel_setpoint; - float m_max_rpm; - float m_max_accel; - - float m_rpm_coef; - - ros::Time m_last_set_time; - ros::Time m_last_update_time; - ros::Duration m_timeout; - - ros::Duration m_update_pd; - ros::Timer m_update_timer; - - bool m_update = false; - bool m_online = false; - bool m_is_setup = false; + ros::NodeHandle m_nh; + ros::Publisher m_motor_data_pub; + ros::Subscriber m_motor_cmd_sub; + + MotorConfig m_config; + + float m_setpoint = 0.0; + float m_last_setpoint = 0.0; + float m_accel_setpoint; + + ros::Time m_last_set_time; + ros::Time m_last_update_time; + + ros::Timer m_update_timer; + + bool m_update = false; + bool m_online = false; + bool m_is_setup = false; }; class CanMotor : public Motor { public: - CanMotor( - ros::NodeHandle nh, const std::string& name, uint32_t id, uint32_t can_id, MotorType m_type, - ControlType c_type, ros::Duration update_pd = ros::Duration(MOTOR_LOOP_PERIOD), float accel_setpoint = DEFAULT_MAX_ACCEL, float max_accel = DEFAULT_MAX_ACCEL, - float max_rpm = DEFAULT_MAX_RPM, float gear_reduc = 1.0f, ros::Duration timeout = ros::Duration(2.0) - ); - virtual ~CanMotor() = default; - + CanMotor(ros::NodeHandle nh, MotorConfig const &config); + virtual ~CanMotor() = default; + protected: - using FramePtr = boost::shared_ptr; - virtual void can_rx_callback(FramePtr frame) = 0; + using FramePtr = boost::shared_ptr; + virtual void can_rx_callback(FramePtr frame) = 0; + protected: - uint32_t m_can_id; + uint32_t m_can_id; - ros::Publisher m_can_tx_pub; - ros::Subscriber m_can_rx_sub; + ros::Publisher m_can_tx_pub; + ros::Subscriber m_can_rx_sub; }; -class SerialMotor : public Motor { - -}; diff --git a/hwctrl/include/hardware/vesc.h b/hwctrl/include/hardware/vesc.h index 0c78f0dd..33ac3a70 100644 --- a/hwctrl/include/hardware/vesc.h +++ b/hwctrl/include/hardware/vesc.h @@ -4,13 +4,7 @@ class VescMotor : public CanMotor { public: - VescMotor(ros::NodeHandle nh, const std::string& name, uint32_t id, - uint32_t can_id, - ros::Duration update_pd = ros::Duration(MOTOR_LOOP_PERIOD), - float accel_setpoint = DEFAULT_MAX_ACCEL, - float max_accel = DEFAULT_MAX_ACCEL, - float max_rpm = DEFAULT_MAX_RPM, float gear_reduc = 1.0f, - ros::Duration timeout = ros::Duration(2.0)); + VescMotor(ros::NodeHandle nh, MotorConfig const& config); virtual ~VescMotor() = default; virtual void setup() override final; diff --git a/hwctrl/src/hardware/bmc.cpp b/hwctrl/src/hardware/bmc.cpp index 06e34e70..2e686f8a 100644 --- a/hwctrl/src/hardware/bmc.cpp +++ b/hwctrl/src/hardware/bmc.cpp @@ -1,12 +1,7 @@ #include "hardware/bmc.h" -BMCMotor::BMCMotor(ros::NodeHandle nh, const std::string& name, uint32_t id, - uint32_t can_id, ros::Duration update_pd, - float accel_setpoint, float max_accel, float max_rpm, - float gear_reduc, ros::Duration timeout) - : CanMotor(nh, name, id, can_id, MotorType::BMC, ControlType::Position, - update_pd, accel_setpoint, max_accel, max_rpm, gear_reduc, - timeout) {} +BMCMotor::BMCMotor(ros::NodeHandle nh, MotorConfig const& config) + : CanMotor(nh, config) {} void BMCMotor::setup() { m_is_setup = true; } diff --git a/hwctrl/src/hardware/motor.cpp b/hwctrl/src/hardware/motor.cpp index b14ca99c..be918637 100644 --- a/hwctrl/src/hardware/motor.cpp +++ b/hwctrl/src/hardware/motor.cpp @@ -1,7 +1,5 @@ #include "hardware/motor.h" -#include - MotorType get_motor_type(boost::string_view type_str) { if (type_str.compare("vesc") == 0) { return MotorType::Vesc; @@ -13,23 +11,13 @@ MotorType get_motor_type(boost::string_view type_str) { return MotorType::None; } -Motor::Motor(ros::NodeHandle nh, const std::string& name, uint32_t id, - MotorType m_type, ControlType c_type, ros::Duration update_pd, - float accel_setpoint, float max_accel, float max_rpm, - float gear_reduc, ros::Duration timeout) - : m_nh(nh), - m_name(name), - m_id(id), - m_type(m_type), - m_control_type(c_type), - m_update_pd(update_pd), - m_timeout(timeout), - m_accel_setpoint(accel_setpoint), - m_max_rpm(max_rpm), - m_max_accel(max_accel), - m_rpm_coef(gear_reduc) { +Motor::Motor(ros::NodeHandle nh,MotorConfig const& config) + : m_nh(nh), m_config(config) +{ m_motor_data_pub = m_nh.advertise("motor_data", 128); - m_update_timer = m_nh.createTimer(m_update_pd, &Motor::set_update_flag, this); + m_motor_cmd_sub = m_nh.subscribe(m_config.cmd_topic, 64, &Motor::setpoint_callback, this); + m_update_timer = m_nh.createTimer(m_config.update_pd, &Motor::set_update_flag, this); + m_update_timer.start(); } void Motor::set_setpoint(ros::Time time, float setpoint, float acceleration) { @@ -38,14 +26,8 @@ void Motor::set_setpoint(ros::Time time, float setpoint, float acceleration) { m_last_set_time = time; } -CanMotor::CanMotor(ros::NodeHandle nh, const std::string& name, uint32_t id, - uint32_t can_id, MotorType m_type, ControlType c_type, - ros::Duration update_pd, float accel_setpoint, - float max_accel, float max_rpm, float gear_reduc, - ros::Duration timeout) - : Motor(nh, name, id, m_type, c_type, update_pd, accel_setpoint, max_accel, - max_rpm, gear_reduc, timeout), - m_can_id(can_id) { +CanMotor::CanMotor(ros::NodeHandle nh, MotorConfig const& config) + : Motor(nh, config), m_can_id(config.id) { m_can_tx_pub = m_nh.advertise("can_frames_tx", 128); m_can_rx_sub = m_nh.subscribe("can_frames_rx", 128, &CanMotor::can_rx_callback, this); diff --git a/hwctrl/src/hardware/vesc.cpp b/hwctrl/src/hardware/vesc.cpp index a4539992..0e54fa62 100644 --- a/hwctrl/src/hardware/vesc.cpp +++ b/hwctrl/src/hardware/vesc.cpp @@ -6,13 +6,8 @@ #include "types.h" #include "util.h" -VescMotor::VescMotor(ros::NodeHandle nh, const std::string& name, uint32_t id, - uint32_t can_id, ros::Duration update_pd, - float accel_setpoint, float max_accel, float max_rpm, - float gear_reduc, ros::Duration timeout) - : CanMotor(nh, name, id, can_id, MotorType::Vesc, ControlType::RPM, - update_pd, accel_setpoint, max_accel, max_rpm, gear_reduc, - timeout) {} +VescMotor::VescMotor(ros::NodeHandle nh, MotorConfig const& config) + : CanMotor(nh, config) {} void VescMotor::setup() { // something maybe @@ -28,7 +23,7 @@ void VescMotor::can_rx_callback(FramePtr frame) { switch (cmd) { case CanPacketId::CAN_PACKET_STATUS: { - if(m_id != can_id) { + if(m_config.id != can_id) { // this status message is not for us break; } @@ -48,8 +43,8 @@ void VescMotor::can_rx_callback(FramePtr frame) { // publish rpm data hwctrl::MotorData msg; msg.data_type = msg.RPM; - msg.id = m_id; - msg.value = rpm / m_rpm_coef; + msg.id = m_config.id; + msg.value = rpm / m_config.gear_reduc; msg.timestamp = ros::Time::now(); m_motor_data_pub.publish(msg); @@ -81,12 +76,12 @@ void VescMotor::update(ros::Time time) { } m_last_setpoint += delta; - if ((time.toSec() - m_last_set_time.toSec()) > m_timeout.toSec()) { + if ((time.toSec() - m_last_set_time.toSec()) > m_config.timeout.toSec()) { // shut down the motor m_last_setpoint = 0; } - float eRPM = m_rpm_coef * m_last_setpoint; + float eRPM = m_config.gear_reduc * m_last_setpoint; m_last_update_time = time; send_rpm_frame(time, eRPM); @@ -138,7 +133,7 @@ void VescMotor::send_values_frame(ros::Time time) { void VescMotor::update_values_from_buffer(uint8_t* buffer){ - m_type = MotorType::Vesc; + m_config.m_type = MotorType::Vesc; // fill_data_from_buffer() size_t ind = 1; m_temp_mos1 = buffer::get_floating(buffer, 10.0f, ind); diff --git a/hwctrl/src/threads/motor_thread.cpp b/hwctrl/src/threads/motor_thread.cpp index 2e0c8c04..96b00fa8 100644 --- a/hwctrl/src/threads/motor_thread.cpp +++ b/hwctrl/src/threads/motor_thread.cpp @@ -145,6 +145,7 @@ void MotorThread::read_from_server() { const std::string full_name = base + "/" + *name; ROS_INFO("Checking for parameters under %s/...", full_name.c_str()); const std::string name_param = full_name + "/name"; + const std::string topic_param = full_name + "/topic"; const std::string type_param = full_name + "/type"; const std::string id_param = full_name + "/id"; const std::string gear_reduc_param = full_name + "/gear_reduction"; @@ -152,74 +153,66 @@ void MotorThread::read_from_server() { const std::string max_accel_param = full_name + "/max_acceleration"; const std::string period_param = full_name + "/update_period"; - std::string name_str; - MotorType type; - ControlType c_type; - int id; - int can_id; - double gearing; - double max_rpm; - double max_acc; - ros::Duration update_pd; - ros::Duration timeout = ros::Duration(2.0); + MotorConfig config; boost::shared_ptr motor; int found = 0; if (m_nh.hasParam(name_param)) { - m_nh.getParam(name_param, name_str); - ROS_INFO(" - Found motor name: %s", name_str.c_str()); + m_nh.getParam(name_param, config.name); + ROS_INFO(" - Found motor name: %s", config.name.c_str()); found++; } - if (m_nh.hasParam(type_param)) { + if (m_nh.hasParam(name_param)) { + m_nh.getParam(topic_param, config.cmd_topic); + ROS_INFO(" - Found motor name: %s", config.cmd_topic.c_str()); + found++; + } + if (m_nh.hasParam(type_param)) { std::string type_str; m_nh.getParam(type_param, type_str); ROS_INFO(" - Found motor type: %s", type_str.c_str()); - type = get_motor_type(type_str); + config.m_type = get_motor_type(type_str); found++; } if(m_nh.hasParam(id_param)) { + int id; m_nh.getParam(id_param, id); ROS_INFO(" - Found ID: %d", id); - can_id = id; // + config.id = id; found++; } if (m_nh.hasParam(gear_reduc_param)) { - m_nh.getParam(gear_reduc_param, gearing); - ROS_INFO(" - Found gear reduction: %.2f", gearing); + m_nh.getParam(gear_reduc_param, config.gear_reduc); + ROS_INFO(" - Found gear reduction: %.2f", config.gear_reduc); found++; } if (m_nh.hasParam(max_rpm_param)) { - m_nh.getParam(max_rpm_param, max_rpm); - ROS_INFO(" - Found max RPM: %.2f", max_rpm); + m_nh.getParam(max_rpm_param, config.max_rpm); + ROS_INFO(" - Found max RPM: %.2f", config.max_rpm); found++; } if (m_nh.hasParam(max_accel_param)) { - m_nh.getParam(max_accel_param, max_acc); - ROS_INFO(" - Found max acceleration: %.2f", max_acc); + m_nh.getParam(max_accel_param, config.max_accel); + ROS_INFO(" - Found max acceleration: %.2f", config.max_accel); found++; } if (m_nh.hasParam(period_param)) { double pd; m_nh.getParam(period_param, pd); - update_pd = ros::Duration(pd); + config.update_pd = ros::Duration(pd); ROS_INFO(" - Found motor update period: %.3fs", pd); } - switch (type) { + switch (config.m_type) { case MotorType::Vesc: { - auto temp = boost::make_shared( - m_nh, name_str, id, (uint32_t)can_id, update_pd, - (float)max_acc, (float)max_acc, (float)max_rpm, (float)gearing, - timeout); + auto temp = boost::make_shared(m_nh, config); motor = temp; break; } case MotorType::BMC: { - auto temp = boost::make_shared( - m_nh, name_str, id, (uint32_t)can_id, update_pd, - (float)max_acc, (float)max_acc, (float)max_rpm, (float)gearing, - timeout); + auto temp = boost::make_shared(m_nh, config); + motor = temp; break; } case MotorType::Sabertooth: @@ -229,7 +222,7 @@ void MotorThread::read_from_server() { break; } - if (found > 0 && motor != nullptr) m_motors.insert({id, motor}); + if (found > 0 && motor != nullptr) m_motors.insert({config.id, motor}); } } From 92c6f8b8c60a3ac306a32dfbcea4d77ac877f08b Mon Sep 17 00:00:00 2001 From: Ben Scholar Date: Thu, 11 Feb 2021 17:29:01 -0500 Subject: [PATCH 2/6] added topics to yaml --- hwctrl/3 | 131 ++++++++++++++++++++++++++++++++++++++++++++++++++++ "hwctrl/\\" | 37 +++++++++++++++ 2 files changed, 168 insertions(+) create mode 100644 hwctrl/3 create mode 100644 "hwctrl/\\" diff --git a/hwctrl/3 b/hwctrl/3 new file mode 100644 index 00000000..fe53aa4a --- /dev/null +++ b/hwctrl/3 @@ -0,0 +1,131 @@ +#pragma once + +#include +#include + +#include +#include + +#include + +#include +#include + +#include "sensor.h" + +#define DEFAULT_MAX_ACCEL 30.0 +#define DEFAULT_MAX_RPM 50.0 +#define MOTOR_LOOP_PERIOD 0.005 + +enum class ControlType { + None, RPM, Position +}; + +enum class MotorType { + None, Vesc, Sabertooth, BMC +}; + +MotorType get_motor_type(boost::string_view); + +struct MotorConfig { + std::string name; + std::string cmd_topic; + + uint32_t id; + + MotorType m_type; + ControlType c_type; + + ros::Duration update_pd = ros::Duration(MOTOR_LOOP_PERIOD); + ros::Duration timeout = ros::Duration(2.0); + + float accel_setpoint = DEFAULT_MAX_ACCEL; + float max_accel = DEFAULT_MAX_ACCEL; + float max_rpm = DEFAULT_MAX_RPM; + float gear_reduc = 1.0f; +}; + +class Motor { +public: + Motor( + ros::NodeHandle nh, const std::string& name, uint32_t id, MotorType m_type, std::string cmd_topic, + ControlType c_type, ros::Duration update_pd = ros::Duration(MOTOR_LOOP_PERIOD), float accel_setpoint = DEFAULT_MAX_ACCEL, float max_accel = DEFAULT_MAX_ACCEL, + float max_rpm = DEFAULT_MAX_RPM, float gear_reduc = 1.0f, ros::Duration timeout = ros::Duration(2.0) + ); + virtual ~Motor() = default; + + // allow moves but not copies + Motor(Motor const&) = delete; + void operator=(Motor const&) = delete; + + virtual void setup() = 0; + virtual void update(ros::Time time) = 0; // send the setpoint and return sensor data + virtual void stop() = 0; + virtual void limit_stop(LimitSwitch::Direction direction) {}; + + void set_setpoint(ros::Time time, float setpoint = 0.0f, float acceleration = 0.0f); + + inline const std::string& get_name() const { return m_name; } + inline uint32_t get_id() const { return m_id; } + inline MotorType get_motor_type() const { return m_type; } + inline ControlType get_control_type() const { return m_control_type; } + + inline bool is_online() const { return m_online; } + inline bool ready_to_update() const { return m_update; } + +private: + void set_update_flag(const ros::TimerEvent&) { + m_update = true; + } + +protected: + ros::NodeHandle m_nh; + ros::Publisher m_motor_data_pub; + + std::string m_cmd_topic; + ros::Subscriber m_motor_cmd_sub; + + std::string m_name; + uint32_t m_id; + MotorType m_type; + ControlType m_control_type; + + float m_setpoint = 0.0; + float m_last_setpoint = 0.0; + float m_accel_setpoint; + float m_max_rpm; + float m_max_accel; + + float m_rpm_coef; + + ros::Time m_last_set_time; + ros::Time m_last_update_time; + ros::Duration m_timeout; + + ros::Duration m_update_pd; + ros::Timer m_update_timer; + + bool m_update = false; + bool m_online = false; + bool m_is_setup = false; +}; + +class CanMotor : public Motor { +public: + CanMotor( + ros::NodeHandle nh, const std::string& name, uint32_t id, uint32_t can_id, MotorType m_type, + ControlType c_type, ros::Duration update_pd = ros::Duration(MOTOR_LOOP_PERIOD), float accel_setpoint = DEFAULT_MAX_ACCEL, float max_accel = DEFAULT_MAX_ACCEL, + float max_rpm = DEFAULT_MAX_RPM, float gear_reduc = 1.0f, ros::Duration timeout = ros::Duration(2.0) + ); + virtual ~CanMotor() = default; + +protected: + using FramePtr = boost::shared_ptr; + virtual void can_rx_callback(FramePtr frame) = 0; +protected: + uint32_t m_can_id; + + ros::Publisher m_can_tx_pub; + ros::Subscriber m_can_rx_sub; +}; + diff --git "a/hwctrl/\\" "b/hwctrl/\\" new file mode 100644 index 00000000..26551114 --- /dev/null +++ "b/hwctrl/\\" @@ -0,0 +1,37 @@ +#include "hardware/motor.h" + +#include + +MotorType get_motor_type(boost::string_view type_str) { + if (type_str.compare("vesc") == 0) { + return MotorType::Vesc; + } else if (type_str.compare("bmc") == 0) { + return MotorType::BMC; + } else if (type_str.compare("sbrth") == 0) { + return MotorType::Sabertooth; + } + return MotorType::None; +} + +Motor::Motor(ros::NodeHandle nh,MotorConfig const& config) + : m_nh(nh), m_config(config) +{ + m_motor_data_pub = m_nh.advertise(m_config.cmd_topic, 128); + m_update_timer = m_nh.createTimer(m_config.update_pd, &Motor::set_update_flag, this); +} + +void Motor::set_setpoint(ros::Time time, float setpoint, float acceleration) { + m_setpoint = setpoint; + m_accel_setpoint = acceleration; + m_last_set_time = time; +} + +CanMotor::CanMotor(ros::NodeHandle nh, + uint32_t can_id, MotorType m_type) + : Motor(nh, name, id, m_type, cmd_topic,c_type, update_pd, accel_setpoint, max_accel, + max_rpm, gear_reduc, timeout), + m_can_id(can_id) { + m_can_tx_pub = m_nh.advertise("can_frames_tx", 128); + m_can_rx_sub = + m_nh.subscribe("can_frames_rx", 128, &CanMotor::can_rx_callback, this); +} From 078e9d9d1d17b067bbc8066735d8ed74c4c25a93 Mon Sep 17 00:00:00 2001 From: Ben Scholar Date: Thu, 11 Feb 2021 17:30:20 -0500 Subject: [PATCH 3/6] removed weird files that I created on accident (learning vim) --- hwctrl/3 | 131 ---------------------------------------------------- "hwctrl/\\" | 37 --------------- 2 files changed, 168 deletions(-) delete mode 100644 hwctrl/3 delete mode 100644 "hwctrl/\\" diff --git a/hwctrl/3 b/hwctrl/3 deleted file mode 100644 index fe53aa4a..00000000 --- a/hwctrl/3 +++ /dev/null @@ -1,131 +0,0 @@ -#pragma once - -#include -#include - -#include -#include - -#include - -#include -#include - -#include "sensor.h" - -#define DEFAULT_MAX_ACCEL 30.0 -#define DEFAULT_MAX_RPM 50.0 -#define MOTOR_LOOP_PERIOD 0.005 - -enum class ControlType { - None, RPM, Position -}; - -enum class MotorType { - None, Vesc, Sabertooth, BMC -}; - -MotorType get_motor_type(boost::string_view); - -struct MotorConfig { - std::string name; - std::string cmd_topic; - - uint32_t id; - - MotorType m_type; - ControlType c_type; - - ros::Duration update_pd = ros::Duration(MOTOR_LOOP_PERIOD); - ros::Duration timeout = ros::Duration(2.0); - - float accel_setpoint = DEFAULT_MAX_ACCEL; - float max_accel = DEFAULT_MAX_ACCEL; - float max_rpm = DEFAULT_MAX_RPM; - float gear_reduc = 1.0f; -}; - -class Motor { -public: - Motor( - ros::NodeHandle nh, const std::string& name, uint32_t id, MotorType m_type, std::string cmd_topic, - ControlType c_type, ros::Duration update_pd = ros::Duration(MOTOR_LOOP_PERIOD), float accel_setpoint = DEFAULT_MAX_ACCEL, float max_accel = DEFAULT_MAX_ACCEL, - float max_rpm = DEFAULT_MAX_RPM, float gear_reduc = 1.0f, ros::Duration timeout = ros::Duration(2.0) - ); - virtual ~Motor() = default; - - // allow moves but not copies - Motor(Motor const&) = delete; - void operator=(Motor const&) = delete; - - virtual void setup() = 0; - virtual void update(ros::Time time) = 0; // send the setpoint and return sensor data - virtual void stop() = 0; - virtual void limit_stop(LimitSwitch::Direction direction) {}; - - void set_setpoint(ros::Time time, float setpoint = 0.0f, float acceleration = 0.0f); - - inline const std::string& get_name() const { return m_name; } - inline uint32_t get_id() const { return m_id; } - inline MotorType get_motor_type() const { return m_type; } - inline ControlType get_control_type() const { return m_control_type; } - - inline bool is_online() const { return m_online; } - inline bool ready_to_update() const { return m_update; } - -private: - void set_update_flag(const ros::TimerEvent&) { - m_update = true; - } - -protected: - ros::NodeHandle m_nh; - ros::Publisher m_motor_data_pub; - - std::string m_cmd_topic; - ros::Subscriber m_motor_cmd_sub; - - std::string m_name; - uint32_t m_id; - MotorType m_type; - ControlType m_control_type; - - float m_setpoint = 0.0; - float m_last_setpoint = 0.0; - float m_accel_setpoint; - float m_max_rpm; - float m_max_accel; - - float m_rpm_coef; - - ros::Time m_last_set_time; - ros::Time m_last_update_time; - ros::Duration m_timeout; - - ros::Duration m_update_pd; - ros::Timer m_update_timer; - - bool m_update = false; - bool m_online = false; - bool m_is_setup = false; -}; - -class CanMotor : public Motor { -public: - CanMotor( - ros::NodeHandle nh, const std::string& name, uint32_t id, uint32_t can_id, MotorType m_type, - ControlType c_type, ros::Duration update_pd = ros::Duration(MOTOR_LOOP_PERIOD), float accel_setpoint = DEFAULT_MAX_ACCEL, float max_accel = DEFAULT_MAX_ACCEL, - float max_rpm = DEFAULT_MAX_RPM, float gear_reduc = 1.0f, ros::Duration timeout = ros::Duration(2.0) - ); - virtual ~CanMotor() = default; - -protected: - using FramePtr = boost::shared_ptr; - virtual void can_rx_callback(FramePtr frame) = 0; -protected: - uint32_t m_can_id; - - ros::Publisher m_can_tx_pub; - ros::Subscriber m_can_rx_sub; -}; - diff --git "a/hwctrl/\\" "b/hwctrl/\\" deleted file mode 100644 index 26551114..00000000 --- "a/hwctrl/\\" +++ /dev/null @@ -1,37 +0,0 @@ -#include "hardware/motor.h" - -#include - -MotorType get_motor_type(boost::string_view type_str) { - if (type_str.compare("vesc") == 0) { - return MotorType::Vesc; - } else if (type_str.compare("bmc") == 0) { - return MotorType::BMC; - } else if (type_str.compare("sbrth") == 0) { - return MotorType::Sabertooth; - } - return MotorType::None; -} - -Motor::Motor(ros::NodeHandle nh,MotorConfig const& config) - : m_nh(nh), m_config(config) -{ - m_motor_data_pub = m_nh.advertise(m_config.cmd_topic, 128); - m_update_timer = m_nh.createTimer(m_config.update_pd, &Motor::set_update_flag, this); -} - -void Motor::set_setpoint(ros::Time time, float setpoint, float acceleration) { - m_setpoint = setpoint; - m_accel_setpoint = acceleration; - m_last_set_time = time; -} - -CanMotor::CanMotor(ros::NodeHandle nh, - uint32_t can_id, MotorType m_type) - : Motor(nh, name, id, m_type, cmd_topic,c_type, update_pd, accel_setpoint, max_accel, - max_rpm, gear_reduc, timeout), - m_can_id(can_id) { - m_can_tx_pub = m_nh.advertise("can_frames_tx", 128); - m_can_rx_sub = - m_nh.subscribe("can_frames_rx", 128, &CanMotor::can_rx_callback, this); -} From 3c4f2d5c5dcc8601d2ef1a9bca09e619a72eea14 Mon Sep 17 00:00:00 2001 From: Ben Scholar Date: Thu, 11 Feb 2021 17:32:08 -0500 Subject: [PATCH 4/6] actually added yaml this time --- glenn_launcher/params/hardware/hwctrl_params.yaml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/glenn_launcher/params/hardware/hwctrl_params.yaml b/glenn_launcher/params/hardware/hwctrl_params.yaml index 4ed514c9..05f2bd91 100644 --- a/glenn_launcher/params/hardware/hwctrl_params.yaml +++ b/glenn_launcher/params/hardware/hwctrl_params.yaml @@ -95,6 +95,7 @@ hardware: port_drive: id: 5 name: 'PORT DRIVE' + topic: '/glenn_base/port_motor_cmd' type: 'vesc' can_id: 5 gear_reduction: 630 # gear_reduc*pole_pairs @@ -104,6 +105,7 @@ hardware: starboard_drive: id: 6 name: 'STARBOARD DRIVE' + topic: '/glenn_base/starboard_motor_cmd' type: 'vesc' can_id: 6 gear_reduction: 630 # gear_reduc*pole_pairs @@ -114,6 +116,7 @@ hardware: id: 7 name: 'DEPOSITION DUNKERINO' type: 'vesc' + topic: "/deposition/winch_cmd" can_id: 7 gear_reduction: 245 # gear_reduc*pole_pairs max_rpm: 140 @@ -122,6 +125,7 @@ hardware: exc_belt: id: 8 name: 'EXCAVATION BELT' + topic: "/excavation/conveyor_cmd" type: 'vesc' can_id: 8 gear_reduction: 245 # gear_reduc*pole_pairs @@ -131,6 +135,7 @@ hardware: exc_translation: id: 9 name: 'EXCAVATION TRANSLATION' + topic: "/excavation/depth_cmd" type: 'vesc' can_id: 9 gear_reduction: 245 # gear_reduc*pole_pairs @@ -140,10 +145,12 @@ hardware: exc_port_act: id: 10 name: 'EXCAVATION PORT ACTUATOR' + topic: "/excavation/angle_cmd" type: 'brushed_motor' can_id: 10 exc_starboard_act: id: 11 name: 'EXCAVATION STARBOARD ACTUATOR' + topic: "/excavation/angle_cmd" type: 'brushed_motor' can_id: 11 From 2b7d7ec202bb63f7b6d69696ee30ed96bfd6ed47 Mon Sep 17 00:00:00 2001 From: Ben Scholar Date: Fri, 12 Feb 2021 10:32:52 -0500 Subject: [PATCH 5/6] removed motor_thread setpoint callback, since all motors handle this individually --- hwctrl/src/threads/motor_thread.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/hwctrl/src/threads/motor_thread.cpp b/hwctrl/src/threads/motor_thread.cpp index 96b00fa8..6a015fd7 100644 --- a/hwctrl/src/threads/motor_thread.cpp +++ b/hwctrl/src/threads/motor_thread.cpp @@ -18,8 +18,6 @@ #include "util.h" MotorThread::MotorThread(ros::NodeHandle nh) : HwctrlThread("motor_thread", nh, 1000) { - m_motor_set_sub = m_nh.subscribe("motor_setpoints", 128, - &MotorThread::set_motor_callback, this); m_estop_sub = m_nh.subscribe("estop", 128, &MotorThread::estop_callback, this); m_can_rx_sub = m_nh.subscribe("can_rx_frames", 128, &MotorThread::can_rx_callback, this); @@ -226,15 +224,6 @@ void MotorThread::read_from_server() { } } -void MotorThread::set_motor_callback(boost::shared_ptr msg) { - try { - m_motors.at(msg->id)->set_setpoint(ros::Time::now(), msg->setpoint, - msg->acceleration); - } catch(std::out_of_range&) { - ROS_WARN("Motor with ID %d is not defined.", msg->id); - } -} - void MotorThread::estop_callback(boost::shared_ptr msg) { // If estop is not enabled, powerup the motors if (!msg->data) { From 42d95b15963c77b3f3d8d41be449d290c8616841 Mon Sep 17 00:00:00 2001 From: Ethan Frank Date: Sat, 13 Feb 2021 19:34:36 -0500 Subject: [PATCH 6/6] Make cmd_to_wheel_speeds use individual motor topics --- .../autonomy/automodule/cmd_to_wheel_speeds.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/autonomy/src/autonomy/automodule/cmd_to_wheel_speeds.py b/autonomy/src/autonomy/automodule/cmd_to_wheel_speeds.py index f023e761..a5036fbb 100644 --- a/autonomy/src/autonomy/automodule/cmd_to_wheel_speeds.py +++ b/autonomy/src/autonomy/automodule/cmd_to_wheel_speeds.py @@ -3,7 +3,7 @@ import rospy from geometry_msgs.msg import Twist -from hwctrl.msg import MotorCmd, DriveMotorCmd +from hwctrl.msg import MotorCmd # Make these members of class later failed = False @@ -15,6 +15,12 @@ class CmdToWheelSpeedsNode: + """ + Most ros nodes publish robot command velocities as a Twist message + The hwctrl expects MotorCmd messages (units of RPM in this case) + This node does that conversion. + This could be factored out to the hwctrl if needed + """ def __init__(self): rospy.init_node("cmd_to_wheel_speeds_node", anonymous=False) @@ -24,7 +30,9 @@ def __init__(self): self.motor_acceleration = rospy.get_param('/motor_command_accel') - self.motor_setpoint_pub = rospy.Publisher("/glenn_base/motor_cmds", DriveMotorCmd, queue_size=2) + self.left_motor_pub = rospy.Publisher("/glenn_base/port_motor_cmd", MotorCmd, queue_size=2) + self.right_motor_pub = rospy.Publisher("/glenn_base/starboard_motor_cmd", MotorCmd, queue_size=2) + rospy.Subscriber("/glenn_base/cmd_vel", Twist, self.cmd_vel_callback, queue_size=1) rospy.spin() @@ -38,4 +46,6 @@ def cmd_vel_callback(self, msg): right_cmd = MotorCmd(setpoint=right_speed, acceleration=self.motor_acceleration) left_cmd = MotorCmd(setpoint=left_speed, acceleration=self.motor_acceleration) - self.motor_setpoint_pub.publish(right=right_cmd, left=left_cmd) + + self.left_motor_pub.publish(left_cmd) + self.right_motor_pub.publish(right_cmd)