Micro ROS agent stopping and restarting when driving stepper motor. Dual core

TheHassanShahzad commented 7 months ago

Issue template

Hardware description: ESP32 and NEMA 17 driven with A4988 on 1/16th step mode
Installation type: humble

Steps to reproduce the issue

I found that i cannot drive the stepper motor within the main loop where 'RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)))' is being spun. Since the steppers are extremely time sensitive, a delay of a few microseconds within the loop driving the steppers can affect the speed. I decided to use the ESP32 second core to handle stepper control and the main core for micro-ros. I have a gamepad connected where the y axis of a joystick on the gamepad is inclination and the x axis is steer_vel (these values only make sense since i am making a 2 wheel balancing robot)

Here is my code that causes the steppers to spin using the joystick for about a few seconds and then the micro-ros-agent restarts again and again.

#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/twist.h>

#include <Arduino.h>

rcl_subscription_t subscriber;
geometry_msgs__msg__Twist msg;
rclc_executor_t executor;
rcl_allocator_t allocator;
rclc_support_t support;
rcl_node_t node;

#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)){error_loop();}}

#include <AccelStepper.h>

// Define the number of steps per revolution for your stepper motor
const int step_resolution = 16;  // One 16th of a full step. Full step is 1.8 degrees
//const int STEPS_PER_REVOLUTION = 200.0 / step_size;

// Define the pins connected to the A4988 driverS
const int L_STEP_PIN = 33; 
const int L_DIR_PIN = 25; 
const int R_STEP_PIN = 12; 
const int R_DIR_PIN = 13;
const int L_MS1 = 32;
const int L_MS2 = 19;
const int L_MS3 = 18;
const int R_MS1 = 26;
const int R_MS2 = 27;
const int R_MS3 = 14;

AccelStepper L_stepper(1, L_STEP_PIN, L_DIR_PIN); // Define left stepper motor object
AccelStepper R_stepper(1, R_STEP_PIN, R_DIR_PIN); // Define right stepper motor object

TaskHandle_t Core1Task; // Task handle for Core 1

float inclination;
float steer_vel;

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

void driveStepper(void *param){
  pinMode(L_MS1, OUTPUT);
  pinMode(L_MS2, OUTPUT);
  pinMode(L_MS3, OUTPUT);
  pinMode(R_MS1, OUTPUT);
  pinMode(R_MS2, OUTPUT);
  pinMode(R_MS3, OUTPUT);

  // Set microstep pins to HIGH for 16 microsteps
  digitalWrite(L_MS1, HIGH);
  digitalWrite(L_MS2, HIGH);
  digitalWrite(L_MS3, HIGH);
  digitalWrite(R_MS1, HIGH);
  digitalWrite(R_MS2, HIGH);
  digitalWrite(R_MS3, HIGH);

  L_stepper.setMaxSpeed(8000);
  L_stepper.setAcceleration(3000);
  R_stepper.setMaxSpeed(8000);
  R_stepper.setAcceleration(3000);

  int count = 0;
  int iterations = 1000;
  int steps_per_cycle = 200;

  while (true){
    R_stepper.setSpeed(inclination*steps_per_cycle);
    R_stepper.runSpeed(); 

  }
}
//twist message cb
void subscription_callback(const void *msgin) {
  const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin;
  // if velocity in x direction is 0 turn off LED, if 1 turn on LED
  digitalWrite(LED_PIN, (msg->linear.x == 0) ? LOW : HIGH);
  inclination = msg->linear.x;
  steer_vel = msg->linear.y;
}

void setup() {

  xTaskCreatePinnedToCore(
    driveStepper,          // Function to implement the task
    "Core 1 Task",      // Name of the task
    10000,              // Stack size in words
    NULL,               // Task input parameter
    1,                  // Priority of the task
    &Core1Task,         // Task handle
    0                   // Run task on Core 1
  );

  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, Twist),
    "/combined_data"));

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

}

void loop() {
//  delay(100);
  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

Expected behavior

The stepper should always spin when the joystick feeds it commands instead of stopping the micro ros- agent and starting again

Actual behavior

stepper motor spins for a bit then stops then spins again once micro-ros-agent restablishes connection

Additional information

pablogs9 commented 7 months ago

Maybe you can increase the priority of the stepper task to a greater number than the micro-ROS task

TheHassanShahzad commented 7 months ago

Hi Pablo. I again ran into this issue in another project involving ESP NOW.

It seems that when doing tasks on another core it seems that playing with the priority level is the way to prevent the micro ros agent from deleting the session and re establishing it.

For the stepper a priority of 0 worked but for the ESP NOW I had to use 3

Not sure what is going on

micro-ROS / micro_ros_arduino