ARDUINO自平衡小车 手机蓝牙控制
本帖最后由 阿布都 于 2015-1-15 19:36 编辑ARDUINO自平衡小车 手机蓝牙控制 视频等审核通过了以后再发布 本帖最后由 阿布都 于 2015-1-18 15:21 编辑
//原始6050数据+双测速码盘
#include "Wire.h" //串口
#include "I2Cdev.h" //IIC总线
#include "MPU6050.h" //加速度与陀螺传感器
//自定义变量
char val='z'; //调节与控制命令字
int Speed_need = 0, Turn_need = 0; //运动速度,转弯速度
float speeds, speeds_filter, positions,Q; //速度,速度滤波,位置
float diff_speeds, dspeeds = 0, dspeeds_all = 0; //速度差
int text_time = 0, spcount = 0, dspcount = 0; //测试时间,速度测量次数
//PID参数
double Output = 0; //PID输出
float Kp=12, Kd=0.11, Ksp = 0.10 ,Ksi = 0.05, Kdsp = 5; //PID角度环、速度环系数
// MPU6050参数
MPU6050 accelgyro;
int16_t ax, ay, az, gx, gy, gz;
//角度参数
float Gyro_y; //Y轴陀螺仪数据暂存
float Angle_ax; //由加速度计算的倾斜角度
float Angle; //一阶互补滤波计算出的小车最终倾斜角度
float Angle0 = 2.00; //机械平衡角
//引脚分配
int PinA_right= 2; //interrupt 0
int PinA_left= 3; //interrupt 1
int E_left =5;//ENA
int M_left =4;
int M_right =7;
int E_right =6; //ENB
/*int PinA_left = 2; //中断0
int PinA_right = 3; //中断1
int M_left = 6;
int M_left2 = 7;
int E_left = 10; //ENA
int M_right = 8;
int M_right2= 9;
int E_right = 11; //ENB*/
//电机输出
int PWM_right = 0, PWM_left = 0;
int PWM_left_least = 35, PWM_right_least = 40; //左右电机启动补偿50
//测速码盘中断
int count_right = 0;
int count_left = 0;
void Code_left() {
if(Output>=0) {
count_left ++;
}else {
count_left --;
}
} //左测速码盘计数
void Code_right() {
if(Output>=0) {
count_right ++;
}else {
count_right --;
}
} //右测速码盘计数
//电机输出
void SetMotorVoltage(int nLeftVol, int nRightVol) {
if(nLeftVol >=0)
{digitalWrite(M_left,HIGH);
} else {
digitalWrite(M_left,LOW);
nLeftVol=-nLeftVol;
}
if(nRightVol >= 0) {
digitalWrite(M_right,HIGH);
} else {
digitalWrite(M_right,LOW);
nRightVol=-nRightVol;
}
if(nLeftVol>255) { nLeftVol = 255 ; } //防止PWM值超过255
if(nRightVol>255) { nRightVol = 255 ; }//防止PWM值超过255
analogWrite(E_left,nLeftVol);
analogWrite(E_right,nRightVol);
}
/*void SetMotorVoltage(int nLeftVol, int nRightVol) {
if(nLeftVol >=0) {
digitalWrite(M_left, LOW);
digitalWrite(M_left2, HIGH);
}else {
digitalWrite(M_left, HIGH);
digitalWrite(M_left2, LOW);
nLeftVol = -nLeftVol;
}
if(nRightVol >= 0) {
digitalWrite(M_right, LOW);
digitalWrite(M_right2, HIGH);
}else {
digitalWrite(M_right, HIGH);
digitalWrite(M_right2, LOW);
nRightVol = -nRightVol;
}
if(nLeftVol > 255) nLeftVol = 255; //防止PWM值超过255
if(nRightVol > 255) nRightVol = 255; //防止PWM值超过255
analogWrite(E_left, nLeftVol);
analogWrite(E_right, nRightVol);
}*/
//计算小车角度
void Angle_Calcu(void) {
Angle_ax = (ax+200)/200 ; //去除零点偏移1942,16384*3.14/1.2*180//弧度转换为度,
Gyro_y = -(gy - 10.58)/16.4; //去除零点偏移119,2000deg/s 16.4 LSB/(deg/s)250---131
Angle = 0.97 * (Angle + Gyro_y * 0.0005) + 0.03 * Angle_ax;
}
void setup() {
Wire.begin();
Serial.begin(9600);
accelgyro.initialize(); //初始化设备
//引脚模式设置
pinMode(E_right, OUTPUT); pinMode(M_right, OUTPUT);
pinMode(E_left, OUTPUT);pinMode(M_left, OUTPUT);
pinMode(PinA_right,INPUT);pinMode(PinA_left,INPUT);//
/*pinMode(E_left, OUTPUT);
pinMode(M_left, OUTPUT);
pinMode(M_left2, OUTPUT); //左电机
pinMode(E_right, OUTPUT);
pinMode(M_right, OUTPUT);
pinMode(M_right2, OUTPUT); //右电机
pinMode(PinA_right, INPUT);
pinMode(PinA_left, INPUT); //测速码盘输入*/
//中断设置
attachInterrupt(0, Code_right, RISING);
attachInterrupt(1, Code_left, RISING);
}
void loop() {
if (Serial.available() > 0) {
val = Serial.read();
//参数调节
if(val == 'X') Kp += 0.1;
if(val == 'x') Kp -= 0.1;
if(val == 'V') Kd += 0.01;
if(val == 'v') Kd -= 0.01;
if(val == 'N') Ksp += 0.1;
if(val == 'n') Ksp -= 0.1;
if(val == 'M') Ksi += 0.01;
if(val == 'm') Ksi -= 0.01;
if(val == 'L') Angle0 += 0.1;
if(val == 'l') Angle0 -= 0.1;
if(val == 'P') PWM_left_least += 1;
if(val == 'p') PWM_left_least -= 1;
if(val == 'G') PWM_right_least += 1;
if(val == 'g') PWM_right_least -= 1;
//参数查看
if(val == 'H') {
Serial.print(ax); Serial.print("\t"); //用于测量零点偏移
Serial.println(gy); //用于测量零点偏移
}
if(val == 'I') {
Serial.print(Angle0); Serial.print("\t");
Serial.print(PWM_left_least); Serial.print("\t");
Serial.print(PWM_right_least); Serial.print("\t");
Serial.print(Kp); Serial.print("\t");
Serial.print(Kd); Serial.print("\t");
Serial.print(Ksp); Serial.print("\t");
Serial.print(Ksi); Serial.print("\t");
Serial.print(Q); Serial.print("\t");
Serial.println(Kdsp);
}
if(val == 'J') {
Serial.print(Angle); Serial.print("\t");
Serial.println(dspeeds_all);
}
if(val == 'K') Kdsp += 1;
if(val == 'k') Kdsp -= 1;
//小车控制
if(val == 'A') {
Speed_need = 1500; //前进
Turn_need = 0;
}
if(val == 'B') {
Speed_need = -1500;
00; //后退
Turn_need = 0;
}
if(val == 'C') {
Speed_need = 0;
Turn_need = 20; //左转
}
if(val == 'D') {
Speed_need = 0;
Turn_need = -20; //右转
}
if(val == 'E')
{
Speed_need = 0; //停止
Turn_need = 0;
}
if(val == 'F')
{
Speed_need = 0; //停止
Turn_need = 0;
}
}
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //获取传感器原始值
Angle_Calcu(); //计算角度
PWM_Calcu(); //PWM输出计算
spcount ++;
if(spcount > 40)
{
spcount = 0;
dspcount ++;
//速度与速度积分
speeds = (count_left + count_right)*0.5;
diff_speeds = count_left - count_right;
dspeeds += diff_speeds;
if(dspcount > 80) {
dspeeds_all = dspeeds;
dspeeds = 0;
}
speeds_filter *= 0.85; //一阶互补滤波
speeds_filter += speeds*0.15;
positions += speeds_filter;
positions = constrain(positions, -300, 300); //抗积分饱和
count_left = 0;
count_right = 0;
}
}
void PWM_Calcu(void)
{
if (abs(Angle) > 45)
{
SetMotorVoltage(0,0); //角度大于30度 停止PWM输出
}
else
{
//PWM输出计算
Q= Ksi*positions;
Output = Kp*(Angle - Angle0) + Kd*Gyro_y + Ksp*(speeds_filter -Speed_need) + Q;
if(Turn_need == 0)
{
PWM_left = Output - Kdsp * dspeeds_all;
PWM_right = Output + Kdsp * dspeeds_all;
}
PWM_left = Output - Turn_need;
PWM_right = Output + Turn_need;
if(PWM_left >= 0)
{
PWM_left += PWM_left_least;
}
else
{
PWM_left -= PWM_left_least;
}
if(PWM_right >= 0)
{
PWM_right += PWM_right_least;
}
else
{
PWM_right -= PWM_right_least;
}
SetMotorVoltage(PWM_left, PWM_right);
}
}
欢迎大家交流交流 http://www.geek-workshop.com/thread-11629-1-1.html一只参考这个帖子,楼主特别好,很耐心的一一解答我的各种问题,终于成功的做出来了,在此感谢这位楼主。。。 轮胎很霸气
本帖最后由 阿布都 于 2015-1-15 22:36 编辑
http://v.youku.com/v_show/id_XODcyNjc2OTY0.html569620972 发表于 2015-1-15 21:08 static/image/common/back.gif
轮胎很霸气
因为电机转速低所以采用了大一点的轮子:) 视频也放出来了 嘿嘿 恭喜樓主, 看來很順暢呢.
大家都順利完成, 我的連站也不行, 是我自己太懶了.{:soso_e149:}
樓主的車子, 有用測速的嗎? 换成步进电机试试呀?
:D http://v.youku.com/v_show/id_XODcyODM2MDQw.html
昨天找到以前买的香蕉电机,胶带粘一下,再弄了个。。。不用测速的情况下,也还可以。。。。就是电机质量差。。。转速差异。。。。。哎 阿布都 发表于 2015-1-15 21:50 static/image/common/back.gif
因为电机转速低所以采用了大一点的轮子
看了你的视频感觉真棒:victory:,看来香蕉电机的确要差一截子。 看起来很不平稳 wujingyu 发表于 2015-1-16 10:21 static/image/common/back.gif
看了你的视频感觉真棒,看来香蕉电机的确要差一截子。
我的电机还得换,转速比较低,现在电机快坏了 Super169 发表于 2015-1-16 00:53 static/image/common/back.gif
恭喜樓主, 看來很順暢呢.
大家都順利完成, 我的連站也不行, 是我自己太懶了.
樓主的車子, ...
嘿嘿 ,坚持就是胜利,只要你坚持下去就会成功的,恩恩 用了测速编码器,不用的话,控制的时候会越走越快,最终会到下,我发现论坛利好人很多,遇到难题都愿意解答 1090805647 发表于 2015-1-16 11:05 static/image/common/back.gif
看起来很不平稳
恩恩 ,电机快坏了,等换了好的电机后对参数重新调整 落点人 发表于 2015-1-16 09:43 static/image/common/back.gif
昨天找到以前买的香蕉电机,胶带粘一下,再弄了个。。。不用测速的情况下,也还可以。。。。就是电机质 ...
我的车稳定性还是得调试,等我换了好点的电机厚,对参数重新调整