基于arduino的自平衡车代码

基于arduino的自平衡车代码

#include "Wire.h"

#include "I2Cdev.h"

#include "MPU6050.h"

MPU6050 accelgyro;

int16_t ax, ay, az;

int16_t gx, gy, gz;

int timeChange;

int E1 = 6;

int M1 = 7;

int E2 = 5;

int M2 = 4;

#define LED_PIN 13

#define pi 3.14159

#define Gry_offset -20

#define Gyr_Gain 0.00763358

bool blinkState = false;

float angleA,omega,f_angle;

float kp, ki, kd;

float SampleTime = 0.08; //-------------------互补滤波+PID 采样时间0.08 s

unsigned long lastTime;

float Input, Output, Setpoint;

float errSum,dErr,error,lastErr;

float LSpeed_Need=0.0,RSpeed_Need=0.0;

float LOutput,ROutput;

unsigned long preTime = 0;

void setup() {

Wire.begin();

Serial.begin(38400);

Serial.println("Initializing I2C devices...");

accelgyro.initialize();

Serial.println("Testing device connections...");

Serial.println(accelgyro.testConnection() ? "MPU6050 successful" : "MPU6050 connection failed");

pinMode(LED_PIN, OUTPUT);

pinMode(M1, OUTPUT);

pinMode(M2, OUTPUT);

connection

}

void loop() {

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

angleA= atan2(ay , az) * 180 / pi+0.5;

omega= Gyr_Gain * (gx + Gry_offset);

blinkState = !blinkState;

digitalWrite(LED_PIN, blinkState);

Serial.print(angleA);

Serial.print(omega);

if (abs(angleA)<45) {

PIDfilter();

PWMB();

}

}

void PIDfilter()

{

kp =0.9;

ki =0.0002;

kd =0.26;

unsigned long now = millis(); float dt = (now - preTime) / 1000.0; preTime = now;

float K = 0.8;

float A = K / (K + dt);

f_angle = A * (f_angle + omega * dt) + (1-A) * angleA;

Serial.println(f_angle);

// 当前时间(ms)

// 微分时间(ms)

timeChange = (now - lastTime);

if(timeChange>=SampleTime)

{

Setpoint=LSpeed_Need;// =0，(+ -值使电机前进或后退)

Input =f_angle;

error = Setpoint- Input;

errSum += error* timeChange;

dErr = (error - lastErr)/ timeChange;

//PID Output

Output = kp * error + ki * errSum + kd * dErr;

LOutput=Output+RSpeed_Need+0.34;//左电机

ROutput=Output-RSpeed_Need+0.34;//右电机

lastErr = error;

lastTime = now;

Serial.print( Output);

Serial.print( ROutput);

Serial.println( LOutput);

}

}

void PWMB(){

if(LOutput>0.5)//左电机-------或者取0

{

digitalWrite(M1, HIGH);

//digitalWrite(E1, LOW);

Serial.print( 'a');

analogWrite(E1,min(255,abs(LOutput)+255)); //PWM调速a==0-255

}

else if(LOutput<-0.5)//-------或者取0

{

digitalWrite(M1, LOW);

//digitalWrite(E1, HIGH);

analogWrite(E1,min(255,abs(LOutput)+255)); //PWM调速a==0-255

Serial.print( 'b');

}

else //刹车-------取0后可以不用

{

digitalWrite(M1, LOW);

digitalWrite(E1, 0);

Serial.print( 'c');

}

if(ROutput>0.5)//右电机-

-------或者取0

{

digitalWrite(M2, HIGH);

//digitalWrite(E2, LOW);

analogWrite(E2,min(255,abs(ROutput)+253));

Serial.print( 'd');

}

else if(ROutput<-0.5)//-------或者取0

{

digitalWrite(M2, LOW);

//digitalWrite(E2, HIGH);

analogWrite(E2,min(255,abs(ROutput)+253));

Serial.print( 'e');

}

else//刹车-------取0后可以不用

{

digitalWrite(M2, LOW);

digitalWrite(E2, 0);

Serial.print( 'f');

}

// analogWrite(E1,min(255,abs(LOutput)+25)); //PWM调速a==0-255

//analogWrite(E2,min(255,abs(ROutput)+23));

}