關於平衡車校正程式碼

lieak59

[table=98%]
[tr][td]小弟最近好不容易求到了可以讓平衡車子站起來的程式碼


#include "Wire.h"
#include <Servo.h>
#include <Kalman.h>

#include "I2Cdev.h"
#include "MPU6050.h"
float fRad2Deg = 57.295779513f;       //將弧度轉為角度的乘數
const int nCalibTimes = 1000;         //校準時讀數的次數
int calibData[6];                     //校準數據
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;
bool blinkState = false;
char flag = false;                    //打印開關

unsigned long nLastTime = 0;          //上一次讀數的時間

Kalman kalmanRoll;                    //Roll角濾波器
Kalman kalmanPitch;                   //Pitch角濾波器

/*********** PID控制器參數 *********/
float ITerm, lastInput;              // 積分項、前次輸入
float Output = 0.0;                  // PID輸出值

/***********電機參數**************/
Servo ServoL;
Servo ServoR;

/***********四元數參數***********/
#define KP 0.025f//10.0f
#define KI 0.0f//0.008f
#define halfT 0.001f
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
float Angle_roll, Angle_pitch, Angle_yaw;
byte ENA = 5;byte IN1 = 9; byte IN2 = 10;
byte ENB = 6;byte IN3 = 11;byte IN4 = 12;


void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();

    // initialize serial communication
    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
    // it's really up to you depending on your project)
    Serial.begin(38400);

    // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
for( byte i = 0; i <= 12 ; i++){
pinMode(i,OUTPUT);
}
    // configure Arduino LED for

}

void loop() {
    int16_t readouts[6];
  ReadAccGyr(readouts); //讀出測量值

  float realVals[6];
  Rectify(readouts, realVals); //根據校準的偏移量進行糾正

  //四元數解析出歐拉角
// AngleUpdate(realVals);

  //計算加速度向量的模長，均以g為單位
  float fRoll = GetRoll(realVals); //計算Roll角
  float fPitch = GetPitch(realVals); //計算Pitch角

  //計算兩次測量的時間間隔dt，以秒為單位
  unsigned long nCurTime = micros();
  float dt = (double)(nCurTime - nLastTime) / 1000000.0;
  //對Roll角和Pitch角進行卡