Skip to content

4x4 drive #3

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

Open
BartVanderHaagen wants to merge 9 commits into
base: main
Choose a base branch
from
Open

4x4 drive #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
2cd950f
Add files via upload
BartVanderHaagen May 26, 2024
862df62
Add files via upload
BartVanderHaagen May 26, 2024
d3f925e
Add files via upload
BartVanderHaagen May 26, 2024
a9d8d44
latest version
BartVanderHaagen May 27, 2024
aa901b0
Update README.md
BartVanderHaagen May 27, 2024
a3a387d
Update README.md
BartVanderHaagen May 27, 2024
631b2d9
Add files via upload
BartVanderHaagen May 27, 2024
3931198
Add files via upload
BartVanderHaagen May 27, 2024
a407c48
Add files via upload
BartVanderHaagen Jun 3, 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
3 changes: 1 addition & 2 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
# robot_base
The challange in the 4x4 wheeldrive is the 2 USB connections to be made in the hardawre interface
this is WIP
The chalenge in the 4x4 wheeldrive is the 2 USB ports that spawn to run in sync with eachother - WIP

# robot_base
46 changes: 26 additions & 20 deletions ...ardware_interface/include/hoverboard_hardware_interface/hoverboard_hardware_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,20 +29,23 @@ namespace hoverboard_hardware_interface
class HoverboardHardwareInterface : public hardware_interface::SystemInterface
{
struct HardwareConfig
{
std::string leftWheelJointName = "left_front_wheel_joint";
std::string rightWheelJointName = "right_front_wheel_joint";
{
std::string frontLeftWheelJointName = "left_front_wheel_joint";
std::string frontRightWheelJointName = "right_front_wheel_joint";
std::string backLeftWheelJointName = "left_back_wheel_joint";
std::string backRightWheelJointName = "right_back_wheel_joint";

float loopRate = 30.0;
int encoderTicksPerRevolution = 1024;
};
float loopRate = 30.0;
int encoderTicksPerRevolution = 1024;
};

struct SerialPortConfig
{
std::string device = "/dev/ttyUSB0";
int baudRate = 115200;
int timeout = 1000;
};
struct SerialPortConfig
{
std::string frontDevice = "/dev/ttyUSB0";
std::string backDevice = "/dev/ttyUSB1";
int baudRate = 115200;
int timeout = 1000;
};

public:
RCLCPP_SHARED_PTR_DEFINITIONS(HoverboardHardwareInterface)
Expand All @@ -67,17 +70,20 @@ namespace hoverboard_hardware_interface

void motorWheelFeedbackCallback(MotorWheelFeedback);

private:
private:
SerialPortService frontSerialPortService;
SerialPortService backSerialPortService;

SerialPortService serialPortService;
HardwareConfig hardwareConfig;
SerialPortConfig serialPortConfig;

HardwareConfig hardwareConfig;
SerialPortConfig serialPortConfig;
MotorWheel frontLeftWheel;
MotorWheel frontRightWheel;
MotorWheel backLeftWheel;
MotorWheel backRightWheel;

MotorWheel leftWheel;
MotorWheel rightWheel;
bool connect();
bool disconnect();

bool connect();
bool disconnect();
};
}
15 changes: 9 additions & 6 deletions hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_protocol.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,14 +18,15 @@

#define HEAD_FRAME 0xABCD

// enum class MOTOR_STATES {
// UNOCUPPIED = 0b00,
// RUN = 0b01,
// BRAKE = 0b11,
// LOCK_SHAFT = 0b10,
// };
enum class MOTOR_STATES {
UNOCUPPIED = 0b00,
RUN = 0b01,
BRAKE = 0b11,
LOCK_SHAFT = 0b10,
};

typedef struct {
uint8_t hoverboard_id; // Identifier for the hoverboard
uint16_t head;
int16_t command1;
int16_t command2;
Expand All @@ -40,8 +41,10 @@ typedef struct {
} MotorWheelFeedback;

typedef struct {
uint8_t hoverboard_id; // Identifier for the hoverboard
uint16_t head = HEAD_FRAME;
int16_t steer;
int16_t speed;
uint16_t checksum;
} MotorWheelDriveControl;

29 changes: 6 additions & 23 deletions hoverboard_hardware_interface/include/hoverboard_hardware_interface/serial_port_service.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,26 +1,9 @@
// Copyright 2023 Robert Gruberski (Viola Robotics Sp. z o.o. Poland)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once

#include <string>

#include <boost/asio.hpp>
#include <boost/thread.hpp>

#include "rclcpp/rclcpp.hpp"

#include "serial_port_protocol.hpp"

#define SERIAL_PORT_READ_BUF_SIZE 256
Expand All @@ -31,8 +14,7 @@ namespace hoverboard_hardware_interface
{
class SerialPortService
{
public:

public:
SerialPortService() = default;

bool connect(const std::string &serial_device, int baud_rate, int timeout);
Expand All @@ -45,10 +27,10 @@ namespace hoverboard_hardware_interface

void BindMotorWheelFeedbackCallback(std::function<void(MotorWheelFeedback)>);

private:

private:
boost::asio::io_service io_service;
serial_port_ptr port;
serial_port_ptr port; // Declaration of 'port' as a member variable

boost::mutex mutex;

uint16_t head_frame = 0;
Expand All @@ -66,4 +48,5 @@ namespace hoverboard_hardware_interface

MotorWheelFeedback motorWheelFeedback {};
};
}
} // namespace hoverboard_hardware_interface

138 changes: 98 additions & 40 deletions hoverboard_hardware_interface/src/hoverboard_hardware_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,19 +22,22 @@ namespace hoverboard_hardware_interface
return hardware_interface::CallbackReturn::ERROR;
}

hardwareConfig.leftWheelJointName = info.hardware_parameters.at("left_front_wheel_joint_name");
hardwareConfig.rightWheelJointName = info.hardware_parameters.at("right_front_wheel_joint_name");
hardwareConfig.frontLeftWheelJointName = info.hardware_parameters.at("left_front_wheel_joint_name");
hardwareConfig.frontRightWheelJointName = info.hardware_parameters.at("right_front_wheel_joint_name");
hardwareConfig.backLeftWheelJointName = info.hardware_parameters.at("left_back_wheel_joint_name");
hardwareConfig.backRightWheelJointName = info.hardware_parameters.at("right_back_wheel_joint_name");
hardwareConfig.loopRate = std::stof(info.hardware_parameters.at("loop_rate"));
// hardwareConfig.encoderTicksPerRevolution = std::stoi(info.hardware_parameters.at("encoder_ticks_per_revolution"));
//hardwareConfig.encoderTicksPerRevolution = std::stoi(info.hardware_parameters.at("encoder_ticks_per_revolution"));

serialPortConfig.device = info.hardware_parameters.at("device");
serialPortConfig.frontDevice = info.hardware_parameters.at("front_device");
serialPortConfig.backDevice = info.hardware_parameters.at("back_device");
serialPortConfig.baudRate = std::stoi(info.hardware_parameters.at("baud_rate"));
serialPortConfig.timeout = std::stoi(info.hardware_parameters.at("timeout"));

leftWheel = MotorWheel(info.hardware_parameters.at("left_front_wheel_joint_name"),
std::stoi(info.hardware_parameters.at("encoder_ticks_per_revolution")));
rightWheel = MotorWheel(info.hardware_parameters.at("right_front_wheel_joint_name"),
std::stoi(info.hardware_parameters.at("encoder_ticks_per_revolution")));
frontLeftWheel = MotorWheel(hardwareConfig.frontLeftWheelJointName, hardwareConfig.encoderTicksPerRevolution);
frontRightWheel = MotorWheel(hardwareConfig.frontRightWheelJointName, hardwareConfig.encoderTicksPerRevolution);
backLeftWheel = MotorWheel(hardwareConfig.backLeftWheelJointName, hardwareConfig.encoderTicksPerRevolution);
backRightWheel = MotorWheel(hardwareConfig.backRightWheelJointName, hardwareConfig.encoderTicksPerRevolution);

for (const hardware_interface::ComponentInfo & joint : info.joints)
{
Expand All @@ -51,7 +54,7 @@ namespace hoverboard_hardware_interface
{
RCLCPP_FATAL(
rclcpp::get_logger("HoverboardHardwareInterface"),
"Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
"Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(),
joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);

return hardware_interface::CallbackReturn::ERROR;
Expand All @@ -61,7 +64,7 @@ namespace hoverboard_hardware_interface
{
RCLCPP_FATAL(
rclcpp::get_logger("HoverboardHardwareInterface"),
"Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), joint.state_interfaces.size());
"Joint '%s' has %zu state interfaces. 2 expected.", joint.name.c_str(), joint.state_interfaces.size());

return hardware_interface::CallbackReturn::ERROR;
}
Expand All @@ -70,7 +73,7 @@ namespace hoverboard_hardware_interface
{
RCLCPP_FATAL(
rclcpp::get_logger("HoverboardHardwareInterface"),
"Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(),
"Joint '%s' has '%s' as first state interface. '%s' expected.", joint.name.c_str(),
joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);

return hardware_interface::CallbackReturn::ERROR;
Expand All @@ -80,7 +83,7 @@ namespace hoverboard_hardware_interface
{
RCLCPP_FATAL(
rclcpp::get_logger("HoverboardHardwareInterface"),
"Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(),
"Joint '%s' has '%s' as second state interface. '%s' expected.", joint.name.c_str(),
joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);

return hardware_interface::CallbackReturn::ERROR;
Expand All @@ -94,11 +97,17 @@ namespace hoverboard_hardware_interface
{
std::vector<hardware_interface::StateInterface> state_interfaces;

state_interfaces.emplace_back(hardware_interface::StateInterface(leftWheel.name, hardware_interface::HW_IF_POSITION, &leftWheel.position));
state_interfaces.emplace_back(hardware_interface::StateInterface(leftWheel.name, hardware_interface::HW_IF_VELOCITY, &leftWheel.velocity));
state_interfaces.emplace_back(hardware_interface::StateInterface(frontLeftWheel.name, hardware_interface::HW_IF_POSITION, &frontLeftWheel.position));
state_interfaces.emplace_back(hardware_interface::StateInterface(frontLeftWheel.name, hardware_interface::HW_IF_VELOCITY, &frontLeftWheel.velocity));

state_interfaces.emplace_back(hardware_interface::StateInterface(rightWheel.name, hardware_interface::HW_IF_POSITION, &rightWheel.position));
state_interfaces.emplace_back(hardware_interface::StateInterface(rightWheel.name, hardware_interface::HW_IF_VELOCITY, &rightWheel.velocity));
state_interfaces.emplace_back(hardware_interface::StateInterface(frontRightWheel.name, hardware_interface::HW_IF_POSITION, &frontRightWheel.position));
state_interfaces.emplace_back(hardware_interface::StateInterface(frontRightWheel.name, hardware_interface::HW_IF_VELOCITY, &frontRightWheel.velocity));

state_interfaces.emplace_back(hardware_interface::StateInterface(backLeftWheel.name, hardware_interface::HW_IF_POSITION, &backLeftWheel.position));
state_interfaces.emplace_back(hardware_interface::StateInterface(backLeftWheel.name, hardware_interface::HW_IF_VELOCITY, &backLeftWheel.velocity));

state_interfaces.emplace_back(hardware_interface::StateInterface(backRightWheel.name, hardware_interface::HW_IF_POSITION, &backRightWheel.position));
state_interfaces.emplace_back(hardware_interface::StateInterface(backRightWheel.name, hardware_interface::HW_IF_VELOCITY, &backRightWheel.velocity));

return state_interfaces;
}
Expand All @@ -107,8 +116,10 @@ namespace hoverboard_hardware_interface
{
std::vector<hardware_interface::CommandInterface> command_interfaces;

command_interfaces.emplace_back(hardware_interface::CommandInterface(leftWheel.name, hardware_interface::HW_IF_VELOCITY, &leftWheel.command));
command_interfaces.emplace_back(hardware_interface::CommandInterface(rightWheel.name, hardware_interface::HW_IF_VELOCITY, &rightWheel.command));
command_interfaces.emplace_back(hardware_interface::CommandInterface(frontLeftWheel.name, hardware_interface::HW_IF_VELOCITY, &frontLeftWheel.command));
command_interfaces.emplace_back(hardware_interface::CommandInterface(frontRightWheel.name, hardware_interface::HW_IF_VELOCITY, &frontRightWheel.command));
command_interfaces.emplace_back(hardware_interface::CommandInterface(backLeftWheel.name, hardware_interface::HW_IF_VELOCITY, &backLeftWheel.command));
command_interfaces.emplace_back(hardware_interface::CommandInterface(backRightWheel.name, hardware_interface::HW_IF_VELOCITY, &backRightWheel.command));

return command_interfaces;
}
Expand All @@ -117,12 +128,15 @@ namespace hoverboard_hardware_interface
{
RCLCPP_INFO(rclcpp::get_logger("HoverboardHardwareInterface"), "Configuring... please wait a moment...");

if (!serialPortService.connect(serialPortConfig.device, serialPortConfig.baudRate, serialPortConfig.timeout))
if (!connect())
{
return hardware_interface::CallbackReturn::ERROR;
}

serialPortService.BindMotorWheelFeedbackCallback(
frontSerialPortService.BindMotorWheelFeedbackCallback(
std::bind(&HoverboardHardwareInterface::motorWheelFeedbackCallback, this, std::placeholders::_1)
);
backSerialPortService.BindMotorWheelFeedbackCallback(
std::bind(&HoverboardHardwareInterface::motorWheelFeedbackCallback, this, std::placeholders::_1)
);

Expand All @@ -133,7 +147,7 @@ namespace hoverboard_hardware_interface
{
RCLCPP_INFO(rclcpp::get_logger("HoverboardHardwareInterface"), "Cleaning up... please wait a moment...");

if (!serialPortService.disconnect())
if (!disconnect())
{
return hardware_interface::CallbackReturn::ERROR;
}
Expand All @@ -159,43 +173,87 @@ namespace hoverboard_hardware_interface

hardware_interface::return_type HoverboardHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration & period)
{
serialPortService.read();
frontSerialPortService.read();
backSerialPortService.read();

double lastPosition = leftWheel.position;
leftWheel.position = leftWheel.calculateEncoderAngle();
leftWheel.velocity = (leftWheel.position - lastPosition) / period.seconds();
double lastPosition = frontLeftWheel.position;
frontLeftWheel.position = frontLeftWheel.calculateEncoderAngle();
frontLeftWheel.velocity = (frontLeftWheel.position - lastPosition) / period.seconds();

lastPosition = rightWheel.position;
rightWheel.position = rightWheel.calculateEncoderAngle();
rightWheel.velocity = (rightWheel.position - lastPosition) / period.seconds();
lastPosition = frontRightWheel.position;
frontRightWheel.position = frontRightWheel.calculateEncoderAngle();
frontRightWheel.velocity = (frontRightWheel.position - lastPosition) / period.seconds();

lastPosition = backLeftWheel.position;
backLeftWheel.position = backLeftWheel.calculateEncoderAngle();
backLeftWheel.velocity = (backLeftWheel.position - lastPosition) / period.seconds();

lastPosition = backRightWheel.position;
backRightWheel.position = backRightWheel.calculateEncoderAngle();
backRightWheel.velocity = (backRightWheel.position - lastPosition) / period.seconds();

return hardware_interface::return_type::OK;
}

hardware_interface::return_type HoverboardHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &)
{
MotorWheelDriveControl motorWheelDriveControl;
MotorWheelDriveControl frontMotorWheelDriveControl;
MotorWheelDriveControl backMotorWheelDriveControl;

const double speed = ((leftWheel.command / 0.10472) + (rightWheel.command / 0.10472)) / 2.0;
const double steer = ((leftWheel.command / 0.10472) - speed) * 2.0;
const double frontSpeed = ((frontLeftWheel.command / 0.10472) + (frontRightWheel.command / 0.10472)) / 2.0;
const double frontSteer = ((frontLeftWheel.command / 0.10472) - frontSpeed) * 2.0;

const double backSpeed = ((backLeftWheel.command / 0.10472) + (backRightWheel.command / 0.10472)) / 2.0;
const double backSteer = ((backLeftWheel.command / 0.10472) - backSpeed) * 2.0;

// TODO: radius should be read from the urdf file, check calculations
motorWheelDriveControl.speed = (int16_t) (speed);
motorWheelDriveControl.steer = (int16_t) (steer);
motorWheelDriveControl.checksum = (uint16_t)(motorWheelDriveControl.head ^ motorWheelDriveControl.steer ^ motorWheelDriveControl.speed);
frontMotorWheelDriveControl.speed = static_cast<int16_t>(frontSpeed);
frontMotorWheelDriveControl.steer = static_cast<int16_t>(frontSteer);
frontMotorWheelDriveControl.checksum = static_cast<uint16_t>(frontMotorWheelDriveControl.head ^ frontMotorWheelDriveControl.steer ^ frontMotorWheelDriveControl.speed);

backMotorWheelDriveControl.speed = static_cast<int16_t>(backSpeed);
backMotorWheelDriveControl.steer = static_cast<int16_t>(backSteer);
backMotorWheelDriveControl.checksum = static_cast<uint16_t>(backMotorWheelDriveControl.head ^ backMotorWheelDriveControl.steer ^ backMotorWheelDriveControl.speed);

RCLCPP_INFO(rclcpp::get_logger("SerialPortService"), "Front: %i %i", frontMotorWheelDriveControl.speed, frontMotorWheelDriveControl.steer);
RCLCPP_INFO(rclcpp::get_logger("SerialPortService"), "Back: %i %i", backMotorWheelDriveControl.speed, backMotorWheelDriveControl.steer);

RCLCPP_INFO(rclcpp::get_logger("SerialPortService"), "Sending front motor commands: speed=%i, steer=%i", frontMotorWheelDriveControl.speed, frontMotorWheelDriveControl.steer);
RCLCPP_INFO(rclcpp::get_logger("SerialPortService"), "Sending back motor commands: speed=%i, steer=%i", backMotorWheelDriveControl.speed, backMotorWheelDriveControl.steer);

RCLCPP_INFO(rclcpp::get_logger("HoverboardHardwareInterface"), "Writing commands to motors...");


frontSerialPortService.write(reinterpret_cast<const char*>(&frontMotorWheelDriveControl), sizeof(MotorWheelDriveControl));
backSerialPortService.write(reinterpret_cast<const char*>(&backMotorWheelDriveControl), sizeof(MotorWheelDriveControl));

return hardware_interface::return_type::OK;
}

RCLCPP_INFO(rclcpp::get_logger("SerialPortService"), "%i %i", motorWheelDriveControl.speed, motorWheelDriveControl.steer);
void HoverboardHardwareInterface::motorWheelFeedbackCallback(MotorWheelFeedback motorWheelFeedback)
{
frontLeftWheel.updateEncoderTicks(motorWheelFeedback.leftMotorEncoderCumulativeCount);
frontRightWheel.updateEncoderTicks(motorWheelFeedback.rightMotorEncoderCumulativeCount);
backLeftWheel.updateEncoderTicks(motorWheelFeedback.leftMotorEncoderCumulativeCount);
backRightWheel.updateEncoderTicks(motorWheelFeedback.rightMotorEncoderCumulativeCount);
}

serialPortService.write((const char*) &motorWheelDriveControl, sizeof(MotorWheelDriveControl));
bool HoverboardHardwareInterface::connect()
{
bool frontConnected = frontSerialPortService.connect(serialPortConfig.frontDevice, serialPortConfig.baudRate, serialPortConfig.timeout);
bool backConnected = backSerialPortService.connect(serialPortConfig.backDevice, serialPortConfig.baudRate, serialPortConfig.timeout);

return hardware_interface::return_type::OK;
return frontConnected && backConnected;
}

void HoverboardHardwareInterface::motorWheelFeedbackCallback(MotorWheelFeedback motorWheelFeedback)
bool HoverboardHardwareInterface::disconnect()
{
leftWheel.updateEncoderTicks(motorWheelFeedback.leftMotorEncoderCumulativeCount);
rightWheel.updateEncoderTicks(motorWheelFeedback.rightMotorEncoderCumulativeCount);
bool frontDisconnected = frontSerialPortService.disconnect();
bool backDisconnected = backSerialPortService.disconnect();

return frontDisconnected && backDisconnected;
}
}

PLUGINLIB_EXPORT_CLASS(hoverboard_hardware_interface::HoverboardHardwareInterface, hardware_interface::SystemInterface)

Loading