BNO055 to control Nema 17 stepper motor with a4988 driver via rosserial

Question

My goal is to control the position and speed of a Nema 17 stepper motor based on the euler angle of a BNO055 inertial measurement unit. I am using an ESP32 to flash the code via WIFI to rosserial. I am powering the Nema 17 with a 12V power source and the BNO055 with a small external 5V battery pack.

In summary, the stepper motor should move between 0-4100 steps which would be mapped to -90 and 90 degrees of the BNO055's y-axis.

For this, I need to read the output of the BNO055 sensor as often as possible and only change directions of the Nema 17 when the BNO055 has changed position relative to the mapping.

The PROBLEM I am having is that when I incorporate reading the sensor in my code, my motor starts to shake and does not rotate smoothly. I am wondering how I can get both things to work simultaneously (reading sensor and moving nema 17).

PS: I will control speed by calculating a PI control with the BNO055 sensor and adjusting the delayMicroseconds() accordingly... but first thing is to get the readings and motor movement smooth.

Below is a code snippet I am using to debug this problem:

#include <WiFi.h>
#include <ros.h>
#include <Wire.h>
#include <std_msgs/Header.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <HardwareSerial.h>
#include <analogWrite.h>
#include <MultiStepper.h>
#include <AccelStepper.h>
#include <Stepper.h> 
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>

  //////////////////////
  //       BNO055     //
  //////////////////////

Adafruit_BNO055 bno_master = Adafruit_BNO055(55, 0x29);
Adafruit_BNO055 bno_slave = Adafruit_BNO055(55, 0x28); 
geometry_msgs::Quaternion Quaternion;
std_msgs::String imu_msg; 

#define I2C_SDA 21
#define I2C_SCL 22
TwoWire I2Cbno = TwoWire(0); // I2C connection will increase 6Hz data transmission

float ax_m, ay_m, az_m, ax_s, ay_s, az_s; // accelerometer
float gw_m, gx_m, gy_m, gz_m, gw_s, gx_s, gy_s, gz_s; // gyroscope
float ex_m, ey_m, ez_m, ex_s, ey_s, ez_s; // euler
float qw_m, qx_m, qy_m, qz_m, qw_s, qx_s, qy_s, qz_s; // quaternions

  //////////////////////
  // WiFi Definitions //
  //////////////////////

const char* ssid = "FRITZ!Box 7430 PN"; // Sebas: "WLAN-481774"; Paula: "FRITZ!Box 7430 PN"; ICS: ICS24; Hotel Citadelle Blaye
const char* password = "37851923282869978396"; // Sebas: "Kerriganrocks!1337"; Paula: "37851923282869978396"; ICS: uZ)7xQ*0; citadelle

IPAddress server(192,168,178,112); // ip of your ROS server
IPAddress ip_address;
WiFiClient client;

int status = WL_IDLE_STATUS;
//long motorTimer = 0, getImuDataTimer = 0, millisNew = 0; //millisOld = 0,

  //////////////////////
  //   Stepper motor  //
  //////////////////////

int stepPin = 4;
int stepPinState = LOW;
int dirPin = 2;
int dirPinState = HIGH;

unsigned long millisOld1 = 0;
unsigned long millisOld2 = 0;
long motorTimer = 1; // in milliseconds
long getImuDataTimer = 10; // in milliseconds

double maxPosition = 4100;
double stepsMoved = 0;

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

class WiFiHardware {
  public:
  WiFiHardware() {};

  void init() {
    // do your initialization here. this probably includes TCP server/client setup
    client.connect(server, 11411);
  }

  // read a byte from the serial port. -1 = failure
  int read() {
    // implement this method so that it reads a byte from the TCP connection and returns it
    //  you may return -1 is there is an error; for example if the TCP connection is not open
    return client.read();         //will return -1 when it will works
  }

  // write data to the connection to ROS
  void write(uint8_t* data, int length) {
    // implement this so that it takes the arguments and writes or prints them to the TCP connection
    for(int i=0; i<length; i++)
      client.write(data[i]);
  }

  // returns milliseconds since start of program
  unsigned long time() {
     return millis(); // easy; did this one for you
  }
};

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

int i;

void chatterCallback(const std_msgs::String& msg) {
  i = atoi(msg.data);
//  s.write(i);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void setupWiFi()
{
  // WIFI setup
  WiFi.begin(ssid, password);
  Serial.print("\nConnecting to "); Serial.println(ssid);
  uint8_t i = 0;
  while (WiFi.status() != WL_CONNECTED && i++ < 20) delay(500);
  if(i == 21){
    Serial.print("Could not connect to"); Serial.println(ssid);
    while(1) delay(500);
  }
  Serial.print("Ready! Use ");
  Serial.print(WiFi.localIP());
  Serial.println(" to access client");
}
  
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

ros::Subscriber<std_msgs::String> sub("message", &chatterCallback);
ros::Publisher pub("imu_data/", &imu_msg);
ros::NodeHandle_<WiFiHardware> nh;

void setup() {
  // set the digital pins as outputs
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);

  Serial.begin(57600);
  setupWiFi();
    
  // I2C connection IMUs
  Wire.begin(I2C_SDA, I2C_SCL);
  I2Cbno.begin(I2C_SDA, I2C_SCL, 400000); 

  bno_master.begin();
  bno_slave.begin();

  // get imu calibrations
  uint8_t system, gyro, accel, mg = 0;
  bno_master.getCalibration(&system, &gyro, &accel, &mg);
  bno_slave.getCalibration(&system, &gyro, &accel, &mg);

  bno_master.setExtCrystalUse(true);
  bno_slave.setExtCrystalUse(true);

  nh.initNode();
  nh.advertise(pub);
}

/////////////////////////////
/// GET IMU DATA FUNCTION ///
/////////////////////////////

int get_imu_data(){

  imu::Vector<3> Euler_s = bno_slave.getVector(Adafruit_BNO055::VECTOR_EULER); // 100 Hz capacity by BNO055 // IF I COMMENT THIS LINE OUT AND SET VARIABLES BELOW TO SET VALUES, MY MOTOR RUNS PERFECTLY
  
  // Euler
  float ex_s = Euler_s.x();
  float ey_s = Euler_s.y();
  float ez_s = Euler_s.z();

  // putting data into string since adding accel, gyro, and both imu data becomes too cumbersome for rosserial buffer size. String is better for speed of data
  String data = String(ex_s) + "," + String(ey_s) + "," + String(ez_s) + "!";

  int length_data = data.indexOf("!") + 1;
  char data_final[length_data + 1];
  data.toCharArray(data_final, length_data + 1);
  imu_msg.data = data_final;
  
  pub.publish(&imu_msg);
  nh.spinOnce();
  
  Serial.println(ey_s);
  
  return ey_s;  // ex_s, ey_s, ez_s
}

/////////////////////////////
//         MAIN LOOP       // 
/////////////////////////////

void loop() {

   unsigned long currentMillis = millis();
   
  //////////////////
  // GET IMU DATA //
  //////////////////

  if(currentMillis - millisOld2 >= getImuDataTimer)
  {
    ey_s = get_imu_data(); 
    Serial.print(ey_s);
  }

  //////////////// 
  // MOVE MOTOR //
  ////////////////
  // later, the direction will depend on the output of ey_s

   if((dirPinState == HIGH) && (currentMillis - millisOld1 >= motorTimer))
   { 
    if(stepsMoved <= maxPosition) 
      {
       digitalWrite(dirPin, dirPinState);
       millisOld1 = currentMillis; // update time
       stepsMoved += 5;
         for(int i =0; i<=5; i++)
         {
          digitalWrite(stepPin, HIGH);
          delayMicroseconds(1200); // constant speed
          digitalWrite(stepPin, LOW);
          }
      Serial.println(stepsMoved); // checking
      }
    else if(stepsMoved > maxPosition)
      {
       dirPinState = LOW; 
       millisOld1 = currentMillis; // update time
       stepsMoved = 0;
      }
   }

   if((dirPinState == LOW) && (currentMillis - millisOld1 >= motorTimer))
   { 
    if(stepsMoved <= maxPosition) 
      {
       digitalWrite(dirPin, dirPinState);
       millisOld1 = currentMillis; // update time
       stepsMoved += 5;
         for(int i =0; i<=5; i++)
         {
          digitalWrite(stepPin, HIGH);
          delayMicroseconds(1200); // constant speed
          digitalWrite(stepPin, LOW);
          }
      Serial.println(stepsMoved); // checking
      }
    else if(stepsMoved > maxPosition)
      {
       dirPinState = HIGH; 
       millisOld1 = currentMillis; // update time
       stepsMoved = 0;
      }
   }
}

I have tried the AccelStepper.h library but not getting the outputs desired in terms of position control and speed updates.

score 0 · Answer 1 · answered Oct 27 '21 at 09:36

Arduino's all-in-one loop() is not the correct architecture for controlling real-time systems. Motor control requires rather accurate timing - e.g. looks like you wish to update motor control output with a frequency of 833 Hz (from the 1.2 ms delay) which should then be fairly accurate and stable.

Unfortunately you're not getting anywhere near this, as you're doing a bunch of non-critical stuff in each loop which potentially takes a very long (and undeterministic) amount of time - waiting for the IMU to give you a sample, printing to the serial port, talking to some ROS component, etc. Meanwhile the real-time critical control signal to your motor is waiting for all this to finish before it can do its work. Note that printing a few lines to the serial could already take dozens of milliseconds, so your delayMicroseconds(1200); is analogous to measuring a cut with a caliper and then making the cut with an axe with your eyes closed.

A real-time critical process should execute in its own thread which has higher priority than the non-real-time critical stuff. In your case it should probably run off a timer with a 1.2 ms period. The timer handler should execute with higher priority than all the other stuff, calculate desired output to motor using last received sensor input (i.e. don't go asking the IMU for a fresh reading when it's time to move the motor) and exit.

Then you can run all the other stuff from the loop() in idle priority which simply gets pre-empted when the motor control does its work.

Depending on how critical the accurate timing of IMU input is, you may want to run this also in a separate thread with a priority somewhere between the motor control interrupt and idle (remember to yield some CPU cycles to loop() or it'll starve).

Thanks for your reply. So what I understand from what you said is that I should read in the sensor data less frequently to allow the motor to move. Depending on the interval at which I read my data, the motor might still jitter (say if I read IMU data before the motor finished moving) or it might overshoot the position (say if new IMU data is read after target position is reached). Did I understand you correctly on this regard? Additionally, I am not familiar with threads. Is this an alternative to `loop()` ? Finally, is there an alternative to Arduino IDE that allows for real-time systems? — Paulita, Oct 27 '21 at 12:26
No, you can read in the sensor data as frequently as you're able to. I have no idea how long is the delay between requesting data from IMU and getting the response, but this obviously limits your maximum sampling frequency. It's just that you shouldn't let your motor wait while you're doing other stuff which takes time and doesn't have deadlines. If you're guaranteed to get a response from the IMU quickly enough (e.g. less than a millisecond) and without much deviance (i.e. it always takes 1 ms +/- 0.5 ms), go ahead and read it each time before you run the motor control algorithm at 833 Hz. — Tarmo, Oct 27 '21 at 12:42
Now that I think of it, you probably can't run a stepper motor at 833 Hz so you'll probably have more time than 1.2 ms between motor control events. — Tarmo, Oct 27 '21 at 12:48

BNO055 to control Nema 17 stepper motor with a4988 driver via rosserial

1 Answers1