Arduino Nano:A4988 使用串行输入时通过定时器进行步进控制不稳定

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

一旦我开始通过蓝牙发送数据(即使我发送一个零，这不会影响动态(，步进器的行为很奇怪，系统在短时间内变得不稳定(它是一个自平衡机器人(。知道为什么吗？这是代码。我缩短了它，以保持在每个帖子的最大字符数以下。

#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] = ''; // terminate the string
recvInProgress = false;
ndx = 0;
newData = true;
}
}
else if (rc == startMarker) {
recvInProgress = true;
}
}
}