#include <I2Cdev.h>
#include<Wire.h>
#include<Stepper.h>
#include "MPU6050.h"
#include <PWM.h>
#include "PID_v1.h"
#define Gry_offset 296 //陀螺仪X轴的静态漂移
#define Gyr_Gain -0.00763 //X轴读数转化为角速=1/131，向前方向与X轴相反
#define ACC_Gain 0.000061 //读数转化为加速度值=1/16384
#define pi 3.1415926
#define k 0.715 //一阶互补滤波权重取值
//步数等于360/电机的步距角 1.8
#define STEPS 200
//电机对象stepper
Stepper stepper(STEPS, 8, 9, 10, 11);
int previous = 0; //储存历史读数
int val;
const int aInPin = A0;
//设置每台电机的速度和方向引脚
int speed1 = 3;
int speed2 = 11;
int direction1 = 12;
int direction2 = 13;
int Speed_need = 0;
int Turn_need = 0;
float LSpeed_Need=0.0,RSpeed_Need=0.0;// 转弯设置
float speeds,speeds_filter,positions;
float diff_speeds,diff_speeds_all;
float Kp = 10, Kd = 0.06, Ksp = 2.8, Ksi = 0.11;
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
//motor
int dirPin1 = 8;
int stepperPin1 = 9;
int dirPin2 = 6;
int stepperPin2 = 7;
/* 角度参数 */
float Gyro_y; //Y轴陀螺仪数据暂存
float Accel_x; //X轴加速度值暂存
float Angle_ax; //由加速度计算的倾斜角度
float Angle_gy; //由角速度计算的倾斜角度
float Angle; //小车最终倾斜角度
int Setpoint = 0;
/********** 互补滤波器参数 *********/
unsigned long now ;
unsigned long preTime = 0;
float angleG = 0;
/*********** PID控制器参数 *********/
unsigned long lastTime = 0; // 前次时间
float ITerm = 0.0, lastInput = 0.0; // 积分项、前次输入
float Output = 0.0; // PID输出值
float LOutput,ROutput;
bool blinkState = false;
//******卡尔曼参数************
float Q_angle=0.001; //相对于加速度计的噪音协方差
float Q_gyro=0.003;
float R_angle=0.5; //测量噪声协方差
float dt=0.01; //dt为kalman滤波器采样时间;
char C_0 = 1;
float Q_bias, Angle_err; //Q_bias相对于陀螺仪的噪音协方差
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } }; //协方差误差矩阵
// 卡尔曼滤波20MHz的处理时间约0.77ms；
void Kalman_Filter(float Accel,float Gyro)
{
Angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle;//zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[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 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle+= K_0 * Angle_err; //后验估计
Q_bias+= K_1 * Angle_err; //后验估计
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}
void PID1()
{
now = millis(); // 当前时间(ms)
float TimeChange = (now - lastTime) / 1000.0; // 采样时间(s)
float Kp = 10.0, Ki = 0.23, Kd = 0.0; // 比例系数、积分系数、微分系数
float SampleTime = 0.1; // 采样时间(s)
float Setpoint =-2.0; // 设定目标值(degree)
//float outMin = -400.0, outMax = +400.0; // 输出上限、输出下限
if(TimeChange >= SampleTime) { // 到达预定采样时间时
float Input = angleG; // 输入赋值
float error = Setpoint - Input; // 偏差值
ITerm+= (Ki * error * TimeChange); // 计算积分项
// ITerm = constrain(ITerm, outMin, outMax); // 限定值域
float DTerm = Kd * (Input - lastInput) / TimeChange; // 计算微分项
Output = Kp * error + ITerm + DTerm; // 计算输出值
// Output = constrain(Output, outMin, outMax); // 限定值域
LOutput=Output+RSpeed_Need; //左电机
ROutput=Output-RSpeed_Need; //右电机
lastInput = Input; // 记录输入值
lastTime = now; // 记录本次时间
}
}
void PWM1(){
if(Output > 0){
step_forwards(Output);
}
if (Output < 0){
step_backwards(abs(Output));
}
}
//电机运转控制
void step_forwards(int speeds){
digitalWrite(dirPin1,0);
digitalWrite(dirPin2,1);
digitalWrite(stepperPin1, HIGH);
digitalWrite(stepperPin2, HIGH);
delayMicroseconds(speeds);
digitalWrite(stepperPin1, LOW);
digitalWrite(stepperPin2, LOW);
delayMicroseconds(speeds);
}
void step_backwards(int speeds){
digitalWrite(dirPin1,1);
digitalWrite(dirPin2,0);
digitalWrite(stepperPin1, HIGH);
digitalWrite(stepperPin2, HIGH);
delayMicroseconds(speeds);
digitalWrite(stepperPin1, LOW);
digitalWrite(stepperPin2, LOW);
delayMicroseconds(speeds);
}
const int MPU_addr=0x68; // I2C address of the MPU-6050
void setup(){
stepper.setSpeed(200);
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Serial.begin(38400);
accelgyro.initialize(); //MPU6050初始化
pinMode(dirPin1, OUTPUT);
pinMode(stepperPin1, OUTPUT);
pinMode(dirPin2, OUTPUT);
pinMode(stepperPin2, OUTPUT);
}
void loop()
{
//获取传感器读数
val = analogRead(aInPin);
val = map(val, 0, 1023, 0, 99);
Serial.println(val);
if(val > 50)
{
stepper.step(STEPS / 2);
delay(500);
}
else
{
stepper.step(-STEPS / 2);
delay(500);
}
//移动步数
stepper.step(val - previous);
//保存历史读数
previous = val;
//MP6050
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取六轴数值
float Y_Accelerometer = ay * ACC_Gain;//转换为向前的加速度(g)，为负值
float Z_Accelerometer = az * ACC_Gain;//转换为向下的加速度(g)
float angleA= atan(Y_Accelerometer/ Z_Accelerometer)* (-180)/ pi;//加速度仪，反正切获得弧度值，乘以180度/pi
now = millis(); //当前时间ms
float dt = (now - preTime) / 1000.0; //微分时间
preTime = now; //上一次采样时间
float gx_revised = gx + Gry_offset;//陀螺仪x轴静态时修正后的角速度读数，向前为负值
float omega= Gyr_Gain* gx_revised;//陀螺仪，转换为向前的角速度(o/s)，Gyr_Gain取负，故负负得正
float angle_dt = omega * dt;//角度的单次积分值
//angleG+= angle_dt;//陀螺仪，积分获得角度值
angleG+= (Gyr_Gain * (gx + Gry_offset)) * dt;
float A= k/ (k+ dt);//陀螺仪的权值
angleG= A* (angleG+ omega* dt)+ (1-A)* angleA;//一阶互补滤波后的输出角度(o)
if (abs(angleG)<45) /////////////////up-right judge
{
PID1();
PWM1();
}
Serial.print(angleG); Serial.print("\t");
Serial.println(Output);
}
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
