人形机械臂肩部回零故障求助：到达90度后电机无法停止

设备与需求 #

• 硬件配置：搭载霍尔编码器（每圈输出17脉冲）的高扭矩DC电机，搭配1:625减速箱；Arduino控制器+L298 H桥驱动模块；限位开关
• 功能需求：设备上电后，肩部先逆时针转动寻找限位开关，触发限位后重置脉冲计数，随后顺时针转动2656.25脉冲（对应90度）后停止
• 当前故障：触发限位开关后，电机正常启动顺时针转动，但到达目标脉冲数后无法停止

问题代码 #

#define Encoder_output_A 2
#define Encoder_output_B 3
int y;
int pos = 0;
int motorpin1=5;
int motorpin2=4;
float Count_pulses = 0;
float Count_rounds = 0;
int Limswitch = 7;

void setup() {
  Serial.begin(9600); // 开启串口通信
  pinMode(Encoder_output_A,INPUT); // 设置编码器A相为输入
  pinMode(Encoder_output_B,INPUT); // 设置编码器B相为输入
  attachInterrupt(digitalPinToInterrupt(Encoder_output_A),DC_Motor_Encoder,RISING);
  pinMode(motorpin1,OUTPUT);
  pinMode(motorpin2,OUTPUT);
  pinMode(Limswitch,INPUT);
  homing();
}
 
void loop() {
  Serial.print("Result: ");
  Serial.println(Count_pulses); 
  Serial.println(Count_rounds);
  delay(100);

  y=digitalRead(Limswitch);

  if(y==LOW ||Count_pulses>=2656.25)// 17 x 625 x 0.25 = 2656.25
  {
    digitalWrite(motorpin1,LOW);
    digitalWrite(motorpin2,LOW);
  } else{
    digitalWrite(motorpin1,HIGH);
    digitalWrite(motorpin2,LOW);
  }
}
  
// 编码器脉冲计数函数
void DC_Motor_Encoder(){
  int b = digitalRead(Encoder_output_B);
  if(b > 0){
    Count_pulses++;
    Count_rounds = Count_pulses/17;
  }
  else{
    Count_pulses--;
  }
}

void homing() {
  y=digitalRead(Limswitch);
  if(y==HIGH && Count_pulses==0)
  {
    while(y==HIGH)
    {
      digitalWrite(motorpin1,HIGH);
      digitalWrite(motorpin2,LOW);
      y=digitalRead(Limswitch); 
    }
  } 
  else if(y==LOW){
    while(Count_pulses < 2657){
      digitalWrite(motorpin1,LOW);
      digitalWrite(motorpin2,HIGH);
    }
  }
  if(Count_pulses>=2656.25)
    digitalWrite(motorpin1,LOW);
    digitalWrite(motorpin2,LOW);
}

故障原因分析 #

1. 浮点计数精度问题：用float类型存储脉冲数，数值比较时会出现精度误差，无法准确触发停止条件
2. 回零循环逻辑错误：while(Count_pulses < 2657)的条件设置偏离目标值2656.25，导致电机持续转动无法停止
3. 限位触发后未重置计数：触发限位后未将Count_pulses清零，计数从初始值累加，无法准确计算90度对应的脉冲数
4. 限位开关引脚浮空：未启用内部上拉/下拉电阻，引脚状态不稳定易导致误判

修复后的代码 #

#define Encoder_output_A 2
#define Encoder_output_B 3
#define Limswitch 7
int motorpin1 = 5;
int motorpin2 = 4;
long Count_pulses = 0; // 改用long类型避免浮点误差
float Count_rounds = 0;
bool homingCompleted = false; // 标记回零是否完成

void setup() {
  Serial.begin(9600);
  pinMode(Encoder_output_A, INPUT);
  pinMode(Encoder_output_B, INPUT);
  attachInterrupt(digitalPinToInterrupt(Encoder_output_A), DC_Motor_Encoder, RISING);
  pinMode(motorpin1, OUTPUT);
  pinMode(motorpin2, OUTPUT);
  pinMode(Limswitch, INPUT_PULLUP); // 启用内部上拉电阻，避免引脚浮空
  homing();
}

void loop() {
  Serial.print("当前脉冲数: ");
  Serial.println(Count_pulses);
  Serial.print("当前圈数: ");
  Serial.println(Count_rounds);
  delay(100);

  // 回零完成后停止电机控制
  if (homingCompleted) {
    digitalWrite(motorpin1, LOW);
    digitalWrite(motorpin2, LOW);
    return;
  }

  // 回零过程中的安全停止逻辑
  int y = digitalRead(Limswitch);
  if (y == LOW || Count_pulses >= 2656) { // 取整为2656对应90度
    digitalWrite(motorpin1, LOW);
    digitalWrite(motorpin2, LOW);
    homingCompleted = true;
  }
}

void DC_Motor_Encoder() {
  int b = digitalRead(Encoder_output_B);
  if (b > 0) {
    Count_pulses++;
  } else {
    Count_pulses--;
  }
  Count_rounds = (float)Count_pulses / 17; // 仅显示时转换为浮点
}

void homing() {
  int y = digitalRead(Limswitch);
  
  // 第一步：逆时针转动寻找限位开关
  if (y == HIGH && Count_pulses == 0) {
    while (y == HIGH) {
      digitalWrite(motorpin1, HIGH);
      digitalWrite(motorpin2, LOW);
      y = digitalRead(Limswitch);
      delay(10); // 延迟避免引脚读取过快
    }
    // 触发限位后重置脉冲计数为0
    Count_pulses = 0;
  }

  // 第二步：顺时针转动90度（2656脉冲）
  while (Count_pulses < 2656) {
    digitalWrite(motorpin1, LOW);
    digitalWrite(motorpin2, HIGH);
    delay(1); // 短延迟确保中断有时间更新计数
  }

  // 停止电机并标记回零完成
  digitalWrite(motorpin1, LOW);
  digitalWrite(motorpin2, LOW);
  homingCompleted = true;
}

关键修改说明 #

1. 计数类型优化：将Count_pulses改为long类型，彻底避免浮点精度问题，脉冲计数为整数更符合物理意义
2. 回零逻辑修正：触发限位后立即重置Count_pulses为0，确保从零点开始计数90度脉冲数
3. 循环条件调整：将目标脉冲数取整为2656，循环条件改为Count_pulses < 2656，保证到达目标值后立即停止
4. 引脚稳定性优化：给限位开关启用内部上拉电阻，避免引脚浮空导致的误触发
5. 状态标记添加：增加homingCompleted变量，避免loop逻辑干扰已完成的回零操作

内容的提问来源于stack exchange，提问作者Sabih