雨轩 发表于 2014-11-6 12:46:59

我的平衡小车怎么也站不起来，老是一边倒

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

#include <FlexiTimer2.h>
//#include <LiquidCrystal_I2C.h>
//LiquidCrystal_I2C lcd(0x27,16,2); //set the LCD address to 0x27 for a 16 chars and 2 line display
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

// -----------
#define PinA 2//中断0
#define PinB 3//中断1
const int encoderPinA = 4;
const int encoderPinB = 5;

//unsigned long time = 0;
long countA = 0; //计数值
long countB = 0; //计数值
       
//-------------------------
int TN1=7;
int TN2=8;
int ENA=9;

int TN3=12;
int TN4=13;
int ENB=10;

int LOutput;
int ROutput;
//初始化
char data;
//-----
float angleA,omega;

#define Gry_offset 1   // 陀螺仪偏移量
#define Gyr_Gain 0.00763358    //对应的1G
#define pi 3.14159

float K_angle=4.0;
float K_angle_dot=0.2;       //换算系数：256/10 =25.6；
float K_position=0.1;                //换算系数：(256/10) * (2*pi/(64*12))=0.20944；//256/10:电压换算至PWM，256对应10V；       
float K_position_dot=4;

//float K_position=0.8 * 0.209;                //换算系数：(256/10) * (2*pi/(64*12))=0.20944；//256/10:电压换算至PWM，256对应10V；
//float K_angle=34 * 25.6;   //换算系数：256/10 =25.6；
//float K_position_dot=1.09 * 20.9;                //换算系数：(256/10) * (25*2*pi/(64*12))=20.944；
//float K_angle_dot=2 * 25.6;                //换算系数：256/10 =25.6；
int Speed_Need;
int Turn_Need;
float Output;

static float kp, ki, kd, kpp;
static float P = {{ 1, 0 },{ 0, 1 }};
static float Pdot ={ 0,0,0,0};
//static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,dt=0.01;
static const float Q_angle=0.001, Q_gyro=0.004, R_angle=0.5,dt=0.01;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
static const char C_0 = 1;
float position_dot,position_dot_filter,positiono;
float angle, angle_dot;

void setup()
{
   //---------------
Wire.begin();
    Serial.begin(9600);
   accelgyro.initialize();
   delay(100);
    pinMode(TN1,OUTPUT);         
    pinMode(TN2,OUTPUT);
    pinMode(TN3,OUTPUT);
    pinMode(TN4,OUTPUT);
    pinMode(ENA,OUTPUT);
    pinMode(ENB,OUTPUT);   
    //Serial.println("ki ka kb kc");
   delay(100);
   // -----------
    pinMode(PinA,INPUT); //D2脚为输入
    pinMode(PinB,INPUT); //D3脚为输入

    attachInterrupt(0, blinkA,FALLING);//注册中断0调用函数blinkA
    attachInterrupt(1, blinkB, FALLING);//注册中断1调用函数blinkB
    pinMode(encoderPinA, INPUT);
    pinMode(encoderPinB, INPUT);
    digitalWrite(encoderPinA, HIGH);
    digitalWrite(encoderPinB, HIGH);
// time = millis(); //时间初值
    delay(100);

FlexiTimer2::set(10, flash); // call every 500 1ms "ticks"
FlexiTimer2::start();
}

void loop()
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//SDEER();
   if (Serial.available()>0)
{
      data = Serial.read();
      delay(2);
      switch(data)
      {
         // case ' ':Speed_Need=0;Turn_Need=0;break;
      case 'w':Speed_Need=18;break;
         case 's':Speed_Need=-38;break;
          case 'a':Turn_Need=-30;break;
         case 'd':Turn_Need=30;break;
         default:Speed_Need=0;Turn_Need=0;break;
      }
}

      PWMB();
      /*
      Serial.print(angleA);
Serial.print(',');
Serial.print(omega);
Serial.print(',');
Serial.println(Output);
   
    // Serial.print(position_dot);
//   Serial.print(',');
   
Serial.print(countA);
Serial.print(',');
Serial.print(countB);
Serial.print(',');

*/
    }
//--------------------
void flash()
{
ADOM();
PIDD();
countA=countB=0;
}

//中断0调用函数

void blinkA()
{
// if ((millis() - time) > 3) //防抖动处理
    if (digitalRead(encoderPinA) == HIGH)
    {
       countA ++;
    }
      else
    {
       countA --;
    }
// time = millis();
}

void blinkB()
{
// if ((millis() - time) > 3) //防抖动处理
    if (digitalRead(encoderPinB) == HIGH)
    {
      countB --;
    }
      else
    {
       countB ++;
    }
// time = millis();
}


/*
void SDEER()
{
   if (Serial.available()>0)
{
      data = Serial.read();
      delay(2);
      switch(data)
      {
         // case ' ':Speed_Need=0;Turn_Need=0;break;
      case 'w':Speed_Need=18;break;
         case 's':Speed_Need=-38;break;
          case 'a':Turn_Need=-30;break;
         case 'd':Turn_Need=30;break;
         default:Speed_Need=0;Turn_Need=0;break;
      }
      
    // if(data=='w'){ Speed_Need=5;}
   //else if(data=='s'){ Speed_Need=-5;}
   //else if(data=='a'){ Turn_Need=20;}
    // else if(data=='d'){ Turn_Need=-20;}
    // else{Speed_Need=0;Turn_Need=0;}
   
    // delay(2);
}
}
*/

void ADOM()
{
   angleA= atan2(ay , az) * 180 / pi;   // 根据加速度分量得到的角度(degree)加0.5偏移量
//180度至0至-180（360度）取0度为坚直时中立点 因为坚直时有偏差，所以加0.5....
   omega=Gyr_Gain * (gx +Gry_offset); // 当前角速度(degree/s)
   Kalman_Filter(angleA,omega);
}
// -------------
void Kalman_Filter(double angle_m,double gyro_m)
{
        angle+=(gyro_m-q_bias) * dt;       
        Pdot=Q_angle - P - P;
        Pdot=- P;
        Pdot=- P;
        Pdot=Q_gyro;       
        P += Pdot * dt;
        P += Pdot * dt;
        P += Pdot * dt;
        P += Pdot * dt;       
        angle_err = angle_m - angle;               
        PCt_0 = C_0 * P;
        PCt_1 = C_0 * P;       
        E = R_angle + C_0 * PCt_0;       
        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;       
        t_0 = PCt_0;
        t_1 = C_0 * P;
        P -= K_0 * t_0;
        P -= K_0 * t_1;
        P -= K_1 * t_0;
        P -= K_1 * t_1;               
        angle        += K_0 * angle_err;
        q_bias        += K_1 * angle_err;
        angle_dot = gyro_m-q_bias;
}
voidPIDD(){
      kp=analogRead(0)*0.005;
        ki=analogRead(1)*0.005;
        kd=analogRead(2)*0.004;
        kpp=analogRead(3)*0.004;
      position_dot=(countA+countB)*0.5;                //利用PWM值代替轮速传感器的信号
      position_dot_filter*=0.8;                //车轮速度滤波
      position_dot_filter+=position_dot*0.2;       
      positiono+=position_dot_filter;          //增加这里
      //position+=position_dot;
      positiono+=Speed_Need;   
      positiono= constrain(positiono, -800, +800);
      Output= K_angle*angle *kp + K_angle_dot*angle_dot *ki +K_position*positiono*kd +K_position_dot*position_dot_filter *kpp;
      LOutput=Output+Turn_Need;
      ROutput=Output-Turn_Need;
}


void PWMB()
{
   if(LOutput>3)//左电机-------或者取0
{
    digitalWrite(TN1, HIGH);
    digitalWrite(TN2, LOW);
    analogWrite(ENA,min(255,abs(LOutput)+2)); //PWM调速a==0-255
}
else if(LOutput<-3)//-------或者取0
{
    digitalWrite(TN1, LOW);
    digitalWrite(TN2, HIGH);
    analogWrite(ENA,min(255,abs(LOutput)+2)); //PWM调速a==0-255
}
else
{
    digitalWrite(TN1, LOW);
    digitalWrite(TN2, LOW);
    analogWrite(ENA,0);
}

if(ROutput>3)//右电机--------或者取0
{
    digitalWrite(TN3,LOW);
    digitalWrite(TN4,HIGH);
    analogWrite(ENB,min(255,abs(ROutput)));

}
else if(ROutput<-3)//-------或者取0
{
    digitalWrite(TN3,HIGH);
    digitalWrite(TN4,LOW);
    analogWrite(ENB,min(255,abs(ROutput)));
         
}
else
{
    digitalWrite(TN3, LOW);
    digitalWrite(TN4, LOW);
    analogWrite(ENB,0);
}
//analogWrite(ENA,min(255,abs(LOutput))); //PWM调速a==0-255
//analogWrite(ENB,min(255,abs(ROutput+4)));
}

ketose 发表于 2014-11-7 12:30:05

一边倒的情况，应该是重心没找对。应该是调整传感器的静差。

雨轩 发表于 2014-11-7 19:03:27

我都糊涂了，我问另一个人，他说是我编码器没工作的原因。之前我也想过是陀螺仪没校正，但是没校正应该站不起来呀，可是我的怎么推都推不倒，就是推了会往另一边跑，而且速度会越来越快

ketose 发表于 2014-11-8 22:31:50

本帖最后由 ketose 于 2014-11-8 22:43 编辑

LZ用的电机是什么电机？

雨轩 发表于 2014-11-10 12:30:50

应该是三美的电机，扫地机器人上用的，一个同步带加两极行星减速的

zhoulei 发表于 2014-11-10 16:56:58

我的电机码盘有问题，脉冲极度不均匀，达到了几个毫秒，用它测速根本没意义

ketose 发表于 2014-11-12 13:05:26

雨轩 发表于 2014-11-7 19:03 static/image/common/back.gif
我都糊涂了，我问另一个人，他说是我编码器没工作的原因。之前我也想过是陀螺仪没校正，但是没校正应该站不 ...

就是重心偏了，校正下静差。

SproutME 发表于 2014-11-15 19:37:06

要想实现站立，只要角度环就可以了。而且只要一个比例系数就行了。建议你先将你的卡尔曼滤波器数据通过串口传出来看看。（最近也在做平衡车感觉还是用步进电机好点，如果你用直流电机的话买功率大的，就是转的快扭力大的，电机很重要，不然软件很难调，电机电源最好有稳压模块，不然刚调好的参数换节电池又变了！）
页: [1]
查看完整版本: 我的平衡小车怎么也站不起来，老是一边倒