平衡小车 MPU6050+L298n

我歌月徘徊

现在小车站立没有问题，但是抖动比较厉害。
程序方面是参考别人的，自己调节了一下参数，但是效果不太理想。
使用的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圈（这样理解可以吧）。 所以 这个控制程序不会写了。。。请求帮助啊{:soso_e130:}

代码
[mw_shl_code=bash,true]#include<Wire.h>
#include "gyro_accel.h"
#include "ID_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
  }
  

Betteronly

楼主，
这种直流电机用步进驱动也可以做平衡小车？
我以为这种垃圾电机响应有点慢不能做呢！

学习了，果断收藏。。

抖动问题，我的理解（没实践，纯理论）
响应速度应该是个因素，
还有就是，根据倾斜角大小，电机的维护平衡速度应该有不同的加速度去维护吧？
而不是，单纯判断机身只要不正，马上就修正姿势，
这样判断的是一个点，临界值不是个范围，，这样总是在修正姿势，是不是就抖开了？

舵机定到某位置也是要抖动的，因为要维护那个位置，它肯定是正反转的临界点。。

我空想的哈。

Betteronly

角度 angle_x ，这个就是机身倾斜的角度吗？
如果是的话，
if(angle_x>0)   控制L298n
0是只要倾斜就修正，那肯定抖
比如大于5度，然后加速度1去修正，
大于10度，然后加速2去修正，
这样的思路呢？
不知道能不能实现。 当然多大角度多大加速度去修正这个得实践我感觉。

姿势修正的越及时(0度左右)，抖动越厉害，因为电机总在 正转反转间徘徊，所以都。
如果姿势修正稍微范围放宽些，可能看到的现象是，机身摆动幅度比较大，如果电机加速度够的话，应该能修正过来。
看到的现象应该是，快倒下了，修正起来了，快倒下了修正起来了。。

我歌月徘徊

Betteronly 发表于 2015-11-25 12:03
楼主，
这种直流电机用步进驱动也可以做平衡小车？
我以为这种垃圾电机响应有点慢不能做呢！

电机的平衡速度的确是变化的，如果倾角很大，电机就快速进行修正，反之。。。
在我程序里面也有体现   
Input = angle_x;   //把倾角给input进行PID
  Serial.print("Input=");Serial.print(Input);Serial.print("____");
   
  myPID.Compute();   //计算之后  得到输出output    但是通过串口观察输出 是0  很纳闷。。。
  Serial.print("Output=");Serial.print(Output);Serial.print("____");                        Output 怎么会是0？？？？
   
  analogWrite(motor1PWMPin, Output);    //这就是将pid输出结果按比例控制转速 0-255
  analogWrite(motor2PWMPin, Output);

问题就是 output为什么是0 ？  是0的话 电机转速就是恒定的。。。

我歌月徘徊

Betteronly 发表于 2015-11-25 12:09
角度 angle_x ，这个就是机身倾斜的角度吗？
如果是的话，
if(angle_x>0)   控制L298n

angle _是倾斜角度
你的这个思路也可以，就是这个加速度是人为给他添加的，得经过多次测试。
如果我可以经过PID算法得出Output的话，那么这个加速度就可以让系统自己实现。
程序为何算不出OUTPUT呢？？？ 奇怪

Betteronly

myPID.Compute();
是干什么的？没看到内部实现代码？是库函数？

OUTPUT我没见你哪里设置过啊，一直是初期值吧？
myPid 引用地址，在内部修改是么。。

不解。

我歌月徘徊

Betteronly 发表于 2015-11-25 13:15
myPID.Compute();
是干什么的？没看到内部实现代码？是库函数？

myPID.Compute(); 还是高亮显示的。
这是库函数，看了一圈，看不大懂       PID_v1.zip (7.47 KB, 下载次数: 40)

2015-11-25 13:34 上传

点击文件名下载附件

Betteronly

我歌月徘徊 发表于 2015-11-25 13:34
myPID.Compute(); 还是高亮显示的。
这是库函数，看了一圈，看不大懂      

debug跟一下。

我歌月徘徊

Betteronly 发表于 2015-11-25 14:30
debug跟一下。

？？？什么意思

Betteronly

我还不清楚，arduino应该可以调试吧？你走到库函数里看看