關於平衡車檢測角度數據程式碼

lieak59 · 发表于 2016-2-2 06:59:31

本帖最后由 lieak59 于 2016-2-2 09:03 编辑

小弟最近好不容易求到了可以讓平衡車子站起來的程式碼

#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角进行卡尔曼滤波
  float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt);
  float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], dt);
  //更新本次测的时间
  nLastTime = nCurTime;

  //PID控制
  nCurTime = millis();                                  // 当前时间(ms)
  float TimeCh = (nCurTime - nLastTime) / 1000.0;       // 采样时间(s)
  float Kp = 3.5, Ki = 0.0, Kd = 0.0;                   // 比例系数、积分系数、微分系数
  float SampleTime = 0.1;                               // 采样时间(s)
  float Setpoint = 0;                                  // 设定目标值(degree)
  float outMin = -255, outMax = +255;                   // 输出上限、输出下限
  if (TimeCh >= SampleTime) {                            // 到达预定采样时间时
float Input = fNewPitch;                         // 输入赋值
float error = Setpoint - Input;                   // 偏差值
ITerm += (Ki * error * TimeCh);                   // 计算积分项
ITerm = constrain(ITerm, outMin, outMax);          // 限定值域
float DTerm = Kd * (Input - lastInput) / TimeCh; // 计算微分项
Output = Kp * error + ITerm - DTerm;             // 计算输出值
Output = constrain(Output, outMin, outMax);       // 限定值域
//servoL.write(Output + servoL_offset);          // 控制左驱
//servoR.write(Output + servoR_offset);          // 控制右驱
lastInput = Input;                               // 记录输入值
nLastTime = nCurTime;                            // 记录本次时间
  }
  if (Serial.available() > 0) {
char rece = Serial.read();
delay(10);
if (rece == (char)'#')
{
   Serial.println("Nano is Activity...");
}
flag = rece;
  }
  if (flag == (char)'R')
  {
Serial.print("Roll: (" );
Serial.print(fNewRoll);
Serial.print(" ) \t");
Serial.print("Pitch : (" );
Serial.print(fNewPitch);
Serial.print(" ) \ r \ n ");
  }

  if(Output > 0){
  analogWrite(ENA,Output);
  digitalWrite(IN1,HIGH);digitalWrite(IN2,LOW);
analogWrite(ENB,Output);
  digitalWrite(IN3,HIGH);digitalWrite(IN4,LOW);
  }
  else{analogWrite(ENA,Output);
  digitalWrite(IN1,LOW);digitalWrite(IN2,HIGH);
analogWrite(ENB,Output);
  digitalWrite(IN3,LOW);digitalWrite(IN4,HIGH);


  }
}
void ReadAccGyr(int16_t *pVals)
{
  accelgyro.getMotion6(&pVals[0], &pVals[1], &pVals[2], &pVals[3], &pVals[4], &pVals[5]);
}

//对大量读数进行统计，校准平均偏移量
void Calibration()
{
  int16_t valSums[6] = {0};
  //先求和
  for (int i = 0; i < nCalibTimes; ++i)
  {
ReadAccGyr(valSums);
  }
  //再求平均
  for (int i = 0; i < 6; ++i)
  {
calibData = int(valSums / nCalibTimes);
  }
  calibData[2] += 16384; //设芯片Z轴竖直向下，设定静态工作点。
}

//对读数进行纠正，消除偏移，并转换为物理量。公式见文档。
void Rectify(int *pReadout, float *pRealVals)
{
  for (int i = 0; i < 3; ++i)
  {
pRealVals = (float)(pReadout - calibData) / 16384.0f;
  }

  for (int i = 3; i < 6; ++i)
  {
pRealVals = (float)(pReadout - calibData) / 131.0f;
  }
}

//算得Roll角。算法见文档。
float GetRoll(float *pRealVals)
{
  float fNorm = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
  float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]);
  float fCos = fNormXZ / fNorm;
  if (pRealVals[1] > 0)
  {
return -acos(fCos) * fRad2Deg;
  }
  else
return acos(fCos) * fRad2Deg;
}

//算得Pitch角。算法见文档。
float GetPitch(float *pRealVals)
{
  float fNorm = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
  float fNormYZ = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
  float fCos = fNormYZ / fNorm;
  if (pRealVals[0] < 0)
  {
return -acos(fCos) * fRad2Deg;
  }
  else
return acos(fCos) * fRad2Deg;
}

void AngleUpdate(float *pRealVals)
{
  float norm;
  float vx, vy, vz;
  float ex, ey, ez;

  if (pRealVals[0]*pRealVals[1]*pRealVals[2] == 0)
return;

  norm = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);

  vx = 2 * (q1 * q3 - q0 * q2);
  vy = 2 * (q0 * q1 - q3 * q2);
  vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

  ex = pRealVals[1] / norm * vz - pRealVals[2] / norm * vy;
  ey = pRealVals[2] / norm * vx - pRealVals[0] / norm * vz;
  ez = pRealVals[0] / norm * vy - pRealVals[1] / norm * vx;

  exInt = exInt + ex * KI;
  eyInt = eyInt + ey * KI;
  ezInt = ezInt + ez * KI;

  q0 = q0 + (-q1 * pRealVals[3] + KP * ex + exInt - q2 * pRealVals[4] + KP * ey + eyInt - q3 * pRealVals[5] + KP * ez + ezInt) * halfT;
  q1 = q1 + (q0 * pRealVals[3] + KP * ex + exInt + q2 * pRealVals[5] + KP * ez + ezInt - q3 * pRealVals[4] + KP * ey + eyInt) * halfT;
  q2 = q2 + (q0 * pRealVals[4] + KP * ey + eyInt - q1 * pRealVals[5] + KP * ez + ezInt + q3 * pRealVals[3] + KP * ex + exInt) * halfT;
  q3 = q3 + (q0 * pRealVals[5] + KP * ez + ezInt + q1 * pRealVals[4] + KP * ey + eyInt - q2 * pRealVals[3] + KP * ex + exInt) * halfT;

  norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;

  Angle_roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * fRad2Deg;
  Angle_pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * fRad2Deg;
  Angle_yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * fRad2Deg;
}

　

我看了裡面的程式碼有卡爾曼濾波跟pid的程式碼已知要調整比例係數，就可以做修正了

區間是1-10 可是我調了好幾次都沒有自動校正

因為車子車身本身不平衡看了板上蠻多車子都是靠MPU6050來做平衡校正

東西要趨近於0 我東西是要看pitch 上下特別的是趨近於0馬達會動也不太懂是啥意思

照理說因該是頃斜後他自己會重新立起來跟不倒翁一樣

有必要傳小車圖片? 我的電機是用香蕉電機跟板上的很多位大大一樣不過我香蕉電機是平放不是豎在車子旁邊

控制電機是l298n 但是無法用9v的方型電池催動只能用6個1.5v的電池催動

請問板上大大都是用哪種電壓的電池?

目前遇到的難關就這幾個

1.香蕉電機不會做平衡校正

2.電池的電壓不足供應給電機做校正

3.PID角度不太懂如何抓

4.MPU6050如何對晶片+香蕉電機做校正?

5.是MPU6050控制重心還是要以現實重心讓車子平衡?

雖然網路上有別人做好的晶片我還是想用手邊的mpu6050

我的mpu6050型號是GY-521

之前看蠻多大大好像加了很多東西才讓東西立起來

目前我手邊只有 MPU6050+L298N的東西之後要用藍芽控制前後左右

程式碼已經規劃好希望各位大大可以給些建議或解答幫忙小弟渡過難關

先謝謝大大

anduony · 发表于 2016-2-2 08:54:14

小弟最近好不容易求到了可以讓平衡車子站起來的程式碼
#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角进行卡尔曼滤波
float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt);
float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], dt);
//更新本次测的时间
nLastTime = nCurTime;
//PID控制
nCurTime = millis(); // 当前时间(ms)
float TimeCh = (nCurTime - nLastTime) / 1000.0; // 采样时间(s)
float Kp = 3.5, Ki = 0.0, Kd = 0.0; // 比例系数、积分系数、微分系数
float SampleTime = 0.1; // 采样时间(s)
float Setpoint = 0; // 设定目标值(degree)
float outMin = -255, outMax = +255; // 输出上限、输出下限
if (TimeCh >= SampleTime) { // 到达预定采样时间时
float Input = fNewPitch; // 输入赋值
float error = Setpoint - Input; // 偏差值
ITerm += (Ki * error * TimeCh); // 计算积分项
ITerm = constrain(ITerm, outMin, outMax); // 限定值域
float DTerm = Kd * (Input - lastInput) / TimeCh; // 计算微分项
Output = Kp * error + ITerm - DTerm; // 计算输出值
Output = constrain(Output, outMin, outMax); // 限定值域
//servoL.write(Output + servoL_offset); // 控制左驱
//servoR.write(Output + servoR_offset); // 控制右驱
lastInput = Input; // 记录输入值
nLastTime = nCurTime; // 记录本次时间
}
if (Serial.available() > 0) {
char rece = Serial.read();
delay(10);
if (rece == (char)'#')
{
Serial.println("Nano is Activity...");
}
flag = rece;
}
if (flag == (char)'R')
{
Serial.print("Roll");
Serial.print(fNewRoll);
Serial.print(")\t");
Serial.print("itch");
Serial.print(fNewPitch);
Serial.print(")\r\n");
}
if(Output > 0){
analogWrite(ENA,Output);
digitalWrite(IN1,HIGH);digitalWrite(IN2,LOW);
analogWrite(ENB,Output);
digitalWrite(IN3,HIGH);digitalWrite(IN4,LOW);
}
else{analogWrite(ENA,Output);
digitalWrite(IN1,LOW);digitalWrite(IN2,HIGH);
analogWrite(ENB,Output);
digitalWrite(IN3,LOW);digitalWrite(IN4,HIGH);
}
}
void ReadAccGyr(int16_t *pVals)
{
accelgyro.getMotion6(&pVals[0], &pVals[1], &pVals[2], &pVals[3], &pVals[4], &pVals[5]);
}
//对大量读数进行统计，校准平均偏移量
void Calibration()
{
int16_t valSums[6] = {0};
//先求和
for (int i = 0; i < nCalibTimes; ++i)
{
ReadAccGyr(valSums);
}
//再求平均
for (int i = 0; i < 6; ++i)
{
calibData[i] = int(valSums[i] / nCalibTimes);
}
calibData[2] += 16384; //设芯片Z轴竖直向下，设定静态工作点。
}
//对读数进行纠正，消除偏移，并转换为物理量。公式见文档。
void Rectify(int *pReadout, float *pRealVals)
{
for (int i = 0; i < 3; ++i)
{
pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 16384.0f;
}
for (int i = 3; i < 6; ++i)
{
pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 131.0f;
}
}
//算得Roll角。算法见文档。
float GetRoll(float *pRealVals)
{
float fNorm = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]);
float fCos = fNormXZ / fNorm;
if (pRealVals[1] > 0)
{
return -acos(fCos) * fRad2Deg;
}
else
return acos(fCos) * fRad2Deg;
}
//算得Pitch角。算法见文档。
float GetPitch(float *pRealVals)
{
float fNorm = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
float fNormYZ = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
float fCos = fNormYZ / fNorm;
if (pRealVals[0] < 0)
{
return -acos(fCos) * fRad2Deg;
}
else
return acos(fCos) * fRad2Deg;
}
void AngleUpdate(float *pRealVals)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
if (pRealVals[0]*pRealVals[1]*pRealVals[2] == 0)
return;
norm = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
vx = 2 * (q1 * q3 - q0 * q2);
vy = 2 * (q0 * q1 - q3 * q2);
vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
ex = pRealVals[1] / norm * vz - pRealVals[2] / norm * vy;
ey = pRealVals[2] / norm * vx - pRealVals[0] / norm * vz;
ez = pRealVals[0] / norm * vy - pRealVals[1] / norm * vx;
exInt = exInt + ex * KI;
eyInt = eyInt + ey * KI;
ezInt = ezInt + ez * KI;
q0 = q0 + (-q1 * pRealVals[3] + KP * ex + exInt - q2 * pRealVals[4] + KP * ey + eyInt - q3 * pRealVals[5] + KP * ez + ezInt) * halfT;
q1 = q1 + (q0 * pRealVals[3] + KP * ex + exInt + q2 * pRealVals[5] + KP * ez + ezInt - q3 * pRealVals[4] + KP * ey + eyInt) * halfT;
q2 = q2 + (q0 * pRealVals[4] + KP * ey + eyInt - q1 * pRealVals[5] + KP * ez + ezInt + q3 * pRealVals[3] + KP * ex + exInt) * halfT;
q3 = q3 + (q0 * pRealVals[5] + KP * ez + ezInt + q1 * pRealVals[4] + KP * ey + eyInt - q2 * pRealVals[3] + KP * ex + exInt) * halfT;
norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
Angle_roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * fRad2Deg;
Angle_pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * fRad2Deg;
Angle_yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * fRad2Deg;
}

复制代码

lieak59 · 发表于 2016-2-4 13:11:13

//servoL.write(Output + servoL_offset);

//servoR.write(Output + servoR_offset);

請問這兩個程式碼要做用它才會做平衡?

還是要加啥下去才會做平衡?

剛開始只是測角度後來馬達沒有自己做校正

請問它是要調哪裡才會自己做校正呢?

這兩行前面去掉會有錯

我當初拿到這程式碼只有說調整角度可式調整完之後電機輪子沒辦法自己做平衡

電壓絕對夠可是不清楚做用

有大大可以開導一下?

帐号		自动登录	找回密码
密码			注册