Skip to content

Commit

Permalink
Mecanum wheels control in firmware (#1)
Browse files Browse the repository at this point in the history 
* made firmware compatible with the newest diff_drive_lib library

Signed-off-by: Bitterisland6 <bitterisland6@gmail.com>

* robot interface changes

Signed-off-by: Bitterisland6 <bitterisland6@gmail.com>

* added mecanum required parameters and made firmware use generic controller

Signed-off-by: Bitterisland6 <bitterisland6@gmail.com>

* Changed wheel_odom to include linear speed y and added use of mecanum controller

Signed-off-by: Bitterisland6 <bitterisland6@gmail.com>

* added loading mecanum parameters

Signed-off-by: Bitterisland6 <bitterisland6@gmail.com>

* include mecanum_controller in the configuration header file

Signed-off-by: Bitterisland6 <bitterisland6@gmail.com>

* added separate odometry topic for mecanum wheels

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

* updated platformio.ini file to include newest libries

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

* format code

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

* implemented most of the code review guidelines

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

* added missing includes

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

---------

Signed-off-by: Bitterisland6 <bitterisland6@gmail.com>
Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>
  • Loading branch information
@Bitterisland6
Bitterisland6 authored Aug 4, 2023
1 parent f064cb5 commit 4b87e01
Show file tree
Hide file tree
Showing 6 changed files with 101 additions and 48 deletions.
8 changes: 6 additions & 2 deletions Inc/firmware/configuration.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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,
},
};
4 changes: 2 additions & 2 deletions Inc/firmware/motor_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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){};

Expand Down
16 changes: 9 additions & 7 deletions Inc/firmware/parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,9 @@

#include <ros.h>

#include <diff_drive_controller.hpp>
#include <diff_drive_lib/robot_controller.hpp>

struct Parameters : DiffDriveParams {
struct Parameters : diff_drive_lib::RobotParams {
// Override inherited parameters
Parameters() {
// Motor
Expand All @@ -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);
};
83 changes: 56 additions & 27 deletions Src/firmware/mainf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,13 +7,16 @@
#include <geometry_msgs/Twist.h>
#include <leo_msgs/Imu.h>
#include <leo_msgs/WheelOdom.h>
#include <leo_msgs/WheelOdomMecanum.h>
#include <leo_msgs/WheelStates.h>
#include <std_msgs/Float32.h>
#include <std_srvs/Trigger.h>

#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"
Expand All @@ -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<float> battery_buffer_(BATTERY_BUFFER_SIZE);
static diff_drive_lib::CircularBuffer<float> 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;
Expand All @@ -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;
}

Expand All @@ -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"),
Expand All @@ -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<std_msgs::Float32, WheelWrapper> cmd_pwm_sub_;
ros::Subscriber<std_msgs::Float32, WheelWrapper> 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<geometry_msgs::Twist> twist_sub("cmd_vel",
&cmdVelCallback);
Expand All @@ -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);

Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
}

Expand Down Expand Up @@ -231,7 +251,7 @@ void update() {

if (!configured) return;

dc.update(UPDATE_PERIOD);
controller->update(UPDATE_PERIOD);

if (!nh.connected()) return;

Expand All @@ -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++) {
Expand All @@ -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;
}

Expand Down
34 changes: 26 additions & 8 deletions Src/firmware/parameters.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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);
}
4 changes: 2 additions & 2 deletions platformio.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 4b87e01

Please sign in to comment.