Robofun 機器人論壇

標題: 關於平衡車校正程式碼 [打印本頁]

作者: lieak59 時間: 2016-2-2 07:01
標題: 關於平衡車校正程式碼
[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角進行卡

歡迎光臨 Robofun 機器人論壇 (https://www.robofun.net/forum/)