直流电机/伺服电机控制程序

用Tool IO控制末端控制板AUBO i5 / 上位机 │ │ Tool IO 输出命令 ▼ 自制末端控制板 STM32 / Arduino / ESP32 │ ├── 夹持电机 ├── 剪切电机 / 舵机 / 电磁铁 ├── 限位开关 ├── 力传感器 └── 电机驱动器AUBO 端只负责发命令真正的夹持、剪切、复位、停止逻辑放在控制板里层级负责内容AUBO SDK机械臂运动、发送夹持/剪切/复位/停止命令STM32 控制板电机驱动、传感器读取、夹持力控制、安全保护电机驱动器实际驱动夹爪和剪切机构传感器位置、力、电流、限位反馈一、AUBO SDK 端代码逻辑auto io robot-getIoControl(); // 设置末端工具电压为 24 V io-setToolVoltageOutputDomain(24); // 设置 Tool IO 为输出 io-setToolIoInput(0, false); // DO0夹持 io-setToolIoInput(1, false); // DO1剪切 io-setToolIoInput(2, false); // DO2复位 io-setToolIoInput(3, false); // DO3停止 // 发送夹持命令 void sendClamp() { io-setToolDigitalOutput(0, true); std::this_thread::sleep_for(std::chrono::milliseconds(200)); io-setToolDigitalOutput(0, false); } // 发送剪切命令 void sendCut() { io-setToolDigitalOutput(1, true); std::this_thread::sleep_for(std::chrono::milliseconds(200)); io-setToolDigitalOutput(1, false); } // 发送复位命令 void sendReset() { io-setToolDigitalOutput(2, true); std::this_thread::sleep_for(std::chrono::milliseconds(200)); io-setToolDigitalOutput(2, false); } // 发送停止命令 void sendStop() { io-setToolDigitalOutput(3, true); std::this_thread::sleep_for(std::chrono::milliseconds(200)); io-setToolDigitalOutput(3, false); }二、STM32 末端控制板代码逻辑STM32 端读取 AUBO 的 IO 输入然后执行对应动作void loop() { if (READ_CLAMP_CMD()) { clamp_action(); } if (READ_CUT_CMD()) { cut_action(); } if (READ_RESET_CMD()) { reset_action(); } if (READ_STOP_CMD()) { stop_action(); } safety_check(); }三、夹持动作逻辑void clamp_action() { state CLAMPING; motor_set_dir(CLOSE_DIR); motor_set_pwm(CLAMP_PWM); while (state CLAMPING) { force read_force_sensor(); current read_motor_current(); position read_encoder(); if (force FORCE_LIMIT) { motor_stop(); state IDLE; break; } if (current CURRENT_LIMIT) { motor_stop(); state IDLE; break; } if (position CLOSE_POSITION_LIMIT) { motor_stop(); state IDLE; break; } if (READ_STOP_CMD()) { stop_action(); break; } } }四、剪切动作逻辑void cut_action() { state CUTTING; cutter_set_dir(CUT_DIR); cutter_set_pwm(CUT_PWM); while (state CUTTING) { cutter_current read_cutter_current(); cutter_position read_cutter_encoder(); if (cutter_position CUT_END_POSITION) { cutter_stop(); state IDLE; break; } if (cutter_current CUT_CURRENT_LIMIT) { cutter_stop(); error_flag CUT_OVERLOAD; state ERROR; break; } if (READ_STOP_CMD()) { stop_action(); break; } } }五、复位动作逻辑void reset_action() { state RESETTING; // 夹爪打开 motor_set_dir(OPEN_DIR); motor_set_pwm(RESET_PWM); while (!READ_CLAMP_HOME_SWITCH()) { if (READ_STOP_CMD()) { stop_action(); return; } } motor_stop(); // 剪刀复位 cutter_set_dir(CUTTER_OPEN_DIR); cutter_set_pwm(RESET_PWM); while (!READ_CUTTER_HOME_SWITCH()) { if (READ_STOP_CMD()) { stop_action(); return; } } cutter_stop(); state IDLE; }六、停止动作逻辑void stop_action() { state STOPPED; motor_stop(); cutter_stop(); disable_motor_driver(); disable_cutter_driver(); clear_pwm_output(); error_flag EMERGENCY_STOP; }七、AUBO 主程序流程