小四軸

#include "Wire.h"

#include "I2Cdev.h"

#include "MPU6050.h"

#include <Kalman.h>

#include "Timer.h"

Kalman kalmanX , kalmanY, kalmanZ;

MPU6050 accelgyro;

int16_t ax, ay, az, gx, gy, gz;

double accX, accY, accZ, gyroX, gyroY, gyroZ;

uint32_t timer;

double pitch, roll, yaw, AngleX, AngleY, AngleZ, Xrate, Yrate, Zrate;

//pid参数

float SetpointX=180;

float InputX,OutputX,errSumX,lastErrX;

float kpX=2;

float kiX=0;

float kdX=1;

float SetpointY=180;

float InputY,OutputY,errSumY,lastErrY;

float kpY=2;

float kiY=0;

float kdY=1;

float timeChange=10;//pid采样时间间隔毫秒

int cons=125;

int pwm1,pwm2,pwm3,pwm4;

Timer t;//定时器时间

void setup() {

Wire.begin();

Serial.begin(115200);

pinMode(3,OUTPUT);

pinMode(10,OUTPUT);

pinMode(9,OUTPUT);

pinMode(11,OUTPUT);

accelgyro.initialize();

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

accX = ax; accY = ay; accZ = az; gyroX = gx; gyroY = gy; gyroZ = gz;

pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

roll = atan2(accY , accZ) * RAD_TO_DEG;

yaw = atan2(accX , accY) * RAD_TO_DEG;

kalmanY.setAngle(pitch);

kalmanX.setAngle(roll);

kalmanZ.setAngle(yaw);

t.every(timeChange, PIDCalc) ;//每10毫秒pid一次

}

void PIDCalc ()

{

float errorX = SetpointX - InputX;

errSumX += (errorX * timeChange);

float dErrX = (errorX - lastErrX) / timeChange;

OutputX = kpX * errorX +kiX * errSumX + kdX* dErrX ;

lastErrX = errorX;

float errorY = SetpointY - InputY;

errSumY += (errorY * timeChange);

float dErrY = (errorY - lastErrY) / timeChange;

OutputY = kpY * errorY +kiY * errSumY + kdY * dErrY ;

lastErrY = errorY;

}

void loop() {

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

accX = ax;

accY = ay;

accZ = az;

gyroX = gx;

gyroY = gy;

gyroZ = gz;

double dt = (double)(micros() - timer) / 1000000; // Calculate delta time

timer = micros();

pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

roll = atan2(accY,accZ) * RAD_TO_DEG;

yaw = atan2(accX , accY) * RAD_TO_DEG;

Yrate = gyroY / 131.0; // Convert to deg/s

Xrate = gyroX / 131.0;

Zrate = gyroZ / 131.0;

AngleY = kalmanY.getAngle(pitch, Yrate, dt);

AngleX = kalmanX.getAngle(roll, Xrate, dt);

AngleZ = kalmanZ.getAngle(yaw, Zrate, dt);

InputX=map(AngleX,-180,180,0,360);

InputY=map(AngleY,-180,180,0,360);

t.update();

pwm1=cons+OutputX+OutputY;

pwm1=constrain(pwm1,0,255);

pwm2=cons-OutputX-OutputY;

pwm2=constrain(pwm2,0,255);

pwm3=cons-OutputX+OutputY;

pwm3=constrain(pwm3,0,255);

pwm4=cons+OutputX-OutputY;

pwm4=constrain(pwm4,0,255);

analogWrite(3,pwm1);

analogWrite(10,pwm4);

analogWrite(9,pwm2);

analogWrite(11,pwm3);

Serial.print(pwm1);

Serial.print(',');

Serial.print(pwm2);

Serial.print(',');

Serial.print(pwm3);

Serial.print(',');

Serial.println(pwm4);

}