0

我的目标是根据 BNO055 惯性测量单元的欧拉角来控制 Nema 17 步进电机的位置和速度。我正在使用 ESP32 通过 WIFI 将代码刷新到 rosserial。我使用 12V 电源为 Nema 17 供电，使用小型外部 5V 电池组为 BNO055 供电。

总之，步进电机应在 0-4100 步之间移动，这将映射到 BNO055 y 轴的 -90 度和 90 度。

为此，我需要尽可能多地读取 BNO055 传感器的输出，并且仅在 BNO055 相对于映射改变位置时才改变 Nema 17 的方向。

我遇到的问题是，当我在代码中读取传感器时，我的电机开始抖动并且旋转不顺畅。我想知道如何让这两件事同时工作（读取传感器和移动 nema 17）。

PS：我将通过使用 BNO055 传感器计算 PI 控制并相应地调整 delayMicroseconds() 来控制速度……但首先要使读数和电机运动平稳。

下面是我用来调试这个问题的代码片段：

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

我已经尝试过 AccelStepper.h 库，但在位置控制和速度更新方面没有获得所需的输出。

4

1 回答 1

0

Arduino 的一体机loop()并不是控制实时系统的正确架构。电机控制需要相当准确的时序 - 例如，您希望以 833 Hz 的频率（从 1.2 ms 延迟）更新电机控制输出，这应该是相当准确和稳定的。

不幸的是，您并没有接近这一点，因为您在每个循环中都在做一堆非关键的事情，这可能需要很长（且不确定）的时间 - 等待 IMU 给您一个样本，打印到串行端口，与某些 ROS 组件通信等。同时，电机的实时关键控制信号正在等待这一切完成，然后才能开始工作。请注意，将几行打印到串行可能已经花费了几十毫秒，因此您delayMicroseconds(1200);类似于用卡尺测量切口，然后闭上眼睛用斧头进行切割。

实时关键进程应在其自己的线程中执行，该线程比非实时关键进程具有更高的优先级。在您的情况下，它可能应该运行一个 1.2 ms 周期的计时器。计时器处理程序的执行优先级应高于所有其他内容，使用最后接收到的传感器输入计算电机所需的输出（即在移动电机时不要向 IMU 询问新读数）并退出。

然后你可以从loop()空闲优先级运行所有其他的东西，当电机控制工作时它会被抢占。

根据 IMU 输入的准确时序有多重要，您可能还希望在单独的线程中运行它，优先级介于电机控制中断和空闲之间（请记住产生一些 CPU 周期，loop()否则它会饿死）。

于 2021-10-27T09:36:39.977 回答