diff --git a/Inc/firmware/configuration.hpp b/Inc/firmware/configuration.hpp index 1ba7623..c8c4b47 100644 --- a/Inc/firmware/configuration.hpp +++ b/Inc/firmware/configuration.hpp @@ -5,7 +5,7 @@ #include "mainf.h" #include "usart.h" -#include "diff_drive_controller.hpp" +#include "diff_drive_lib/robot_controller.hpp" #include "firmware/hal_compat.hpp" #include "firmware/motor_controller.hpp" @@ -99,25 +99,29 @@ extern MotorController MotB; extern MotorController MotC; extern MotorController MotD; -const DiffDriveConfiguration DD_CONFIG = { +const diff_drive_lib::RobotConfiguration ROBOT_CONFIG = { .wheel_FL_conf = { .motor = MotC, + .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY, .velocity_rolling_window_size = 10, }, .wheel_RL_conf = { .motor = MotD, + .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY, .velocity_rolling_window_size = 10, }, .wheel_FR_conf = { .motor = MotA, + .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY, .velocity_rolling_window_size = 10, }, .wheel_RR_conf = { .motor = MotB, + .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY, .velocity_rolling_window_size = 10, }, }; \ No newline at end of file diff --git a/Inc/firmware/motor_controller.hpp b/Inc/firmware/motor_controller.hpp index 38de84a..383d305 100644 --- a/Inc/firmware/motor_controller.hpp +++ b/Inc/firmware/motor_controller.hpp @@ -4,7 +4,7 @@ #include "firmware/hal_compat.hpp" -#include "motor_controller_interface.hpp" +#include "diff_drive_lib/motor_controller_interface.hpp" struct MotorConfiguration { GPIO nsleep, phase, mode, fault; @@ -14,7 +14,7 @@ struct MotorConfiguration { bool reverse_polarity; }; -class MotorController : public MotorControllerInterface { +class MotorController : public diff_drive_lib::MotorControllerInterface { public: MotorController(const MotorConfiguration &config) : config_(config){}; diff --git a/Inc/firmware/parameters.hpp b/Inc/firmware/parameters.hpp index e01c2f2..af73753 100644 --- a/Inc/firmware/parameters.hpp +++ b/Inc/firmware/parameters.hpp @@ -2,9 +2,9 @@ #include -#include +#include -struct Parameters : DiffDriveParams { +struct Parameters : diff_drive_lib::RobotParams { // Override inherited parameters Parameters() { // Motor @@ -15,14 +15,16 @@ struct Parameters : DiffDriveParams { wheel_pid_d = 0.0F; wheel_pwm_duty_limit = 100.0F; - // Differential drive - dd_wheel_radius = 0.0625F; - dd_wheel_separation = 0.33F; - dd_angular_velocity_multiplier = 1.91F; - dd_input_timeout = 500; + robot_wheel_radius = 0.0625F; + robot_wheel_separation = 0.358F; + robot_wheel_base = 0.3052F; + robot_angular_velocity_multiplier = 1.76F; + robot_input_timeout = 500; } float battery_min_voltage = 10.0; + bool mecanum_wheels = false; + void load(ros::NodeHandle &nh); }; diff --git a/Src/firmware/mainf.cpp b/Src/firmware/mainf.cpp index 7941c3f..df15e2f 100644 --- a/Src/firmware/mainf.cpp +++ b/Src/firmware/mainf.cpp @@ -7,13 +7,16 @@ #include #include #include +#include #include #include #include #include "mainf.h" -#include "wheel_controller.hpp" +#include "diff_drive_lib/diff_drive_controller.hpp" +#include "diff_drive_lib/mecanum_controller.hpp" +#include "diff_drive_lib/wheel_controller.hpp" #include "firmware/configuration.hpp" #include "firmware/imu_receiver.hpp" @@ -27,11 +30,15 @@ static std_msgs::Float32 battery_averaged; static ros::Publisher battery_pub("firmware/battery", &battery); static ros::Publisher battery_averaged_pub("firmware/battery_averaged", &battery); -static CircularBuffer battery_buffer_(BATTERY_BUFFER_SIZE); +static diff_drive_lib::CircularBuffer battery_buffer_( + BATTERY_BUFFER_SIZE); static bool publish_battery = false; static leo_msgs::WheelOdom wheel_odom; static ros::Publisher wheel_odom_pub("firmware/wheel_odom", &wheel_odom); +static leo_msgs::WheelOdomMecanum wheel_odom_mecanum; +static ros::Publisher wheel_odom_mecanum_pub("firmware/wheel_odom_mecanum", + &wheel_odom_mecanum); static bool publish_wheel_odom = false; static leo_msgs::WheelStates wheel_states; @@ -49,18 +56,18 @@ MotorController MotB(MOT_B_CONFIG); MotorController MotC(MOT_C_CONFIG); MotorController MotD(MOT_D_CONFIG); -static DiffDriveController dc(DD_CONFIG); +static diff_drive_lib::RobotController *controller; static ImuReceiver imu_receiver(&IMU_I2C); Parameters params; void cmdVelCallback(const geometry_msgs::Twist &msg) { - dc.setSpeed(msg.linear.x, msg.angular.z); + controller->setSpeed(msg.linear.x, msg.linear.y, msg.angular.z); } void resetOdometryCallback(const std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res) { - dc.resetOdom(); + controller->resetOdom(); res.success = true; } @@ -84,7 +91,8 @@ void getBoardTypeCallback(const std_srvs::TriggerRequest &req, } struct WheelWrapper { - explicit WheelWrapper(WheelController &wheel, std::string wheel_name) + explicit WheelWrapper(diff_drive_lib::WheelController &wheel, + std::string wheel_name) : wheel_(wheel), cmd_pwm_topic("firmware/wheel_" + wheel_name + "/cmd_pwm_duty"), cmd_vel_topic("firmware/wheel_" + wheel_name + "/cmd_velocity"), @@ -109,17 +117,17 @@ struct WheelWrapper { } private: - WheelController &wheel_; + diff_drive_lib::WheelController &wheel_; std::string cmd_pwm_topic; std::string cmd_vel_topic; ros::Subscriber cmd_pwm_sub_; ros::Subscriber cmd_vel_sub_; }; -static WheelWrapper wheel_FL_wrapper(dc.wheel_FL, "FL"); -static WheelWrapper wheel_RL_wrapper(dc.wheel_RL, "RL"); -static WheelWrapper wheel_FR_wrapper(dc.wheel_FR, "FR"); -static WheelWrapper wheel_RR_wrapper(dc.wheel_RR, "RR"); +static WheelWrapper wheel_FL_wrapper(controller->wheel_FL, "FL"); +static WheelWrapper wheel_RL_wrapper(controller->wheel_RL, "RL"); +static WheelWrapper wheel_FR_wrapper(controller->wheel_FR, "FR"); +static WheelWrapper wheel_RR_wrapper(controller->wheel_RR, "RR"); static ros::Subscriber twist_sub("cmd_vel", &cmdVelCallback); @@ -140,7 +148,11 @@ void initROS() { // Publishers nh.advertise(battery_pub); nh.advertise(battery_averaged_pub); - nh.advertise(wheel_odom_pub); + if (params.mecanum_wheels) { + nh.advertise(wheel_odom_mecanum_pub); + } else { + nh.advertise(wheel_odom_pub); + } nh.advertise(wheel_states_pub); nh.advertise(imu_pub); @@ -169,13 +181,17 @@ void setup() { } params.load(nh); - + if (params.mecanum_wheels) { + controller = new diff_drive_lib::MecanumController(ROBOT_CONFIG); + } else { + controller = new diff_drive_lib::DiffDriveController(ROBOT_CONFIG); + } initROS(); imu_receiver.init(); - // Initialize Diff Drive Controller - dc.init(params); + // Initialize Robot Controller + controller->init(params); configured = true; } @@ -192,7 +208,11 @@ void loop() { } if (publish_wheel_odom) { - wheel_odom_pub.publish(&wheel_odom); + if (params.mecanum_wheels) { + wheel_odom_mecanum_pub.publish(&wheel_odom_mecanum); + } else { + wheel_odom_pub.publish(&wheel_odom); + } publish_wheel_odom = false; } @@ -231,7 +251,7 @@ void update() { if (!configured) return; - dc.update(UPDATE_PERIOD); + controller->update(UPDATE_PERIOD); if (!nh.connected()) return; @@ -248,7 +268,7 @@ void update() { } if (cnt % JOINTS_PUB_PERIOD == 0 && !publish_wheel_states) { - auto dd_wheel_states = dc.getWheelStates(); + auto dd_wheel_states = controller->getWheelStates(); wheel_states.stamp = nh.now(); for (size_t i = 0; i < 4; i++) { @@ -262,15 +282,24 @@ void update() { } if (cnt % ODOM_PUB_PERIOD == 0 && !publish_wheel_odom) { - auto dd_odom = dc.getOdom(); - - wheel_odom.stamp = nh.now(); - wheel_odom.velocity_lin = dd_odom.velocity_lin; - wheel_odom.velocity_ang = dd_odom.velocity_ang; - wheel_odom.pose_x = dd_odom.pose_x; - wheel_odom.pose_y = dd_odom.pose_y; - wheel_odom.pose_yaw = dd_odom.pose_yaw; - + auto dd_odom = controller->getOdom(); + + if (params.mecanum_wheels) { + wheel_odom_mecanum.stamp = nh.now(); + wheel_odom_mecanum.velocity_lin_x = dd_odom.velocity_lin_x; + wheel_odom_mecanum.velocity_lin_y = dd_odom.velocity_lin_y; + wheel_odom_mecanum.velocity_ang = dd_odom.velocity_ang; + wheel_odom_mecanum.pose_x = dd_odom.pose_x; + wheel_odom_mecanum.pose_y = dd_odom.pose_y; + wheel_odom_mecanum.pose_yaw = dd_odom.pose_yaw; + } else { + wheel_odom.stamp = nh.now(); + wheel_odom.velocity_lin = dd_odom.velocity_lin_x; + wheel_odom.velocity_ang = dd_odom.velocity_ang; + wheel_odom.pose_x = dd_odom.pose_x; + wheel_odom.pose_y = dd_odom.pose_y; + wheel_odom.pose_yaw = dd_odom.pose_yaw; + } publish_wheel_odom = true; } diff --git a/Src/firmware/parameters.cpp b/Src/firmware/parameters.cpp index 1b6f7b9..0fb5396 100644 --- a/Src/firmware/parameters.cpp +++ b/Src/firmware/parameters.cpp @@ -5,6 +5,32 @@ static constexpr int TIMEOUT = 1000; void Parameters::load(ros::NodeHandle &nh) { + nh.getParam("firmware/mecanum_wheels", &mecanum_wheels, 1, TIMEOUT); + if (mecanum_wheels) { + robot_wheel_radius = 0.0635F; + robot_wheel_separation = 0.37F; + robot_angular_velocity_multiplier = 1.0F; + nh.getParam("firmware/mecanum_drive/wheel_radius", &robot_wheel_radius, 1, + TIMEOUT); + nh.getParam("firmware/mecanum_drive/wheel_separation", + &robot_wheel_separation, 1, TIMEOUT); + nh.getParam("firmware/mecanum_drive/wheel_base", &robot_wheel_base, 1, + TIMEOUT); + nh.getParam("firmware/mecanum_drive/angular_velocity_multiplier", + &robot_angular_velocity_multiplier, 1, TIMEOUT); + nh.getParam("firmware/mecanum_drive/input_timeout", &robot_input_timeout, 1, + TIMEOUT); + } else { + nh.getParam("firmware/diff_drive/wheel_radius", &robot_wheel_radius, 1, + TIMEOUT); + nh.getParam("firmware/diff_drive/wheel_separation", &robot_wheel_separation, + 1, TIMEOUT); + nh.getParam("firmware/diff_drive/angular_velocity_multiplier", + &robot_angular_velocity_multiplier, 1, TIMEOUT); + nh.getParam("firmware/diff_drive/input_timeout", &robot_input_timeout, 1, + TIMEOUT); + } + nh.getParam("firmware/wheels/encoder_resolution", &wheel_encoder_resolution, 1, TIMEOUT); nh.getParam("firmware/wheels/torque_constant", &wheel_torque_constant, 1, @@ -14,13 +40,5 @@ void Parameters::load(ros::NodeHandle &nh) { nh.getParam("firmware/wheels/pid/d", &wheel_pid_d, 1, TIMEOUT); nh.getParam("firmware/wheels/pwm_duty_limit", &wheel_pwm_duty_limit, 1, TIMEOUT); - - nh.getParam("firmware/diff_drive/wheel_radius", &dd_wheel_radius, 1, TIMEOUT); - nh.getParam("firmware/diff_drive/wheel_separation", &dd_wheel_separation, 1, - TIMEOUT); - nh.getParam("firmware/diff_drive/angular_velocity_multiplier", - &dd_angular_velocity_multiplier, 1, TIMEOUT); - nh.getParam("firmware/diff_drive/input_timeout", &dd_input_timeout, 1, - TIMEOUT); nh.getParam("firmware/battery_min_voltage", &battery_min_voltage, 1, TIMEOUT); } diff --git a/platformio.ini b/platformio.ini index c76601f..c7408db 100644 --- a/platformio.ini +++ b/platformio.ini @@ -20,8 +20,8 @@ build_flags = extra_scripts = pre:firmware_version.py lib_deps = - https://github.com/fictionlab/diff_drive_lib.git#1.1 - https://github.com/fictionlab/rosserial_lib.git#1.0 + https://github.com/fictionlab/diff_drive_lib.git#1.5 + https://github.com/fictionlab/rosserial_lib.git#1.1 [platformio] include_dir = Inc