极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 73886|回复: 70

ARDUINO自平衡小车 手机蓝牙控制

[复制链接]
发表于 2015-1-15 19:33:05 | 显示全部楼层 |阅读模式
本帖最后由 阿布都 于 2015-1-15 19:36 编辑

ARDUINO自平衡小车 手机蓝牙控制   视频等审核通过了以后再发布

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复

使用道具 举报

 楼主| 发表于 2015-1-15 19:42:56 | 显示全部楼层
本帖最后由 阿布都 于 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);
  }
}
[/code]欢迎大家交流交流
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-15 19:46:45 | 显示全部楼层
http://www.geek-workshop.com/thread-11629-1-1.html  一只参考这个帖子,楼主特别好,很耐心的一一解答我的各种问题,终于成功的做出来了,在此感谢这位楼主。。。
回复 支持 反对

使用道具 举报

发表于 2015-1-15 21:08:20 | 显示全部楼层
轮胎很霸气
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-15 21:50:26 | 显示全部楼层
本帖最后由 阿布都 于 2015-1-15 22:36 编辑

569620972 发表于 2015-1-15 21:08
轮胎很霸气


因为电机转速低所以采用了大一点的轮子
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-15 22:37:34 | 显示全部楼层
视频也放出来了 嘿嘿
回复 支持 反对

使用道具 举报

发表于 2015-1-16 00:53:46 | 显示全部楼层
恭喜樓主, 看來很順暢呢.  
大家都順利完成, 我的連站也不行, 是我自己太懶了.{:soso_e149:}
樓主的車子, 有用測速的嗎?
回复 支持 反对

使用道具 举报

发表于 2015-1-16 09:00:30 | 显示全部楼层
换成步进电机试试呀?
回复 支持 反对

使用道具 举报

发表于 2015-1-16 09:43:19 | 显示全部楼层


昨天找到以前买的香蕉电机,胶带粘一下,再弄了个。。。不用测速的情况下,也还可以。。。。就是电机质量差。。。转速差异。。。。。哎
回复 支持 反对

使用道具 举报

发表于 2015-1-16 10:21:17 | 显示全部楼层
阿布都 发表于 2015-1-15 21:50
因为电机转速低所以采用了大一点的轮子

看了你的视频感觉真棒,看来香蕉电机的确要差一截子。
回复 支持 反对

使用道具 举报

发表于 2015-1-16 11:05:21 | 显示全部楼层
看起来很不平稳
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-16 13:49:53 | 显示全部楼层
wujingyu 发表于 2015-1-16 10:21
看了你的视频感觉真棒,看来香蕉电机的确要差一截子。

我的电机还得换,转速比较低,现在电机快坏了
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-16 13:51:42 | 显示全部楼层
Super169 发表于 2015-1-16 00:53
恭喜樓主, 看來很順暢呢.  
大家都順利完成, 我的連站也不行, 是我自己太懶了.
樓主的車子, ...

嘿嘿 ,坚持就是胜利,只要你坚持下去就会成功的,恩恩 用了测速编码器,不用的话,控制的时候会越走越快,最终会到下,我发现论坛利好人很多,遇到难题都愿意解答
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-16 13:52:40 | 显示全部楼层
1090805647 发表于 2015-1-16 11:05
看起来很不平稳

恩恩 ,电机快坏了,等换了好的电机后对参数重新调整
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-16 13:55:06 | 显示全部楼层
落点人 发表于 2015-1-16 09:43
昨天找到以前买的香蕉电机,胶带粘一下,再弄了个。。。不用测速的情况下,也还可以。。。。就是电机质 ...

我的车稳定性还是得调试,等我换了好点的电机厚,对参数重新调整
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-19 14:23 , Processed in 0.046407 second(s), 24 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表