Trouble moving stepper motor using ROS2 and micro-ros

Question

I'm having some trouble trying to move a stepper motor using micro-ros. I use ROS2 Iron in my ubuntu 22.04 host computer, a teensy 4.1 with the AccelStepper library to move the stepper and micro-ros to comunicate between my computer and the teensy.

When I use a simple example code from AccelStepper library everything works fine, but when I try using a code with a subscriber getting information from a ROS2 topic the stepper no longer moves at the required speed, it just moves at the minimum speed.

I have also noticed that, if when running a simple example code from AccelStepper with no micro-ros code, I start a micro-ros agent serial communication with the teensy, the stepper will start moving erraticaly and not in a constan speed, it accelerates and deccelerates randomly. When I shut down the micro-ros agent the stepper goes back to running at a constant speed without any trouble.

In the past I managed to move a DC motor with ROS2 and micro-ros without experiencing any trouble, I am just experiencing problems with the stepper motor.

I have no clue why this happens and how I could solve this problem, I would much apriciate any help.

This is de code I'm using.

#define ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h>
#include <AccelStepper.h>


#include <micro_ros_arduino.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <geometry_msgs/msg/quaternion.h>

rcl_subscription_t subscriber;
geometry_msgs__msg__Quaternion msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;


const int numSteppers = 1;

const int stepperPullPins[numSteppers] = {7};
const int stepperDirPins[numSteppers] = {8};
const int stepperEnablePins[numSteppers] = {9};

AccelStepper stepper[numSteppers] {
  AccelStepper(1,stepperPullPins[0],stepperDirPins[0])
};

const int encoderPinsA[numSteppers] = {4};
const int encoderPinsB[numSteppers] = {3};

Encoder encoder[numSteppers] = {
  Encoder(encoderPinsA[0], encoderPinsB[0])
};


int posReal[numSteppers] = {0};
float pulses = 360.0*20.0*4.0;    
float steps = 8000.0;  

int posObjectiu[numSteppers] = {0};


#define LED_PIN 13

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void subscription_callback(const void * msgin)
{  
  const geometry_msgs__msg__Quaternion * msg = (const geometry_msgs__msg__Quaternion *)msgin;

  posObjectiu[0]=int(msg->w);
}

void setup() {

  set_microros_transports();
  
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);  
  
  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Quaternion),
    "steppers"));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));


  for (int i = 0; i < numSteppers; i++) {

    pinMode(stepperPullPins[i],OUTPUT);
    pinMode(stepperDirPins[i],OUTPUT);
    pinMode(stepperEnablePins[i],OUTPUT);
    digitalWrite(stepperEnablePins[i],HIGH);

    pinMode(encoderPinsA[i], INPUT_PULLUP);
    pinMode(encoderPinsB[i], INPUT_PULLUP);

    stepper[i].setMaxSpeed(5000.0);
    stepper[i].setAcceleration(500.0);
    stepper[i].setSpeed(100.0);

    stepper[i].setMinPulseWidth(10); 
  }
  
  delay(1000);
  
  Serial.begin(9600);
  Serial.println("Test:");

}

void loop() {

  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));

  for (int i = 0; i < numSteppers; i++) {
    
    posReal[i] = encoder[i].read()*(steps/pulses);
    Serial.println(posReal[i]);

    stepper[i].moveTo(posObjectiu[i]);
    stepper[i].setSpeed(250.0);
    stepper[i].runSpeedToPosition();

  }
  
}
``` 

Answer 1

I had the same issue when making a 2 wheeled ROS2 self balancing robot. I was using an ESP32 with 2 cores so i utillized the second core to run commands i parallel with freeRTOS. One core drove the stepper motors and the other handled communication with microROS and IMU stuff

The reason for this problem is stepper motor control deals with microsecond long steps. Any delay in the loop controlling the steppers causes things to go haywire. I couldnt even get a serial print running in the code while i tried moving a stepper motor at a certain velocity.

For teensy if you dont have a second core, i would still suggest to look into freeRTOS and have a job that is solely for stepper control. Meddle with the priority setting to make it work as required

Answer 2

I was facing similar issue also with you and I solved it by using an external NE555 based pulse generator to generate the pulse for the stepper motor. The micro-controller then only needs to care about determining the Enable and Direction of the rotation.