From 6156657c2eb8c6e68b603bcf122eac7aee81b98f Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 28 Jan 2026 17:09:50 +0100 Subject: [PATCH] Fixed firmware and toid_bot_description --- firmware/base/src/config.h | 6 +++++- firmware/base/src/main.c | 6 ------ firmware/base/src/stepper.c | 2 +- mg_control/launch/toid.launch.py | 4 ++-- mg_control/src/mg_control.cpp | 10 ++++++---- scripts/test_wheel.sh | 12 +++++++----- toid_bot_description/src/toid_bot_control.xacro | 6 +----- 7 files changed, 22 insertions(+), 24 deletions(-) diff --git a/firmware/base/src/config.h b/firmware/base/src/config.h index e4d7e9c..9092879 100644 --- a/firmware/base/src/config.h +++ b/firmware/base/src/config.h @@ -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 ===================== diff --git a/firmware/base/src/main.c b/firmware/base/src/main.c index bdf5a93..413d4bf 100644 --- a/firmware/base/src/main.c +++ b/firmware/base/src/main.c @@ -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); } diff --git a/firmware/base/src/stepper.c b/firmware/base/src/stepper.c index 54bbfc1..5d05d52 100644 --- a/firmware/base/src/stepper.c +++ b/firmware/base/src/stepper.c @@ -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; diff --git a/mg_control/launch/toid.launch.py b/mg_control/launch/toid.launch.py index 969a78c..b64aa65 100644 --- a/mg_control/launch/toid.launch.py +++ b/mg_control/launch/toid.launch.py @@ -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 - ]) \ No newline at end of file + ]) diff --git a/mg_control/src/mg_control.cpp b/mg_control/src/mg_control.cpp index e9612ac..276da6c 100644 --- a/mg_control/src/mg_control.cpp +++ b/mg_control/src/mg_control.cpp @@ -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 mg::MgStepperInterface::export_s std::vector 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 mg::MgStepperInterface::export std::vector 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; } diff --git a/scripts/test_wheel.sh b/scripts/test_wheel.sh index e2066c8..f6b8c18 100755 --- a/scripts/test_wheel.sh +++ b/scripts/test_wheel.sh @@ -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 diff --git a/toid_bot_description/src/toid_bot_control.xacro b/toid_bot_description/src/toid_bot_control.xacro index eac33b9..286e4f4 100644 --- a/toid_bot_description/src/toid_bot_control.xacro +++ b/toid_bot_description/src/toid_bot_control.xacro @@ -18,18 +18,14 @@ - - - - - \ No newline at end of file +