平衡小车站起来
本帖最后由 tanbocandy 于 2013-8-5 00:17 编辑在这里感谢 弘毅 ，pww999，Randy，arduino官网，基本是看他们的帖子学习的，经过大半个月的理论学习，实验，加上一个星期的制作过程（主要是做架子，调试），我的小车算是能站立，但是有个问题：我用蓝牙控制小车前后（还不带原地旋转）移动，小车老是要倒，程序是参照PWW999的，在接受到命令后，改变平衡的角度，不知道是程序原因还是硬件问题（电机是12V，8KG，100转），程序在后面发出来，全部是用的库，所以看起来还是比较简单的，如果看了我前面两个帖子（MPU6050卡尔曼，和电机PID）的内容都可以看懂这个程序，我的小车平衡角度是180。
还是哪个问题，小车不能前后移动，一动就倒。
http://v.youku.com/v_show/id_XNTkxNzA2MDI0.html
#include <PID_v1.h>
#include "Wire.h"
#include"Kalman.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#define Left1 10
#define Left2 11
#define Right1 5
#define Right2 6
#define drift 20
double Setpoint, Input, Output;
double temp_kp,temp_ki,temp_kd;
//Kp=0.1,Ki=0.05,Kd=0,change depend on your system
PID myPID(&Input,&Output,&Setpoint,1,0,0,DIRECT);
MPU6050 accelgyro;
Kalman kalmanX; // Create the Kalman instances
//Kalman kalmanY;
//Kalman kalmanZ;
int16_t ax, ay, az;
int16_t gx, gy, gz;
uint32_t timer;
int bluetooth_read;
int Lspeed_need,Rspeed_need;
double accXangle, accYangle,accZangle; // Angle calculate using the accelerometer
double gyroXangle, gyroYangle,gyroZangle; // Angle calculate using the gyro
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
void setup()
{
pinMode(Left1,OUTPUT);
pinMode(Left2,OUTPUT);
pinMode(Right1,OUTPUT);
pinMode(Right2,OUTPUT);
car_stop();
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// initialize serial communication
// (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
// it's really up to you depending on your project)
Serial.begin(9600);
// initialize device
accelgyro.initialize();
// verify connection
accelgyro.testConnection();
timer=micros();
Setpoint=180.00;
myPID.SetSampleTime(10);
myPID.SetMode(AUTOMATIC);
}
void loop()
{
// read raw accel/gyro measurements from device
data_recv();
temp_kp=analogRead(A0)*0.15;
temp_ki=analogRead(A1)*0.0002;
temp_kd=analogRead(A2)*0.08;
myPID.SetTunings(temp_kp,temp_ki,temp_kd);
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
accXangle = (atan2(ay,az)+PI)*RAD_TO_DEG;
// accYangle = (atan2(ax,az)+PI)*RAD_TO_DEG;
double gyroXrate = (double)gx/131.0;
//double gyroYrate =-((double)gy/131.0);
gyroXangle+= gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter
// gyroYangle+= gyroYrate*((double)(micros()-timer)/1000000);
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
// kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
timer = micros();
if((kalAngleX>(Setpoint-drift))&&(kalAngleX<(Setpoint+drift)))
{
if((kalAngleX>Setpoint)&&(kalAngleX<(Setpoint+drift)))
{
myPID.SetControllerDirection(REVERSE);
Input=kalAngleX;
myPID.Compute();
analogWrite(Left2,Output); //PWM out put
analogWrite(Right2,Output); //PWM out put
analogWrite(Left1,0); //PWM out put
analogWrite(Right1,0); //PWM out put
}
if((kalAngleX<Setpoint)&&(kalAngleX>(Setpoint-drift)))
{
myPID.SetControllerDirection(DIRECT);
Input=kalAngleX;
myPID.Compute();
analogWrite(Left1,min(Output+Lspeed_need,255)); //PWM out put
analogWrite(Right1,min(Output+Lspeed_need,255)); //PWM out put
analogWrite(Left2,0); //PWM out put
analogWrite(Right2,0); //PWM out put
}
}
else
{
car_stop();
}
// Serial.print(accXangle);Serial.print(",");
// Serial.print(gyroXangle);Serial.print(",");
Serial.println(kalAngleX);
Serial.println(temp_kp);
Serial.println(temp_ki);
Serial.println(temp_kd);
//Serial.print("\t");
//Serial.print(accYangle);Serial.print(",");
//Serial.print(gyroYangle);Serial.print(",");
//Serial.print(kalAngleY);Serial.print("\r\n");
}
void car_stop()
{
analogWrite(Left1,0); //PWM out put
analogWrite(Right1,0); //PWM out put
analogWrite(Left2,0); //PWM out put
analogWrite(Right2,0);
}
void data_recv()
{
if(Serial.available()>0)
{
bluetooth_read=Serial.read();
switch (bluetooth_read)
{
case 0x11:
Setpoint=183.00;
break;
case 0x22:
Setpoint=177.00;
break;
/* case 33:
Lspeed_need=10;
Rspeed_need=-10;
break;
case 44:
Lspeed_need=-10;
Rspeed_need=10;
break; */
}
}
else
{
Setpoint=180.00;
}
}
本帖最后由 pww999 于 2013-8-5 02:14 编辑
效果不错嘛，改变前后角度数值大小，太大会一下就倒，太小又只会点下头！(你电池放在上面，头重脚轻了吧)
如果不知道大概前后数值，可以增加一电位器，从小调节(或者*0.1,,,0.01缩小调节范围) 做的挺好，重心很低。学习了。 pww999 发表于 2013-8-5 02:06 static/image/common/back.gif
效果不错嘛，改变前后角度数值大小，太大会一下就倒，太小又只会点下头！(你电池放在上面，头重脚轻了吧)
...
我看一个国外的网站上说重量在高处会更容易保持平衡，就像人把扫帚倒立起来一样，所以我把电池放在上面的。。。难道被国外的坑了。。。:( 觉得重力在下面好吧，象不倒翕？ 是用的6050么？？？感觉比之前平稳许多，不知道跑起来效果会怎样？？？ 学慧放弃 发表于 2013-8-5 14:16 static/image/common/back.gif
是用的6050么？？？感觉比之前平稳许多，不知道跑起来效果会怎样？？？
就是用的6050我用的是PI控制，但是我看其他好多人都是用的PD，就是跑不起来，跑了就到，PWW999说是前进和后退的角度没调对，我还没试出来。 pww999 发表于 2013-8-5 13:44 static/image/common/back.gif
觉得重力在下面好吧，象不倒翕？
这个也是我郁闷的地方啊，到底是不倒翁还是像扫帚那样。。。。 tanbocandy 发表于 2013-8-5 17:07 static/image/common/back.gif
就是用的6050我用的是PI控制，但是我看其他好多人都是用的PD，就是跑不起来，跑了就到，PWW999说是前进 ...
恩恩，加油，但是国外两轮平衡车都可以站人呢，貌似也是PI控制的 很不错,我什么时候能做一个呢... 不错额，我在用STM32做 你用的是什么样的arduino 主板
楼主的kalman.h哪里来的 分享下调试过程吧。 很好啊，，，坐等楼主让它跑起来
页:
[1]
2