步进电机篇

中断控制方法和PID

#include <MPU6050_tockn.h>
#include <Wire.h>

MPU6050 mpu6050(Wire);
float value, gy;
#define PULSE_WIDTH 1

//-------------------------------------------------------------------------------------------------卡尔曼滤波
typedef struct
{
  float LastP;  //上次估算协方差 初始化值为0.02
  float Now_P;  //当前估算协方差 初始化值为0
  float out;    //卡尔曼滤波器输出 初始化值为0
  float Kg;     //卡尔曼增益 初始化值为0
  float Q;      //过程噪声协方差 初始化值为0.001
  float R;      //观测噪声协方差 初始化值为0.543
} KFP;          //Kalman Filter parameter
float kalmanFilter(KFP *kfp, float input) {
  kfp->Now_P = kfp->LastP + kfp->Q;
  kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
  kfp->out = kfp->out + kfp->Kg * (input - kfp->out);  //因为这一次的预测值就是上一次的输出值
  kfp->LastP = (1 - kfp->Kg) * kfp->Now_P;
  return kfp->out;
}
KFP KFP_X = { 0.02, 0, 0, 0, 0.1, 0.001 };// Q小R大滤波平滑
//-------------------------------------------------------------------------------------------------卡尔曼滤波


//-------------------------------------------------------------------------------------------------PID控制器
float Angle_XX = 0.0f;
double angleoutput = 0;
float kp = 20, kd = 1;
float middleAngle = 0;
double angleout() {
  angleoutput = kp * (Angle_XX + middleAngle) + kd * gy;  //PD算法
  return angleoutput;
}
//-------------------------------------------------------------------------------------------------PID控制器


//-------------------------------------------------------------------------------------------------定时器中断
uint8_t EN = 0;
volatile unsigned long currentTick = 0UL;
volatile unsigned long ticksPerPulse = UINT64_MAX;
void setTimer1(int ocra) {
  TCCR1A = 0;  // set entire TCCR1A register to 0
  TCCR1B = 0;  // same for TCCR1B
  TCNT1 = 0;   // initialize counter value to 0
  OCR1A = ocra;
  TCCR1B |= (1 << WGM12);   // turn on CTC mode
  TCCR1B |= (1 << CS11);    // set prescaler to 8
  TIMSK1 |= (1 << OCIE1A);  // enable timer compare interrupt
}
void setTimers() {
  cli();
  setTimer1(49);  // 40kHz
  sei();
}
//-------------------------------------------------------------------------------------------------PID控制器


void setup() {
  // put your setup code here, to run once:
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  digitalWrite(2, LOW);
  Serial.begin(9600);
  Wire.begin();
  mpu6050.begin();
  //mpu6050.calcGyroOffsets(true);

  setTimers();
  ticksPerPulse = 40;
}


void loop() {
  mpu6050.update();
  Angle_XX = kalmanFilter(&KFP_X, mpu6050.getAngleX());
  gy = mpu6050.getGyroY();

  if (Angle_XX > 0) digitalWrite(2, LOW);
  if (Angle_XX <= 0) digitalWrite(2, HIGH);
  
  if(Angle_XX > -0.1&&Angle_XX < 0.1){
    ticksPerPulse = UINT64_MAX;
  }
  else{
    ticksPerPulse = abs((int)(3500/angleout()));
  }
  
  Serial.println(Angle_XX);
}

ISR(TIMER1_COMPA_vect) {
  if (EN == 0) {
    if (currentTick >= ticksPerPulse) {
      currentTick = 0;
    }
    if (currentTick == 0) {
      PORTD |= _BV(PD3);  // digitalWrite(MOT_B_STEP, HIGH);
    } else if (currentTick == PULSE_WIDTH) {
      PORTD &= ~_BV(PD3);  // digitalWrite(MOT_B_STEP, LOW);
    }
    currentTick++;
  }
}

电机运动快速测试

#define STEP_PIN 13

void setup() {
  pinMode(STEP_PIN,OUTPUT);
}

void loop() {
  digitalWrite(STEP_PIN,LOW);
  delayMicroseconds(80);
  digitalWrite(STEP_PIN,HIGH);
  delayMicroseconds(1);
}

旋转编码器控制步进电机

const int pin_A = 2;
const int pin_B = 3;
const int pin_BT = 6;
const int pin_DIR = 4;
const int pin_CLK = 5;

int previousDir = 0; // 用于存储上一次的 dir 值

boolean CW_1 = 0;
boolean CW_2 = 0;

int dir = 0;
int flag = 0;
int final_DIR = 0;

void printsth() {
  Serial.println(dir);
  final_DIR = dir;
}

static int previous_value = 0;
void checkAndExecuteFunction(int current_value, void (*func)(int)) {
  if (current_value != previous_value) {
    // 参数不同，调用传入的函数指针所指向的函数
    (*func)(current_value);
    // 更新上一次的参数为当前值
    previous_value = current_value;
  }
}

void setup() {
  Serial.begin(9600);

  pinMode(pin_A, INPUT);
  pinMode(pin_BT, INPUT);
  pinMode(pin_B, INPUT);
  pinMode(pin_DIR, OUTPUT);
  pinMode(pin_CLK, OUTPUT);

  attachInterrupt(digitalPinToInterrupt(2), handleInterrupt, CHANGE);
}

void loop() {
 int dirChange = final_DIR - previousDir; // 计算 dir 的变化量
  if (dirChange != 0) {
    int stepsToMove = abs(dirChange); // 获取变化量的绝对值，即要移动的步数

    for (int i = 0; i < stepsToMove*32; i++) {
      digitalWrite(pin_CLK,LOW);
      delayMicroseconds(40);
      digitalWrite(pin_CLK,HIGH);
      delayMicroseconds(1);
    }

    previousDir = final_DIR; // 更新 previousDir 的值为当前的 dir
  }
}


void handleInterrupt() {
  boolean Val_A = digitalRead(pin_A);
  boolean Val_B = digitalRead(pin_B);
  if (flag == 0 && Val_A == LOW) {
    CW_1 = Val_B;
    flag = 1;
  }
  if (flag == 1 && Val_A == HIGH) {
    CW_2 = !Val_B;
    if (CW_1 == true && CW_2 == true) {
      dir++;
      digitalWrite(pin_DIR, LOW);
    }
    if (CW_1 == false && CW_2 == false) {
      dir--;
      digitalWrite(pin_DIR, HIGH);
    }
    flag = 0;
  }
checkAndExecuteFunction(dir, printsth);
}