Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Updated firmware for new pcb

Browse Source
This commit is contained in:
Pimpest
2025-04-11 15:41:02 +02:00
parent a4691caeed
commit f064bcfc69
4 changed files with 34 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
firmware/base/src/config.h
View File

@ -1,10 +1,10 @@
#pragma once
//############## CONFIG STEPPERS ################
#define DIR_PIN_A 16
#define DIR_PIN_B 26
#define PULSE_PIN_A 17
#define PULSE_PIN_B 27
#define DIR_PIN_A 26
#define DIR_PIN_B 16
#define PULSE_PIN_A 27
#define PULSE_PIN_B 17
#define SM_A 0
#define SM_B 1

4
firmware/base/src/main.c
View File

@ -59,8 +59,8 @@ bool update_pos_cb() {
vel_l /=64 * ENCODER_CPR;
vel_r /=64 * ENCODER_CPR;
vel_l *= WHEEL_RADIUS * 2 * M_PI * wheel_ratio_l;
vel_r *= -WHEEL_RADIUS * 2 * M_PI * wheel_ratio_r;
vel_l *= WHEEL_RADIUS * 2 * M_PI;
vel_r *= -WHEEL_RADIUS * 2 * M_PI;
const double linear = (vel_l + vel_r) / 2;
const double angular = (vel_r - vel_l) / wheel_separation;

4
mg_control/src/mg_control.cpp
View File

@ -73,9 +73,9 @@ hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time
try {
odrive_serial_interface.Write("s;");
value.data = left_wheel_vel_cmd / (2 * M_PI);
value.data = -left_wheel_vel_cmd / (2 * M_PI);
for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
value.data = -right_wheel_vel_cmd / (2 * M_PI);
value.data = right_wheel_vel_cmd / (2 * M_PI);
for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
} catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }

34
mg_odometry/src/mg_odom_publisher.cpp
View File

@ -8,7 +8,7 @@
#include <libserial/SerialStream.h>
#include <sys/types.h>
#include "mg_msgs/srv/set_width.hpp"
#include "mg_msgs/srv/send_double.hpp"
#include "std_srvs/srv/empty.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
@ -25,8 +25,8 @@
constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
using SetWidthSrv = mg_msgs::srv::SetWidth;
using ZeroSrv = std_srvs::srv::Empty;
using SendDoubleSrv = mg_msgs::srv::SendDouble;
using ZeroSrv = std_srvs::srv::Empty;
class MgOdomPublisher : public rclcpp::Node {
public:
@ -40,8 +40,10 @@ class MgOdomPublisher : public rclcpp::Node {
timer_ = this->create_wall_timer(std::chrono::milliseconds(TIMEOUT),
std::bind(&MgOdomPublisher::timer_callback, this));
set_width_service_ = create_service<SetWidthSrv>(
set_width_service_ = create_service<SendDoubleSrv>(
"/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
set_ratio_service_ = create_service<SendDoubleSrv>(
"/set_ratio", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
zero_service_
= create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
@ -49,26 +51,42 @@ class MgOdomPublisher : public rclcpp::Node {
private:
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::Service<SetWidthSrv>::SharedPtr set_width_service_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_width_service_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_ratio_service_;
rclcpp::Service<ZeroSrv>::SharedPtr zero_service_;
rclcpp::TimerBase::SharedPtr timer_;
LibSerial::SerialStream enc_serial_port_;
void set_width(const std::shared_ptr<SetWidthSrv::Request> request) {
RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->width);
void set_width(const std::shared_ptr<SendDoubleSrv::Request> request) {
RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->data);
if (enc_serial_port_.IsOpen()) {
union {
std::array<u_char, sizeof(double)> bytes;
double data;
} value{};
value.data = request->width;
value.data = request->data;
enc_serial_port_ << "w;";
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
}
}
void set_ratio(const std::shared_ptr<SendDoubleSrv::Request> request) {
RCLCPP_INFO(get_logger(), "Setting wheel ratio to: %lf", request->data);
if (enc_serial_port_.IsOpen()) {
union {
std::array<u_char, sizeof(double)> bytes;
double data;
} value{};
value.data = request->data;
enc_serial_port_ << "r;";
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
}
}
void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
RCLCPP_INFO(get_logger(), "Zeroing odometry");