Fixed firmware and toid_bot_description

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 =====================

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);
     }

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;

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(

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<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("drivewhl_l_joint", 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_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("drivewhl_l_joint", 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_r_joint", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd);
 
     return command_interfaces;
 }

scripts/test_wheel.sh

@@ -0,0 +1,78 @@
+#!/bin/bash
+
+TARGET="$1"
+
+if [[ "$TARGET" == "-h" || "$TARGET" == "--help" ]]; then
+    exit 0
+fi
+
+if [[ $# -ne 1 ]]; then
+    echo "Error: Expected at least 1 arg"
+    exit 1
+fi
+
+source install/setup.bash
+
+trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT
+
+case $TARGET in
+    "forward") 
+        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
+          header: auto
+          twist:
+            linear:
+              x: 0.4
+              y: 0.0
+              z: 0.0
+            angular:
+              x: 0.0
+              y: 0.0
+              z: 0.0"
+        ;;
+    "backward") 
+        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
+          header: auto
+          twist:
+            linear:
+              x: -0.8
+              y: 0.0
+              z: 0.0
+            angular:
+              x: 0.0
+              y: 0.0
+              z: 0.0"
+        ;;
+    "left") 
+        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
+          header: auto
+          twist:
+            linear:
+              x: 0.0
+              y: 0.0
+              z: 0.0
+            angular:
+              x: 0.0
+              y: 0.0
+              z: 0.4"
+        ;;
+    "right") 
+        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
+          header: auto
+          twist:
+            linear:
+              x: 0.0
+              y: 0.0
+              z: 0.0
+            angular:
+              x: 0.0
+              y: 0.0
+              z: -0.4"
+        ;;
+    "zero")
+        ros2 service call /zero std_srvs/srv/Empty
+        ;;
+    *)
+        echo "Target not defined"
+        exit 1
+        ;;
+esac

toid_bot_description/src/toid_bot_control.xacro

@@ -18,14 +18,10 @@
 
       <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>