[项目]平衡小车

查看: 5808|回复: 10

[项目] 平衡小车

[复制链接]
一直对平衡车感兴趣,又找来一个平衡车的资料。以下内容主要转帖而来,原文http://www.geek-workshop.com/for ... &highlight=L298
小车整体造型大概是这个样子的
1.jpg

2.jpg

小车可以站住,但是小车抖动比较厉害。
使用的mpu6050,使用的一介互补滤波,得出 角度 angle_x 。虽然原文作者调节了参数,但据说效果还不是很理想。
3.png



然后通过 if 语句进行判断    if(angle_x>0)   控制L298n
接下来就是把 angle_x 进行PID 控制。 Input = angle_x;
  myPID.Compute();
  Serial.print(Output=);           通过PID计算得出 Output         疑惑的是 Output竟然是0
4.png

Output=0         这是什么原因呢????

然后把Output当做两个车轮的PWM脉冲,
  analogWrite(motor1PWMPin, Output);
  analogWrite(motor2PWMPin, Output);
Output=0 ,我测量motor1PWMPin motor2PWMPin两个管脚的电压还都是1.75,不管小车角度怎样,电压都不会改变。  ????

设定小车的静态平衡角度为  Setpoint = 10.0;     
初始   Input = 0.0;
  myPID.SetSampleTime(100);    //采样时间
  myPID.SetMode(AUTOMATIC);


PID的调节   PID myPID(&Input, &Output, &Setpoint, 1, 0, -10, DIRECT);

调了几天,没什么效果,真是郁闷。。。
又查了一些资料,可能是电机太水,但是也有看到别人用香蕉电机的,效果也还不错。

斗就斗把,后来加了码盘,用了槽型对射管,对两个轮子进行测速,蓝牙控制准备让小车行走,就使用Arduino uon 的两个外部中断计脉冲的个数,
标志==40的时候 车轮转动一圈,然后当接收到前进的命令的时候,让两个轮子全部正转2圈,但是经过测试 小车一直在保持平衡,所以没时间正转2圈(这样理解可以吧)。 所以 这个控制程序不会写了。。。欢迎大神可提供帮助哦,到时候我也可以去帮帮原作者,装一下大神,哈哈

代码
  1. #include<Wire.h>
  2. #include "gyro_accel.h"
  3. #include "PID_v1.h"

  4. // Defining constants
  5. #define dt 1            // time difference in milli seconds
  6. #define rad2degree 57.3  // radian to degree conversion
  7. #define Filter_gain 0.95  // e.g. angle = angle_gyro*Filter_gain + angle_accel*(1-Filter_gain)

  8. // Global Variables
  9. unsigned long t = 0;      // Time Variables
  10. float angle_x_gyro=0, angle_y_gyro=0, angle_z_gyro=0, angle_x_accel=0, angle_y_accel=0, angle_z_accel=0, angle_x=0, angle_y=0, angle_z=0;
  11. char val ;
  12. int maichong,lefta,leftaa,leftone,righta,rightaa,rightone;
  13. // DC motor driver with L298N
  14. const int motor1PWMPin = 10; // PWM Pin of Motor 1
  15. const int motor1Polarity1 = 9; // Polarity 1 Pin of Motor 1
  16. const int motor1Polarity2 = 8;  // Polarity 2 Pin of Motor 1
  17. const int motor2PWMPin = 5;  // PWM Pin of Motor 2
  18. const int motor2Polarity1 = 6; // Polarity 1 Pin of Motor 2
  19. const int motor2Polarity2 = 7;  // Polarity 2 Pin of Motor 2
  20. const int left=0; //码盘
  21. const int right=1;//码盘
  22. int ValM1 = 255;  // Initial Value for PWM Motor 1
  23. int ValM2 = 255;  // Initial Value for PWM Motor 2

  24. double Setpoint, Input, Output;

  25. PID myPID(&Input, &Output, &Setpoint, 1, 0, -10, DIRECT);
  26. //PID myPID(&Input, &Output, &Setpoint, 25, 4, -5.5, DIRECT);
  27. void setup()
  28. {
  29.   // MPU-6050
  30.   Serial.begin(115200);
  31.   Wire.begin();
  32.   MPU6050_ResetWake();
  33.   MPU6050_SetGains(0,1);  // Setting the lows scale
  34.   MPU6050_SetDLPF(0);    // Setting the DLPF to inf Bandwidth for calibration
  35.   MPU6050_OffsetCal();  // very important
  36.   MPU6050_SetDLPF(6);  // Setting the DLPF to lowest Bandwidth
  37.    
  38.   t = millis();
  39.    
  40.   // DC motor
  41.   pinMode(motor1PWMPin, OUTPUT);
  42.   pinMode(motor1Polarity1, OUTPUT);
  43.   pinMode(motor1Polarity2, OUTPUT);
  44.    
  45.   pinMode(motor2PWMPin, OUTPUT);
  46.   pinMode(motor2Polarity1, OUTPUT);
  47.   pinMode(motor2Polarity2, OUTPUT);
  48.   //0 1 zhongduan
  49. attachInterrupt(left, jishu1, CHANGE);
  50. attachInterrupt(right, jishu, CHANGE);
  51.   
  52.    
  53.   // set enablePin of motor 1 high so that motor 1 can turn on
  54.   digitalWrite(motor1PWMPin, HIGH);
  55.   digitalWrite(motor1Polarity1, HIGH);
  56.   digitalWrite(motor1Polarity2, LOW);

  57.   // set enablePin of motor 2 high so that motor 2 can turn on  
  58.   digitalWrite(motor2PWMPin, HIGH);
  59.   digitalWrite(motor2Polarity1, HIGH);
  60.   digitalWrite(motor2Polarity2, LOW);
  61.    
  62.   Input = 0.0;
  63.   Setpoint = 10.0;
  64.   
  65.   myPID.SetSampleTime(100);
  66.   myPID.SetMode(AUTOMATIC);
  67.   
  68. }
  69. void jishu()
  70. {  
  71.     lefta++;
  72.     if(lefta==45)
  73.     {  
  74.       lefta=0;  //mai chong biao zhi qing ling
  75.       leftone++;//一圈
  76.     }  
  77. }
  78. void jishu1()
  79. {  
  80.     righta++;
  81.     if(righta==45)
  82.     {  
  83.       righta=0;  //mai chong biao zhi qing ling
  84.       rightone++;//一圈
  85.     }  
  86. }
  87. void rightz()//右轮正转
  88. {
  89.   digitalWrite(motor1PWMPin, HIGH);digitalWrite(motor1Polarity1, HIGH);digitalWrite(motor1Polarity2, LOW);
  90. }
  91. void rightf()//右轮反转
  92. {
  93.   digitalWrite(motor1PWMPin, HIGH);digitalWrite(motor1Polarity1, LOW);digitalWrite(motor1Polarity2, HIGH);
  94. }
  95. void leftz()
  96. {
  97.   digitalWrite(motor2PWMPin, HIGH);digitalWrite(motor2Polarity1, HIGH);digitalWrite(motor2Polarity2, LOW);
  98. }
  99. void leftf()
  100. {
  101.   digitalWrite(motor2PWMPin, HIGH);digitalWrite(motor2Polarity1, LOW);digitalWrite(motor2Polarity2, HIGH);
  102. }
  103. void st()//电机停
  104. {
  105.   digitalWrite(motor1PWMPin, HIGH);digitalWrite(motor1Polarity1, LOW);digitalWrite(motor1Polarity2, LOW);
  106.   digitalWrite(motor2PWMPin, HIGH);digitalWrite(motor2Polarity1, LOW);digitalWrite(motor2Polarity2, LOW);
  107. }
  108. void blue()//蓝牙
  109. {
  110.   if (Serial.available() > 0) {
  111.       val = Serial.read();
  112.      /* if(val == 'A') {
  113.         Serial.println(angle_x); //jiaodu
  114.       }*/
  115.       if(val == 'z') {
  116.        rightz();
  117.       }
  118.       if(val == 'x') {
  119.       rightf();
  120.       }
  121.       if(val == 'c') {
  122.        leftz();
  123.       }
  124.       if(val == 'v') {
  125.        leftf();
  126.       }
  127.       if(val == 'b') {
  128.        st();
  129.       }
  130. }}
  131. void loop()
  132. {
  133.   t = millis();
  134.   
  135.   MPU6050_ReadData();
  136.    
  137.   angle_x_gyro = (gyro_x_scalled*((float)dt/1000)+angle_x);
  138.   angle_y_gyro = (gyro_y_scalled*((float)dt/1000)+angle_y);
  139.   angle_z_gyro = (gyro_z_scalled*((float)dt/1000)+angle_z);

  140.   angle_z_accel = atan(accel_z_scalled/(sqrt(accel_y_scalled*accel_y_scalled+accel_x_scalled*accel_x_scalled)))*(float)rad2degree;
  141.   angle_y_accel = -atan(accel_x_scalled/(sqrt(accel_y_scalled*accel_y_scalled+accel_z_scalled*accel_z_scalled)))*(float)rad2degree;
  142.   angle_x_accel = atan(accel_y_scalled/(sqrt(accel_x_scalled*accel_x_scalled+accel_z_scalled*accel_z_scalled)))*(float)rad2degree;

  143.   angle_x = Filter_gain*angle_x_gyro+(1-Filter_gain)*angle_x_accel;
  144.   angle_y = Filter_gain*angle_y_gyro+(1-Filter_gain)*angle_y_accel;
  145.   angle_z = Filter_gain*angle_z_gyro+(1-Filter_gain)*angle_z_accel;
  146.    
  147.   digitalWrite(motor1PWMPin, HIGH);
  148.   digitalWrite(motor2PWMPin, HIGH);

  149. Serial.print("\n");Serial.print("    ");Serial.print("angle_x=");  Serial.print(angle_x);Serial.print("  ");Serial.print("angle_y=");Serial.print(angle_y);Serial.print("  ");Serial.print("angle_z=");Serial.print(angle_z);
  150. Serial.print("____");           //串口输出
  151.    
  152.   // change direction is very important   平衡
  153.   if(angle_x>0)
  154.   {
  155.     myPID.SetControllerDirection(REVERSE);
  156.      
  157.     // set enablePin of motor 1 high so that motor 1 can turn on
  158.     digitalWrite(motor1Polarity1, HIGH);
  159.     digitalWrite(motor1Polarity2, LOW);

  160.     // set enablePin of motor 2 high so that motor 2 can turn on  
  161.     digitalWrite(motor2Polarity1, HIGH);
  162.     digitalWrite(motor2Polarity2, LOW);
  163.   }
  164.   else
  165.   {
  166.     myPID.SetControllerDirection(DIRECT);
  167.     // set enablePin of motor 1 high so that motor 1 can turn on
  168.     digitalWrite(motor1Polarity1, LOW);
  169.     digitalWrite(motor1Polarity2, HIGH);

  170.     // set enablePin of motor 2 high so that motor 2 can turn on  
  171.     digitalWrite(motor2Polarity1, LOW);
  172.     digitalWrite(motor2Polarity2, HIGH);
  173.   }
  174.    
  175.   Input = angle_x;
  176.   Serial.print("Input=");Serial.print(Input);Serial.print("____");
  177.    
  178.   myPID.Compute();
  179.   Serial.print("Output=");Serial.print(Output);Serial.print("____");                        Output 怎么会是0????
  180.    
  181.   analogWrite(motor1PWMPin, Output);
  182.   analogWrite(motor2PWMPin, Output);
  183.   
  184.   Serial.print("Setpoint=");Serial.print(Setpoint); Serial.print("\t");
  185.   Serial.print("left=");Serial.print(lefta);Serial.print("  ");Serial.print(leftone); Serial.print("\t");
  186.   Serial.print("right=");Serial.print(righta);Serial.print("  ");Serial.print(rightone); Serial.print("\t");   //串口输出
  187.    blue();//蓝牙
  188.    
  189.    if(rightone==1) //转动1圈标志
  190.        {
  191.          rightone=0;   
  192.          st();   // 停止转动
  193.        }
  194.       
  195.    
  196.   while((millis()-t) < dt)
  197.   {
  198.     // Do nothing
  199.   }
复制代码

5.jpg


virtualwiz  中级技匠

发表于 2015-12-7 17:00:38

沙发沙发~

曾经想做个超级超级小的平衡车来着:lol学习一下
回复

使用道具 举报

丄帝De咗臂  高级技匠
 楼主|

发表于 2015-12-7 18:14:02

virtualwiz 发表于 2015-12-7 17:00
沙发沙发~

曾经想做个超级超级小的平衡车来着学习一下

超级超级小,到底是多小
回复

使用道具 举报

大连林海  初级技神

发表于 2015-12-7 19:38:11

张禄  你东西全了 也做一个
回复

使用道具 举报

丄帝De咗臂  高级技匠
 楼主|

发表于 2015-12-7 19:45:16

大连林海 发表于 2015-12-7 19:38
张禄  你东西全了 也做一个

多谢鼓励,哈哈,可是我始终战胜不了我的惰性
回复

使用道具 举报

dsweiliang  初级技神

发表于 2015-12-7 19:45:45

做个大型的,能载人的
回复

使用道具 举报

大连林海  初级技神

发表于 2015-12-7 19:55:53

丄帝De咗臂 发表于 2015-12-7 19:45
多谢鼓励,哈哈,可是我始终战胜不了我的惰性

我的懒性也越来越大
回复

使用道具 举报

大连林海  初级技神

发表于 2015-12-7 19:56:06

dsweiliang 发表于 2015-12-7 19:45
做个大型的,能载人的

你得赞助张禄 哈哈
回复

使用道具 举报

Geemi  初级技匠

发表于 2015-12-18 17:46:44

这个东西有点赞呀!!!!!
回复

使用道具 举报

20060606  高级技匠

发表于 2020-9-4 06:06:43

好创意赞一个
回复

使用道具 举报

DFBJFz5siXP  见习技师

发表于 2020-9-23 21:37:25

厉害啊 我到现在还是搞不明白PDI
回复

使用道具 举报

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

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
[[wsData.name]]

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
上海智位机器人股份有限公司 沪ICP备09038501号-4

© 2013-2020 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail