自平衡杆（飞棍）arduino代码
Serial.print("电机状态: ");"启用" : "禁用");// 获取X轴角速度（度/秒）"启用" : "禁用");Serial.println("kpr=value - 设置角速度比例增益");Serial.println("kpa=value - 设置角度比例增益");Serial.println("kia=value - 设置角度积分增益");Serial.println("kd
#include <Arduino.h>
#include <driver/ledc.h>
#include <Wire.h>
#include <MPU6050_light.h>
// 电机驱动引脚定义
#define MOTOR_A_IN1 14
#define MOTOR_A_IN2 12
#define STBY_PIN 13
// PWM参数
#define PWM_FREQ 5000
#define PWM_RESOLUTION 8
#define MOTOR_A_PWM_CHANNEL LEDC_CHANNEL_0
#define MOTOR_B_PWM_CHANNEL LEDC_CHANNEL_1
// PID参数
double Kp_angle = 10; // 角度比例增益
double Ki_angle = 0; // 角度积分增益
double Kd_angle = 4; // 角度微分增益
double Kp_rate = 8; // 角速度比例增益
// 平衡参数
double targetAngle = 0.0; // 目标平衡角度（直立状态）
int maxOutput = 255; // 最大输出限制
// 变量
double angleError, lastAngleError = 0;
double angleIntegral = 0;
double angleDerivative;
double rateError;
double output;
double currentAngle; // X轴角度
double currentRate; // X轴角速度（陀螺仪数据）
bool motorEnabled = true; // 电机使能状态
unsigned long lastTime = 0;
unsigned long sampleTime = 10; // 采样时间(ms)
MPU6050 mpu(Wire);
// 串口命令处理相关变量
String inputString = ""; // 存储接收到的串口数据
boolean stringComplete = false; // 是否接收到完整命令
// 函数前置声明
void setMotorSpeed(int speed);
void handleSerialCommand();
void serialEvent();
void setup() {
Serial.begin(115200);
Wire.begin();
// 初始化电机驱动引脚
pinMode(MOTOR_A_IN1, OUTPUT);
pinMode(MOTOR_A_IN2, OUTPUT);
pinMode(STBY_PIN, OUTPUT);
//digitalWrite(STBY_PIN, HIGH); // 初始使能电机
// PWM配置
ledc_timer_config_t timer_conf = {
.speed_mode = LEDC_LOW_SPEED_MODE,
.duty_resolution = LEDC_TIMER_8_BIT,
.timer_num = LEDC_TIMER_0,
.freq_hz = PWM_FREQ,
.clk_cfg = LEDC_AUTO_CLK
};
ledc_timer_config(&timer_conf);
ledc_channel_config_t channel_A = {
.gpio_num = MOTOR_A_IN1,
.speed_mode = LEDC_LOW_SPEED_MODE,
.channel = MOTOR_A_PWM_CHANNEL,
.timer_sel = LEDC_TIMER_0,
.duty = 0,
.hpoint = 0
};
ledc_channel_config(&channel_A);
ledc_channel_config_t channel_B = {
.gpio_num = MOTOR_A_IN2,
.speed_mode = LEDC_LOW_SPEED_MODE,
.channel = MOTOR_B_PWM_CHANNEL,
.timer_sel = LEDC_TIMER_0,
.duty = 0,
.hpoint = 0
};
ledc_channel_config(&channel_B);
// 初始化MPU6050
byte status = mpu.begin();
Serial.print(F("MPU6050 status: "));
Serial.println(status);
while(status!=0){ } // 等待连接成功
Serial.println(F("Calculating offsets, do not move MPU6050"));
delay(1000);
mpu.calcOffsets(); // 校准陀螺仪
Serial.println("Done!");
Serial.println("系统初始化完成");
Serial.println("输入以下命令:");
Serial.println("1 - 切换电机使能状态");
Serial.println("kpa=value - 设置角度比例增益");
Serial.println("kia=value - 设置角度积分增益");
Serial.println("kda=value - 设置角度微分增益");
Serial.println("kpr=value - 设置角速度比例增益");
Serial.println("show - 显示当前参数和状态");
}
void loop() {
// 处理串口输入
serialEvent();
if (stringComplete) {
handleSerialCommand();
inputString = "";
stringComplete = false;
}
// 更新MPU6050数据
mpu.update();
// 定时控制
unsigned long now = millis();
if(now - lastTime < sampleTime) return;
lastTime = now;
// 获取当前X轴角度和角速度
currentAngle = mpu.getAngleX();
currentRate = mpu.getGyroX(); // 获取X轴角速度（度/秒）
// 计算角度PID
angleError = targetAngle - currentAngle;
angleIntegral += angleError;
angleDerivative = angleError - lastAngleError;
lastAngleError = angleError;
// 计算角速度误差（阻尼项）
rateError = -currentRate; // 我们希望角速度为0（减少震荡）
// 计算总输出（角度PID + 角速度阻尼）
output = Kp_angle * angleError +
Ki_angle * angleIntegral +
Kd_angle * angleDerivative +
Kp_rate * rateError;
// 输出限制
output = constrain(output, -maxOutput, maxOutput);
// 驱动电机（仅在使能状态下）
if(motorEnabled) {
setMotorSpeed(output);
} else {
setMotorSpeed(0); // 禁用状态下停止电机
}
// 调试输出
// Serial.print("Angle: ");
// Serial.print(currentAngle);
// Serial.print(" Rate: ");
// Serial.print(currentRate);
// Serial.print(" Output: ");
// Serial.println(output);
}
void setMotorSpeed(int speed) {
speed = constrain(speed, -255, 255);
if (speed == 0) {
// 停止电机
ledc_set_duty(LEDC_LOW_SPEED_MODE, MOTOR_A_PWM_CHANNEL, 0);
ledc_update_duty(LEDC_LOW_SPEED_MODE, MOTOR_A_PWM_CHANNEL);
ledc_set_duty(LEDC_LOW_SPEED_MODE, MOTOR_B_PWM_CHANNEL, 0);
ledc_update_duty(LEDC_LOW_SPEED_MODE, MOTOR_B_PWM_CHANNEL);
} else if (speed > 0) {
// 电机正转
ledc_set_duty(LEDC_LOW_SPEED_MODE, MOTOR_A_PWM_CHANNEL, speed);
ledc_update_duty(LEDC_LOW_SPEED_MODE, MOTOR_A_PWM_CHANNEL);
ledc_set_duty(LEDC_LOW_SPEED_MODE, MOTOR_B_PWM_CHANNEL, 0);
ledc_update_duty(LEDC_LOW_SPEED_MODE, MOTOR_B_PWM_CHANNEL);
} else {
// 电机反转
ledc_set_duty(LEDC_LOW_SPEED_MODE, MOTOR_A_PWM_CHANNEL, 0);
ledc_update_duty(LEDC_LOW_SPEED_MODE, MOTOR_A_PWM_CHANNEL);
ledc_set_duty(LEDC_LOW_SPEED_MODE, MOTOR_B_PWM_CHANNEL, -speed);
ledc_update_duty(LEDC_LOW_SPEED_MODE, MOTOR_B_PWM_CHANNEL);
}
}
void serialEvent() {
while (Serial.available()) {
char inChar = (char)Serial.read();
inputString += inChar;
if (inChar == '\n') {
stringComplete = true;
}
}
}
void handleSerialCommand() {
inputString.trim();
if (inputString.equals("1")) {
motorEnabled = !motorEnabled; // 切换使能状态
digitalWrite(STBY_PIN, motorEnabled ? HIGH : LOW);
Serial.print("电机已");
Serial.println(motorEnabled ? "启用" : "禁用");
}
else if (inputString.startsWith("kpa=")) {
Kp_angle = inputString.substring(4).toFloat();
Serial.print("Kp_angle set to: ");
Serial.println(Kp_angle);
}
else if (inputString.startsWith("kia=")) {
Ki_angle = inputString.substring(4).toFloat();
Serial.print("Ki_angle set to: ");
Serial.println(Ki_angle);
}
else if (inputString.startsWith("kda=")) {
Kd_angle = inputString.substring(4).toFloat();
Serial.print("Kd_angle set to: ");
Serial.println(Kd_angle);
}
else if (inputString.startsWith("kpr=")) {
Kp_rate = inputString.substring(4).toFloat();
Serial.print("Kp_rate set to: ");
Serial.println(Kp_rate);
}
else if (inputString.equals("show")) {
Serial.println("Current Parameters:");
Serial.print("Kp_angle: "); Serial.print(Kp_angle);
Serial.print(" Ki_angle: "); Serial.print(Ki_angle);
Serial.print(" Kd_angle: "); Serial.print(Kd_angle);
Serial.print(" Kp_rate: "); Serial.println(Kp_rate);
Serial.print("Current Angle: "); Serial.print(currentAngle);
Serial.print(" Current Rate: "); Serial.print(currentRate);
Serial.print(" Output: "); Serial.println(output);
Serial.print("电机状态: "); Serial.println(motorEnabled ? "启用" : "禁用");
}
else {
Serial.println("Unknown command");
Serial.println("Available commands:");
Serial.println("1 - 切换电机使能状态");
Serial.println("kpa=value - set angle proportional gain");
Serial.println("kia=value - set angle integral gain");
Serial.println("kda=value - set angle derivative gain");
Serial.println("kpr=value - set rate proportional gain");
Serial.println("show - display current parameters and status");
}
}
智能硬件社区聚焦AI智能硬件技术生态，汇聚嵌入式AI、物联网硬件开发者，打造交流分享平台，同步全国赛事资讯、开展 OPC 核心人才招募，助力技术落地与开发者成长。
更多推荐
20
0- 0
已为社区贡献1条内容
所有评论(0)