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

Fixed firmware and toid_bot_description

Browse Source
This commit is contained in:
Pimpest
2026-01-28 17:09:50 +01:00
parent d9f179bda5
commit 6156657c2e
7 changed files with 22 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,5 +1,9 @@
#pragma once
// STEP_PIN 26
// DIR_PIN 27
//############## CONFIG STEPPERS ################
#define DIR_PIN_A 26
#define DIR_PIN_B 16
@@ -10,7 +14,7 @@
#define SM_B 1
#define PULSE_PER_REV (1 / 3200.0)
#define SPEED_SET_TIMEOUT 1000
#define SPEED_SET_TIMEOUT 1000000
//###############################################
//================ CONFIG ENCODERS =====================

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

@@ -213,8 +213,6 @@ void stepper_fifo(char c, uint8_t itf) {
double vr = btod(stepper_instr + 10);
set_speeds(vl, vr);
last_speed_change = SPEED_SET_TIMEOUT;
// tud_cdc_n_write(itf, (uint8_t const *) stepper_instr, 18);
// tud_cdc_n_write_flush(itf);
}
@@ -265,10 +263,6 @@ int main()
while (true) {
if(last_speed_change--) {
set_speeds(0, 0);
last_speed_change = SPEED_SET_TIMEOUT;
}
tud_task();
sleep_ms(1);
}

2
firmware/base/src/stepper.c
View File

@@ -34,7 +34,7 @@ void start_pulse(PIO pio, uint sm, uint offset, uint pin, uint freq) {
void update_sm(PIO pio, uint sm, const uint pin ,double v) {
double u_v = fabs(v);
if(u_v > 0.0005)
if(u_v > 0.00005)
pio->txf[sm] = (int)((double)clock_get_hz(clk_sys) * PULSE_PER_REV / u_v) / 2 - 5;
else
pio->txf[sm] = 0;

4
mg_control/launch/toid.launch.py
View File

@@ -48,7 +48,7 @@ def generate_launch_description():
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_sim:=', use_mock])}]
parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}]
)
controller_manager = Node(
@@ -108,4 +108,4 @@ def generate_launch_description():
joint_state_broadcaster,
diffbot_base_controller,
rviz_node
])
])

10
mg_control/src/mg_control.cpp
View File

@@ -31,6 +31,8 @@ CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::Stat
"Failed to open serial port (Is the stepper driver plugged in)");
return CallbackReturn::ERROR;
}
RCLCPP_INFO(rclcpp::get_logger("MgStepperInterface"),
"Configured stepper motor interface");
return CallbackReturn::SUCCESS;
}
@@ -45,8 +47,8 @@ std::vector<hardware_interface::StateInterface> mg::MgStepperInterface::export_s
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.reserve(2);
state_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state);
state_interfaces.emplace_back("right_motor", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state);
state_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state);
state_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state);
return state_interfaces;
}
@@ -55,8 +57,8 @@ std::vector<hardware_interface::CommandInterface> mg::MgStepperInterface::export
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.reserve(2);
command_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd);
command_interfaces.emplace_back("right_motor", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd);
command_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd);
command_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd);
return command_interfaces;
}

12
scripts/test_wheel.sh
View File

@@ -11,6 +11,8 @@ if [[ $# -ne 1 ]]; then
exit 1
fi
source install/setup.bash
trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT
case $TARGET in
@@ -19,7 +21,7 @@ case $TARGET in
header: auto
twist:
linear:
x: 0.1
x: 0.4
y: 0.0
z: 0.0
angular:
@@ -32,7 +34,7 @@ case $TARGET in
header: auto
twist:
linear:
x: -0.1
x: -0.8
y: 0.0
z: 0.0
angular:
@@ -51,20 +53,20 @@ case $TARGET in
angular:
x: 0.0
y: 0.0
z: 0.1"
z: 0.4"
;;
"right")
ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
header: auto
twist:
linear:
x: 0.1
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.1"
z: -0.4"
;;
"zero")
ros2 service call /zero std_srvs/srv/Empty

6
toid_bot_description/src/toid_bot_control.xacro
View File

@@ -18,18 +18,14 @@
<joint name="drivewhl_l_joint">
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="drivewhl_r_joint">
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
</xacro:macro>
</robot>
</robot>