#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空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
