自平衡車

#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=20;

float kiX=0;

float kdX=10;

float SetpointY=180;

float InputY,OutputY,errSumY,lastErrY;

float kpY=20;

float kiY=0;

float kdY=10;

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

int cons=100;

int pwm1,pwm2,pwm3,pwm4;

Timer t;//定时器时间

void front(){

analogWrite(5,255);

analogWrite(6,0);

analogWrite(9,0);

analogWrite(10,255);

}

void setup() {

Wire.begin();

Serial.begin(115200);

pinMode(5,OUTPUT);

pinMode(10,OUTPUT);

pinMode(9,OUTPUT);

pinMode(6,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)+2;

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

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

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

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

t.update();

if(AngleY<=-0.8){

pwm1=cons+OutputY;

pwm1=constrain(pwm1,0,255);

analogWrite(5,0);

analogWrite(10,0);

analogWrite(6, pwm1);

analogWrite(9,pwm1);

}

if(AngleY>=0.8){

pwm1=cons-OutputY;

pwm1=constrain(pwm1,0,255);

analogWrite(6,0);

analogWrite(9,0);

analogWrite(5, pwm1);

analogWrite(10,pwm1);

}

if(AngleY>-0.8&&AngleY<0.8){

pwm1=cons-OutputY;

pwm1=constrain(pwm1,0,255);

analogWrite(6,0);

analogWrite(9,0);

analogWrite(5, 0);

analogWrite(10,0);

}

// analogWrite(5,pwm1);

// analogWrite(6,pwm4);

// analogWrite(9,pwm2);

// analogWrite(10,pwm3);

Serial.print(AngleY);

Serial.print(',');

Serial.println(pwm1);

//Serial.print(',');

// Serial.println(AngleZ);

//Serial.print(',');

//Serial.println(pwm4);

}

#include "Wire.h"

#include "I2Cdev.h"

#include "MPU6050.h"

#include <Kalman.h>

#include "Timer.h"

Kalman kalmanY;

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 SetpointY=0;

float InputY,OutputY,errSumY,lastErrY;

float kpY=20;

float kiY=0;

float kdY=10;

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

int cons=100;

int v,k;

Timer t;//定时器时间

void setup() {

Wire.begin();

Serial.begin(115200);

pinMode(5,OUTPUT);

pinMode(6,OUTPUT);

pinMode(7,OUTPUT);

pinMode(8,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);

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

}

void PIDCalc ()

{

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)+5;

InputY=AngleY;

t.update();

if( AngleY>=0){

digitalWrite(7,1);

digitalWrite(5,0);

}else{

digitalWrite(7,0);

digitalWrite(5,1);

}

k=500/t

for(int i=0 , i<k,i++)

digitalWrite(6,1);

digitalWrite(8,1);

delayMicroseconds(t)

digitalWrite(6,0);

digitalWrite(8,0);

delayMicroseconds(t)

Serial.print(AngleY);

Serial.print(',');

Serial.println(pwm1);

}