c++ - Arduino Nano:使用串行输入时，通过计时器进行的A4988步进控制不稳定

Question

我目前正在使用Arduino Nano通过A4988驱动程序控制步进电机(加速控制)。为此，我使用timer1。我想通过蓝牙调整值。

一旦我开始通过蓝牙发送数据(即使我发送的是零，也不会影响动态)，步进器就会怪异地 Action ，并且在短时间后系统变得不稳定(这是一个自平衡机器人)。有什么想法吗？这是代码。我已将其缩短，以保持在每个帖子的最大字符数以下。

#include "MPU6050.h"
#include "BasicStepperDriver.h"
#include "Kalman.h"
//Definitions
#define RPM 600
#define Microsteps 16
#define CPUFreq 16000000
#define PreC 8
//left
#define Dir 5
#define Step 6
//right
#define Dir1 9
#define Step1 10
#define MS1 4
#define MS2 3
#define MS3 2
#define NUM_SAMPLES 10
//BT Read
int VRead = 0;
float add = 0;
const byte numChars = 32;
char receivedChars[numChars];
static boolean recvInProgress = false;
boolean newData = false;

//Regler
float P = 60.0;
float D = 10;
int sum = 0;
unsigned char sample_count = 0;
float voltage = 0.0;


//Frequency variables
int timerInterval = 10; //Interval to trigger timer interrupt in microseconds
long interruptFreq = 1000000 / timerInterval;
int compare = (CPUFreq / (8 * interruptFreq)) - 1; //compare Value, e.g. 39 in thise case
long pulseFreq;
float calcRPM = 0;
float calcACC = 1;
int SpeedToTimerCount = 0;
int SpeedToTimerCountTemp = 0;
int SpeedToTimerCountCurrent = 10;
int SpeedToTimerCountMemory = 0;
int counter;
boolean emergency = false;
//IMU + Kalman
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
uint32_t timer;
float KalmanAngle;
float KalmanAngleLast;
float arel = 0;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
Kalman kalmanX; // Create the Kalman instances

const unsigned long READ_PERIOD = 20;
const unsigned long SEND_PERIOD = 200;
unsigned long lastTime = 0;
unsigned long lastSendTime = 0;

void setup() {
  pinMode(Step1, OUTPUT); //initilize motor driver output pins
  pinMode(Dir1, OUTPUT);
  pinMode(Step, OUTPUT); //initilize motor driver output pins
  pinMode(Dir, OUTPUT);
  pinMode(MS1, OUTPUT);
  pinMode(MS2, OUTPUT);
  pinMode(MS3, OUTPUT);
  pinMode(A2, INPUT);
  digitalWrite(MS1, HIGH);
  digitalWrite(MS2, HIGH);
  digitalWrite(MS3, HIGH);
  //8 microsteps
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
  Serial.begin(115200);
  Serial.println("Initializing I2C devices...");

  accelgyro.initialize();
  accelgyro.setDLPFMode(MPU6050_DLPF_BW_5);  //5 Hz DLPF
  accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);   // Accel Range of +- 2g

  // verify connection
  Serial.println("Testing device connections...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  //Set the timer to interrupt the program with the Frequency of interruptFreq (50kHz). This enables Pulses of 50kHz or slower which equals a maximum RPM of about 900
  //Once the timer triggers, ISR(TIMER2_COMPA_vect) gets called
  /* TCCR2A = 0;                                                               //Make sure that the TCCR2A register is set to zero
    TCCR2B = 0;                                                               //Make sure that the TCCR2A register is set to zero
    TIMSK2 |= (1 << OCIE2A);                                                  //Set the interupt enable bit OCIE2A in the TIMSK2 register
    TCCR2B |= (1 << CS21);                                                    //Set the CS21 bit in the TCCRB register to set the prescaler to 8
    OCR2A = compare;                                                          //The compare register is set to float compare
    TCCR2A |= (1 << WGM21);     */                                              //Set counter 2 to CTC (clear timer on compare) mode
  // STEPPER MOTORS INITIALIZATION
  // TIMER1 CTC MODE
  TCCR1B &= ~(1 << WGM13);
  TCCR1B |=  (1 << WGM12);
  TCCR1A &= ~(1 << WGM11);
  TCCR1A &= ~(1 << WGM10);

  // output mode = 00 (disconnected)
  TCCR1A &= ~(3 << COM1A0);
  TCCR1A &= ~(3 << COM1B0);

  // Set the timer pre-scaler
  // Generally we use a divider of 8, resulting in a 2MHz timer on 16MHz CPU
  TCCR1B = (TCCR1B & ~(0x07 << CS10)) | (2 << CS10);

  //OCR1A = 125;  // 16Khz
  //OCR1A = 100;  // 20Khz
  OCR1A = compare;   // 25Khz
  TCNT1 = 0;
  TIMSK1 |= (1 << OCIE1A); // Enable Timer1 interrupt
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  kalmanX.setAngle(asin(maptoG(ay) / 9810) * 180 / PI);
  KalmanAngleLast = 0;
  Serial.println("Setup complete");
  timer = micros();
}

void loop() {
  recvWithStartEndMarkers();
  //ensure stability running with a fixed samplerate of 20ms
  if (millis() - lastTime >= READ_PERIOD) {
    calcRPM = calcRPM + calcACC * READ_PERIOD / 1000 + +add;
    lastTime += READ_PERIOD;
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // Hier: Drehung um X -> ay, gx interessant
    double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
    timer = micros();
    float angle = atan2(maptoG(ay), maptoG(az)) * 180 / PI;
    KalmanAngle = kalmanX.getAngle(angle, gx / 131.0, dt);
    if (abs(KalmanAngle) > 37.5) {
      emergency = true;
    }
    arel =  (KalmanAngle - KalmanAngleLast) / dt;
    int deadzone = 2;
    // RPM/s
    calcACC = (P * KalmanAngle + D * (KalmanAngle - KalmanAngleLast) / dt);
    KalmanAngleLast = KalmanAngle;

    // AdjustSpeed via following formula: pulseFreq = RPM/60*200*Microsteps;
    pulseFreq = (-calcRPM / 60) * 200 * Microsteps ;
    if (abs(KalmanAngle) < 0 || emergency) {
      SpeedToTimerCount = 0;
    }
    else {
      if (pulseFreq != 0) {
        SpeedToTimerCountTemp = interruptFreq / (2 * pulseFreq);
      }
      if ((abs(SpeedToTimerCount) == 1000 && abs(SpeedToTimerCountTemp) < 950) || SpeedToTimerCount < 1000) {
        if (SpeedToTimerCountTemp < 5 && SpeedToTimerCountTemp >= 0) {
          SpeedToTimerCount = 5;
        }
        else if (SpeedToTimerCountTemp > -5 && SpeedToTimerCountTemp <= 0) {
          SpeedToTimerCount = -5;
        }
        else if (abs(SpeedToTimerCountTemp) > 1000) {
          if (SpeedToTimerCountTemp > 0) {
            SpeedToTimerCount = 1000;
          }
          else if (SpeedToTimerCountTemp < 0) {
            SpeedToTimerCount = -1000;
          }
        }
        else {
          SpeedToTimerCount = SpeedToTimerCountTemp ;
        }

      }
    }
    if (sample_count < NUM_SAMPLES) {
      sum += analogRead(A2);
      sample_count++;
    }
    else {
      voltage = ((float)sum / (float)NUM_SAMPLES * 5.0) / 1024.0;
      sample_count = 0;
      sum = 0;
    }

    if (newData == true) {
      VRead = atoi(receivedChars);
      add = mapfloat(VRead, 0, 100, 0, 1.5);
      newData = false;
    }
    Serial.print(add);
    Serial.print(";");
    Serial.println(KalmanAngle);
  }
}
double mapfloat(double val, double in_min, double in_max, double out_min, double out_max) {
  return (val - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

ISR(TIMER1_COMPA_vect) {
  counter++;
  if (counter >= abs(SpeedToTimerCountMemory)) {
    SpeedToTimerCountMemory = SpeedToTimerCount;
    counter = 0;
    if (SpeedToTimerCountMemory < 0) {
      PORTB &= 0b11111101;
      PORTD |= 0b00100000;
    }
    else {
      PORTB |= 0b00000010;
      PORTD &= 0b11011111;
    }
  }
  else if (counter == 1) {
    PORTB |= 0b00000100;
    PORTD |= 0b01000000;

  }
  else if (counter == 2) {
    PORTB &= 0b11111011;
    PORTD &= 0b10111111;
  }

}
int maptoG (int16_t value) {
  int returnV = map(value, -32768, +32767, -2 * 9810, +2 * 9810);
  return returnV;
}

void recvWithStartEndMarkers() {

  static byte ndx = 0;
  char startMarker = '<';
  char endMarker = ';';
  char rc;

  if (Serial.available() > 0 && newData == false) {
    rc = Serial.read();
    if (recvInProgress == true) {
      if (rc != endMarker) {
        receivedChars[ndx] = rc;
        ndx++;
        if (ndx >= numChars) {
          ndx = numChars - 1;
        }
      }
      else {
        receivedChars[ndx] = '\0'; // terminate the string
        recvInProgress = false;
        ndx = 0;
        newData = true;
      }
    }

    else if (rc == startMarker) {
      recvInProgress = true;
    }
  }
}

Answer 1

因此，我过去也遇到过类似的问题...当我在使用中断计时器时打开串行监视器时，发生了这种情况。我没有进一步研究这个问题，但是我很确定它与serial.print函数的编写方式有关，由于某种原因，当连接到串行端口时，有一个奇怪的故障会阻止计时器。我也可以尝试另一个不确定的计时器，但是计时器1可能链接到您不希望的代码中的另一个函数，例如delay()和println()。如果您有可能，我还可以尝试arduino mega，因为微 Controller 的处理能力更高。