ARDUINO+MPU6050+步进电机的自平衡小车
本帖最后由 ANDOON 于 2013-6-9 20:12 编辑先亮出我的程序，参看大神们的：
/*****************====================================********************/
//**************** 一阶互补滤波+ PID+ MPU6050********************/
//************************************************************************//
#include <Wire.h>
#include <i2cdev.h>
#include <MPU6050.h>
#include <AFMotor.h>
//*********************************************************/
//*********************************************************/
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
//********************************************************/
//*************************L293D**************************/
// two stepper motors one on each port
AF_Stepper motor1(200, 1);//LEFT
AF_Stepper motor2(200, 2);//RIGHT
#define Gry_offset 296//陀螺仪X轴的静态飘移。
#define Gyr_Gain -0.00763//由陀螺仪X轴读数转换为角速度值，=1/131 敏感比例因子131、65.5、32.8、16.4 LSB/s，向前方向与X轴为反方向，故乘以-1
#define ACC_Gain 0.000061//由读数转换为加速度值，=1/16384Sensitivity Scale Factor 16384、8192、4096、2048 LSB/g
#define pi 3.1415926
#define K 0.715//一阶互补滤波权重取值，不知由来，可更改。
/********** 互补滤波器参数 *********/
unsigned long now ;
unsigned long preTime = 0;
float angleG = 0;
/*********** PID控制器参数 *********/
unsigned long lastTime; // 前次时间
float ITerm, lastInput; // 积分项、前次输入
float Output = 0.0; // PID输出值
float LOutput,ROutput;
float LSpeed_Need=0.0,RSpeed_Need=0.0;// 转弯设置
void setup(){
Wire.begin();
Serial.begin(38400);
accelgyro.initialize();//MPU6050初始化。
motor1.setSpeed(10);
motor2.setSpeed(10);
}
void loop(){
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取六轴数值
float Y_Accelerometer = ay * ACC_Gain; //转换为向前的加速度(g)，为负值
float Z_Accelerometer = az * ACC_Gain; //转换为向下的加速度(g)
float angleA= atan(Y_Accelerometer/ Z_Accelerometer)* (-180)/ pi;//加速度仪，反正切获得弧度值，乘以180度/pi
//获得角度值，乘以-1得正值
ANGLE_1(); //一阶互补滤波后的输出倾斜角度
if (abs(angleA)<45) {// 角度小于45度 运行程序
PIDD(); //PID控制器
DISTAN(); //左右电机步进距离输出
}
else { // 角度大于45度 停止PWM输出
motor1.release();
motor2.release();
}
Serial.print(angleG); Serial.print("\t");
Serial.println(Output);
}
//*********************************************************************/
//*************angleG= 一阶互补滤波后的输出倾斜角度(o)**********************/
void ANGLE_1()
{
now = millis();//当前时间(ms)
float dt = (now - preTime) / 1000.0;//微分时间(s)
preTime = now;//上一次采样时间(ms)
float gx_revised = gx + Gry_offset;//陀螺仪x轴静态时修正后的角速度读数，向前为负值
float omega= Gyr_Gain* gx_revised;//陀螺仪，转换为向前的角速度(o/s)，Gyr_Gain取负，故负负得正
float angle_dt = omega * dt;//角度的单次积分值
//angleG+= angle_dt;//陀螺仪，积分获得角度值
angleG+= (Gyr_Gain * (gx + Gry_offset)) * dt;
float A= K/ (K+ dt);//陀螺仪的权值
angleG= A* (angleG+ omega* dt)+ (1-A)* angleA;//一阶互补滤波后的输出角度(o)
}
//************************PID控制器*************************************/
//*************步进电机根据倾斜程度调整输出距离***************************/
//************左电机输出距离LOutput**************************************/
//************右电机输出距离ROutput**************************************/
//********************************************************************/
void PIDD(){
}
//**********************左右电机步进距离输出******************************/
//***************************************************************/
void DISTAN(){
stepper1.setMaxSpeed(300.0);
stepper1.setAcceleration(100.0);
stepper1.moveTo(LOutput);
stepper2.setMaxSpeed(300.0);
stepper2.setAcceleration(100.0);
stepper2.moveTo(ROutput);
stepper1.run();
stepper2.run();
}
望大神们帮忙修改PID控制和步进电机控制程序，小弟谢谢了。 电机驱动是L293D模块。 那平衡怎么和前进结合的？？？ 很复杂啊，看不懂==。
把这些东西弄成一个库该多好 学慧放弃 发表于 2013-6-9 19:29 static/image/common/back.gif
那平衡怎么和前进结合的？？？
//************************PID控制器*************************************/
//*************步进电机根据倾斜程度调整输出步进距离***************************/
//************左电机输出距离LOutput**************************************/
//************右电机输出距离ROutput**************************************/
//********************************************************************/
void PIDD(){
}
//**********************左右电机步进距离输出******************************/
//***************************************************************/
void DISTAN(){
stepper1.setMaxSpeed(300.0);
stepper1.setAcceleration(100.0);
stepper1.moveTo(LOutput);
stepper2.setMaxSpeed(300.0);
stepper2.setAcceleration(100.0);
stepper2.moveTo(ROutput);
stepper1.run();
stepper2.run();
}
ANDOON 发表于 2013-6-9 19:39 static/image/common/back.gif
//************************PID控制器*************************************/
//*************步进电机 ...
喔，你那小车平衡性能好不？？？不是说陀螺仪算法还是有很大误差吗？？ 学慧放弃 发表于 2013-6-9 19:47 static/image/common/back.gif
喔，你那小车平衡性能好不？？？不是说陀螺仪算法还是有很大误差吗？？
误差不大。。不会控制它平衡，不会算应该让小车步进多少，才能平衡。 ANDOON 发表于 2013-6-9 19:58 static/image/common/back.gif
误差不大。。不会控制它平衡，不会算应该让小车步进多少，才能平衡。
你可以参考德国四轴的算法看看 步进电机做不了平衡车吧？ L╃→煌/xin 发表于 2013-8-17 16:24 static/image/common/back.gif
步进电机做不了平衡车吧？
基于Cortex-M4与步进电机下两轮自平衡小车的移动控制
http://www.amobbs.com/thread-5531409-1-1.html
同感，我也觉得步进电机跑起来真的很慢啊。 第一次见到有人用步进电机做平衡车 本帖最后由 ANDOON 于 2013-9-2 13:07 编辑
/*****************====================================********************/
//**************** 一阶互补滤波+ PID+ MPU6050********************/
//************************************************************************//
#include <Wire.h>
#include <i2cdev.h>
#include <MPU6050.h>
//*********************************************************/
//*********************************************************/
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
//********************************************************/
//*************************MOTOR**************************/
// Define two steppers and the pins they will use
int dirPin_1 = 8;
int stepperPin_1 = 9;
int dirPin_2 = 6;
int stepperPin_2 = 7;
#define Gry_offset 296//陀螺仪X轴的静态飘移。
#define Gyr_Gain -0.00763//由陀螺仪X轴读数转换为角速度值，=1/131 敏感比例因子131、65.5、32.8、16.4 LSB/s，向前方向与X轴为反方向，故乘以-1
#define ACC_Gain 0.000061//由读数转换为加速度值，=1/16384Sensitivity Scale Factor 16384、8192、4096、2048 LSB/g
#define pi 3.1415926
#define K 0.715//一阶互补滤波权重取值，不知由来，可更改。
/********** 互补滤波器参数 *********/
unsigned long now ;
unsigned long preTime = 0;
float angleG = 0;
/*********** PID控制器参数 *********/
unsigned long lastTime = 0; // 前次时间
float ITerm = 0.0, lastInput = 0.0; // 积分项、前次输入
float Output = 0.0; // PID输出值
float LOutput,ROutput;
float LSpeed_Need=0.0,RSpeed_Need=0.0;// 转弯设置
void setup(){
Wire.begin();
Serial.begin(38400);
accelgyro.initialize();//MPU6050初始化。
pinMode(dirPin_1, OUTPUT);
pinMode(stepperPin_1, OUTPUT);
pinMode(dirPin_2, OUTPUT);
pinMode(stepperPin_2, OUTPUT);
}
void loop(){
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取六轴数值
float Y_Accelerometer = ay * ACC_Gain;//转换为向前的加速度(g)，为负值
float Z_Accelerometer = az * ACC_Gain;//转换为向下的加速度(g)
float angleA= atan(Y_Accelerometer/ Z_Accelerometer)* (-180)/ pi;//加速度仪，反正切获得弧度值，乘以180度/pi
//获得角度值，乘以-1得正值
now = millis();//当前时间(ms)
float dt = (now - preTime) / 1000.0;//微分时间(s)
preTime = now;//上一次采样时间(ms)
float gx_revised = gx + Gry_offset;//陀螺仪x轴静态时修正后的角速度读数，向前为负值
float omega= Gyr_Gain* gx_revised;//陀螺仪，转换为向前的角速度(o/s)，Gyr_Gain取负，故负负得正
float angle_dt = omega * dt;//角度的单次积分值
//angleG+= angle_dt;//陀螺仪，积分获得角度值
angleG+= (Gyr_Gain * (gx + Gry_offset)) * dt;
float A= K/ (K+ dt);//陀螺仪的权值
angleG= A* (angleG+ omega* dt)+ (1-A)* angleA;//一阶互补滤波后的输出角度(o)
//********************************************************************
//*********************************************************************
if (abs(angleG)<45) { // 角度小于45度 运行程序
PIDD();
PWMB();
}
Serial.print(angleG); Serial.print("\t");
Serial.println(Output);
}
//*********************************************************************/
//********************************************************************/
/**********=================PID控制器====================*************/
//********************************************************************/
void PIDD(){
now = millis(); // 当前时间(ms)
float TimeChange = (now - lastTime) / 1000.0; // 采样时间(s)
float Kp = 10.0, Ki = 0.23, Kd = 0.0; // 比例系数、积分系数、微分系数
float SampleTime = 0.1; // 采样时间(s)
float Setpoint =-2.0; // 设定目标值(degree)
//float outMin = -400.0, outMax = +400.0; // 输出上限、输出下限
if(TimeChange >= SampleTime) { // 到达预定采样时间时
float Input = angleG; // 输入赋值
float error = Setpoint - Input; // 偏差值
ITerm+= (Ki * error * TimeChange); // 计算积分项
// ITerm = constrain(ITerm, outMin, outMax); // 限定值域
float DTerm = Kd * (Input - lastInput) / TimeChange; // 计算微分项
Output = Kp * error + ITerm + DTerm; // 计算输出值
// Output = constrain(Output, outMin, outMax); // 限定值域
LOutput=Output+RSpeed_Need; //左电机
ROutput=Output-RSpeed_Need; //右电机
lastInput = Input; // 记录输入值
lastTime = now; // 记录本次时间
}
}
//**********************PWM调速输出******************************/
//***************************************************************/
void PWMB(){
if(Output > 0){
step_forwards(Output);
}
if (Output < 0){
step_backwards(abs(Output));
}
}
//**********************电机运转控制******************************/
//***************************************************************/
void step_forwards(int speeds){
digitalWrite(dirPin_1,0);
digitalWrite(dirPin_2,1);
digitalWrite(stepperPin_1, HIGH);
digitalWrite(stepperPin_2, HIGH);
delayMicroseconds(speeds);
digitalWrite(stepperPin_1, LOW);
digitalWrite(stepperPin_2, LOW);
delayMicroseconds(speeds);
}
void step_backwards(int speeds){
digitalWrite(dirPin_1,1);
digitalWrite(dirPin_2,0);
digitalWrite(stepperPin_1, HIGH);
digitalWrite(stepperPin_2, HIGH);
delayMicroseconds(speeds);
digitalWrite(stepperPin_1, LOW);
digitalWrite(stepperPin_2, LOW);
delayMicroseconds(speeds);
}最终能够稳定站立的程序，使用LV8731驱动步进电机，L293输出电流太小，容易热。
dalywan 发表于 2013-9-1 22:40 static/image/common/back.gif
同感，我也觉得步进电机跑起来真的很慢啊。
基于Cortex-M4与步进电机下两轮自平衡小车的移动控制
http://www.amobbs.com/thread-5531409-1-1.html tangmao48 发表于 2013-9-1 23:16 static/image/common/back.gif
第一次见到有人用步进电机做平衡车
基于Cortex-M4与步进电机下两轮自平衡小车的移动控制
http://www.amobbs.com/thread-5531409-1-1.html
页:
[1]
2