平衡小车 DF创客社区 - Powered by Discuz! Archiver

丄帝De咗臂 发表于 2015-12-7 15:07:58

平衡小车

一直对平衡车感兴趣，又找来一个平衡车的资料。以下内容主要转帖而来，原文http://www.geek-workshop.com/forum.php?mod=viewthread&tid=25267&highlight=L298
小车整体造型大概是这个样子的

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

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

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圈（这样理解可以吧）。所以这个控制程序不会写了。。。欢迎大神可提供帮助哦，到时候我也可以去帮帮原作者，装一下大神，哈哈{:5_195:}

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

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

// Global Variables
unsigned long t = 0;    // Time Variables
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;
char val ;
int maichong,lefta,leftaa,leftone,righta,rightaa,rightone;
// DC motor driver with L298N
const int motor1PWMPin = 10; // PWM Pin of Motor 1
const int motor1Polarity1 = 9; // Polarity 1 Pin of Motor 1
const int motor1Polarity2 = 8;// Polarity 2 Pin of Motor 1
const int motor2PWMPin = 5;// PWM Pin of Motor 2
const int motor2Polarity1 = 6; // Polarity 1 Pin of Motor 2
const int motor2Polarity2 = 7;// Polarity 2 Pin of Motor 2
const int left=0; //码盘
const int right=1;//码盘
int ValM1 = 255;// Initial Value for PWM Motor 1
int ValM2 = 255;// Initial Value for PWM Motor 2

double Setpoint, Input, Output;

PID myPID(&Input, &Output, &Setpoint, 1, 0, -10, DIRECT);
//PID myPID(&Input, &Output, &Setpoint, 25, 4, -5.5, DIRECT);
void setup()
{
// MPU-6050
Serial.begin(115200);
Wire.begin();
MPU6050_ResetWake();
MPU6050_SetGains(0,1);// Setting the lows scale
MPU6050_SetDLPF(0); // Setting the DLPF to inf Bandwidth for calibration
MPU6050_OffsetCal();// very important
MPU6050_SetDLPF(6);// Setting the DLPF to lowest Bandwidth

t = millis();

// DC motor
pinMode(motor1PWMPin, OUTPUT);
pinMode(motor1Polarity1, OUTPUT);
pinMode(motor1Polarity2, OUTPUT);

pinMode(motor2PWMPin, OUTPUT);
pinMode(motor2Polarity1, OUTPUT);
pinMode(motor2Polarity2, OUTPUT);
//0 1 zhongduan
attachInterrupt(left, jishu1, CHANGE);
attachInterrupt(right, jishu, CHANGE);

// set enablePin of motor 1 high so that motor 1 can turn on
digitalWrite(motor1PWMPin, HIGH);
digitalWrite(motor1Polarity1, HIGH);
digitalWrite(motor1Polarity2, LOW);

// set enablePin of motor 2 high so that motor 2 can turn on
digitalWrite(motor2PWMPin, HIGH);
digitalWrite(motor2Polarity1, HIGH);
digitalWrite(motor2Polarity2, LOW);

Input = 0.0;
Setpoint = 10.0;

myPID.SetSampleTime(100）;
myPID.SetMode(AUTOMATIC);

}
void jishu()
{
lefta++;
if(lefta==45)
{
   lefta=0;//mai chong biao zhi qing ling
   leftone++;//一圈
}
}
void jishu1()
{
righta++;
if(righta==45)
{
   righta=0;//mai chong biao zhi qing ling
   rightone++;//一圈
}
}
void rightz()//右轮正转
{
digitalWrite(motor1PWMPin, HIGH);digitalWrite(motor1Polarity1, HIGH);digitalWrite(motor1Polarity2, LOW);
}
void rightf()//右轮反转
{
digitalWrite(motor1PWMPin, HIGH);digitalWrite(motor1Polarity1, LOW);digitalWrite(motor1Polarity2, HIGH);
}
void leftz()
{
digitalWrite(motor2PWMPin, HIGH);digitalWrite(motor2Polarity1, HIGH);digitalWrite(motor2Polarity2, LOW);
}
void leftf()
{
digitalWrite(motor2PWMPin, HIGH);digitalWrite(motor2Polarity1, LOW);digitalWrite(motor2Polarity2, HIGH);
}
void st()//电机停
{
digitalWrite(motor1PWMPin, HIGH);digitalWrite(motor1Polarity1, LOW);digitalWrite(motor1Polarity2, LOW);
digitalWrite(motor2PWMPin, HIGH);digitalWrite(motor2Polarity1, LOW);digitalWrite(motor2Polarity2, LOW);
}
void blue()//蓝牙
{
if (Serial.available() > 0) {
   val = Serial.read();
/* if(val == 'A') {
   Serial.println(angle_x); //jiaodu
   }*/
   if(val == 'z') {
   rightz();
   }
   if(val == 'x') {
   rightf();
   }
   if(val == 'c') {
   leftz();
   }
   if(val == 'v') {
   leftf();
   }
   if(val == 'b') {
   st();
   }
}}
void loop()
{
t = millis();

MPU6050_ReadData();

angle_x_gyro = (gyro_x_scalled*((float)dt/1000)+angle_x);
angle_y_gyro = (gyro_y_scalled*((float)dt/1000)+angle_y);
angle_z_gyro = (gyro_z_scalled*((float)dt/1000)+angle_z);

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

angle_x = Filter_gain*angle_x_gyro+(1-Filter_gain)*angle_x_accel;
angle_y = Filter_gain*angle_y_gyro+(1-Filter_gain)*angle_y_accel;
angle_z = Filter_gain*angle_z_gyro+(1-Filter_gain)*angle_z_accel;

digitalWrite(motor1PWMPin, HIGH);
digitalWrite(motor2PWMPin, HIGH);

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);
Serial.print("____");       //串口输出

// change direction is very important 平衡
if(angle_x>0)
{
myPID.SetControllerDirection(REVERSE);

// set enablePin of motor 1 high so that motor 1 can turn on
digitalWrite(motor1Polarity1, HIGH);
digitalWrite(motor1Polarity2, LOW);

// set enablePin of motor 2 high so that motor 2 can turn on
digitalWrite(motor2Polarity1, HIGH);
digitalWrite(motor2Polarity2, LOW);
}
else
{
myPID.SetControllerDirection(DIRECT);
// set enablePin of motor 1 high so that motor 1 can turn on
digitalWrite(motor1Polarity1, LOW);
digitalWrite(motor1Polarity2, HIGH);

// set enablePin of motor 2 high so that motor 2 can turn on
digitalWrite(motor2Polarity1, LOW);
digitalWrite(motor2Polarity2, HIGH);
}

Input = angle_x;
Serial.print("Input=");Serial.print(Input);Serial.print("____");

myPID.Compute();
Serial.print("Output=");Serial.print(Output);Serial.print("____");                      Output 怎么会是0？？？？

analogWrite(motor1PWMPin, Output);
analogWrite(motor2PWMPin, Output);

Serial.print("Setpoint=");Serial.print(Setpoint); Serial.print("\t");
Serial.print("left=");Serial.print(lefta);Serial.print("");Serial.print(leftone); Serial.print("\t");
Serial.print("right=");Serial.print(righta);Serial.print("");Serial.print(rightone); Serial.print("\t"); //串口输出
blue();//蓝牙

if(rightone==1) //转动1圈标志
   {
      rightone=0;
      st(); // 停止转动
   }


while((millis()-t) < dt)
{
// Do nothing
}http://v.youku.com/v_show/id_XMTM5NDgyMTMxMg==.html

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

{:5_159:}沙发沙发～

曾经想做个超级超级小的平衡车来着: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
张禄你东西全了也做一个

多谢鼓励，哈哈，可是我始终战胜不了我的惰性{:5_162:}

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

诩发表于 2021-2-20 18:08:38

蛮厉害的

页: [1]

DF创客社区 - 分享创造的喜悦's Archiver

平衡小车