我的怎么也站不起来!
#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[2][2] = {{ 1, 0 },{ 0, 1 }};
static float Pdot[4] ={ 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[0]=Q_angle - P[0][1] - P[1][0];
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
angle_err = angle_m - angle;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
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[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;
}
void PIDD(){
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)));
}
|