Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Avoid dynamic allocation #10

Merged
merged 2 commits into from
May 6, 2024
Merged

Avoid dynamic allocation #10

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
54ef4c1
Use new diff_drive_lib library
bjsowa May 6, 2024
1b4f605
Use tagged version of diff_drive_lib library
bjsowa May 6, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 1 addition & 5 deletions Inc/firmware/configuration.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ static constexpr I2C_HandleTypeDef& IMU_I2C = hi2c1;
constexpr uint16_t PWM_RANGE = 1000;

// Number of encoder readings to remember when estimating the wheel velocity
constexpr uint32_t ENCODER_BUFFER_SIZE = 10;
constexpr uint32_t VELOCITY_ROLLING_WINDOW_SIZE = 10;

// The period (in number of calls to the update() function) at which the battery
// voltage is probed
Expand Down Expand Up @@ -125,24 +125,20 @@ constexpr diff_drive_lib::RobotConfiguration ROBOT_CONFIG = {
{
.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,
},
};
30 changes: 16 additions & 14 deletions Src/firmware/mainf.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,8 @@ static std_msgs__msg__Float32 battery;
static std_msgs__msg__Float32 battery_averaged;
static rcl_publisher_t battery_pub;
static rcl_publisher_t battery_averaged_pub;
static float battery_buffer_memory[BATTERY_BUFFER_SIZE];
static diff_drive_lib::CircularBuffer<float> battery_buffer(
BATTERY_BUFFER_SIZE, battery_buffer_memory);
static diff_drive_lib::CircularBuffer<float, BATTERY_BUFFER_SIZE>
battery_buffer;
static std::atomic_bool publish_battery(false);

static leo_msgs__msg__WheelOdom wheel_odom;
Expand Down Expand Up @@ -121,10 +120,11 @@ MotorController MotB(MOT_B_CONFIG);
MotorController MotC(MOT_C_CONFIG);
MotorController MotD(MOT_D_CONFIG);

static uint8_t
controller_buffer[std::max(sizeof(diff_drive_lib::DiffDriveController),
sizeof(diff_drive_lib::MecanumController))];
static diff_drive_lib::RobotController* controller;
static uint8_t controller_buffer[std::max(
sizeof(diff_drive_lib::DiffDriveController<VELOCITY_ROLLING_WINDOW_SIZE>),
sizeof(diff_drive_lib::MecanumController<VELOCITY_ROLLING_WINDOW_SIZE>))];
static diff_drive_lib::RobotController<VELOCITY_ROLLING_WINDOW_SIZE>*
controller;
static ImuReceiver imu_receiver(&IMU_I2C);

static Parameters params;
Expand Down Expand Up @@ -168,16 +168,16 @@ static void getBoardTypeCallback(const void* reqin, void* resin) {

static void wheelCmdPWMDutyCallback(const void* msgin, void* context) {
const std_msgs__msg__Float32* msg = (std_msgs__msg__Float32*)msgin;
diff_drive_lib::WheelController* wheel =
(diff_drive_lib::WheelController*)context;
auto wheel = static_cast<
diff_drive_lib::WheelController<VELOCITY_ROLLING_WINDOW_SIZE>*>(context);
wheel->disable();
wheel->motor.setPWMDutyCycle(msg->data);
}

static void wheelCmdVelCallback(const void* msgin, void* context) {
const std_msgs__msg__Float32* msg = (std_msgs__msg__Float32*)msgin;
diff_drive_lib::WheelController* wheel =
(diff_drive_lib::WheelController*)context;
auto wheel = static_cast<
diff_drive_lib::WheelController<VELOCITY_ROLLING_WINDOW_SIZE>*>(context);
wheel->enable();
wheel->setTargetVelocity(msg->data);
}
Expand Down Expand Up @@ -409,14 +409,16 @@ void initController() {
&wheel_odom_mecanum_pub, &node,
ROSIDL_GET_MSG_TYPE_SUPPORT(leo_msgs, msg, WheelOdomMecanum),
"~/wheel_odom_mecanum");
controller =
new (controller_buffer) diff_drive_lib::MecanumController(ROBOT_CONFIG);
controller = new (controller_buffer)
diff_drive_lib::MecanumController<VELOCITY_ROLLING_WINDOW_SIZE>(
ROBOT_CONFIG);
} else {
rclc_publisher_init_best_effort(
&wheel_odom_pub, &node,
ROSIDL_GET_MSG_TYPE_SUPPORT(leo_msgs, msg, WheelOdom), "~/wheel_odom");
controller = new (controller_buffer)
diff_drive_lib::DiffDriveController(ROBOT_CONFIG);
diff_drive_lib::DiffDriveController<VELOCITY_ROLLING_WINDOW_SIZE>(
ROBOT_CONFIG);
}
controller->init(params);
}
Expand Down
2 changes: 1 addition & 1 deletion platformio.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ board_microros_distro = humble-fictionlab
board_microros_transport = serial
board_microros_user_meta = colcon.meta
lib_deps =
https://github.com/fictionlab/diff_drive_lib.git#1.6
https://github.com/fictionlab/diff_drive_lib.git#2.0
https://github.com/fictionlab/micro_ros_platformio#fictionlab-v1
lib_compat_mode = off

Expand Down
Loading