關於平衡車檢測角度數據程式碼
本帖最后由 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; //校准数据
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;
ReadAccGyr(readouts); //读出测量值
float realVals;
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, dt);
float fNewPitch = kalmanPitch.getAngle(fPitch, realVals, 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, &pVals, &pVals, &pVals, &pVals, &pVals);
}
//对大量读数进行统计，校准平均偏移量
void Calibration()
{
int16_t valSums = {0};
//先求和
for (int i = 0; i < nCalibTimes; ++i)
{
ReadAccGyr(valSums);
}
//再求平均
for (int i = 0; i < 6; ++i)
{
calibData = int(valSums / nCalibTimes);
}
calibData += 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 * pRealVals + pRealVals * pRealVals + pRealVals * pRealVals);
float fNormXZ = sqrt(pRealVals * pRealVals + pRealVals * pRealVals);
float fCos = fNormXZ / fNorm;
if (pRealVals > 0)
{
return -acos(fCos) * fRad2Deg;
}
else
return acos(fCos) * fRad2Deg;
}
//算得Pitch角。算法见文档。
float GetPitch(float *pRealVals)
{
float fNorm = sqrt(pRealVals * pRealVals + pRealVals * pRealVals + pRealVals * pRealVals);
float fNormYZ = sqrt(pRealVals * pRealVals + pRealVals * pRealVals);
float fCos = fNormYZ / fNorm;
if (pRealVals < 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*pRealVals*pRealVals == 0)
return;
norm = sqrt(pRealVals * pRealVals + pRealVals * pRealVals + pRealVals * pRealVals);
vx = 2 * (q1 * q3 - q0 * q2);
vy = 2 * (q0 * q1 - q3 * q2);
vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
ex = pRealVals / norm * vz - pRealVals / norm * vy;
ey = pRealVals / norm * vx - pRealVals / norm * vz;
ez = pRealVals / norm * vy - pRealVals / norm * vx;
exInt = exInt + ex * KI;
eyInt = eyInt + ey * KI;
ezInt = ezInt + ez * KI;
q0 = q0 + (-q1 * pRealVals + KP * ex + exInt - q2 * pRealVals + KP * ey + eyInt - q3 * pRealVals + KP * ez + ezInt) * halfT;
q1 = q1 + (q0 * pRealVals + KP * ex + exInt + q2 * pRealVals + KP * ez + ezInt - q3 * pRealVals + KP * ey + eyInt) * halfT;
q2 = q2 + (q0 * pRealVals + KP * ey + eyInt - q1 * pRealVals + KP * ez + ezInt + q3 * pRealVals + KP * ex + exInt) * halfT;
q3 = q3 + (q0 * pRealVals + KP * ez + ezInt + q1 * pRealVals + KP * ey + eyInt - q2 * pRealVals + 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的東西 之後要用藍芽控制前後左右
程式碼已經規劃好 希望各位大大可以給些建議或解答幫忙小弟渡過難關
先謝謝大大
小弟最近好不容易求到了可以讓平衡車子站起來的程式碼
#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; //校准数据
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;
ReadAccGyr(readouts); //读出测量值
float realVals;
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, dt);
float fNewPitch = kalmanPitch.getAngle(fPitch, realVals, 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, &pVals, &pVals, &pVals, &pVals, &pVals);
}
//对大量读数进行统计，校准平均偏移量
void Calibration()
{
int16_t valSums = {0};
//先求和
for (int i = 0; i < nCalibTimes; ++i)
{
ReadAccGyr(valSums);
}
//再求平均
for (int i = 0; i < 6; ++i)
{
calibData = int(valSums / nCalibTimes);
}
calibData += 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 * pRealVals + pRealVals * pRealVals + pRealVals * pRealVals);
float fNormXZ = sqrt(pRealVals * pRealVals + pRealVals * pRealVals);
float fCos = fNormXZ / fNorm;
if (pRealVals > 0)
{
return -acos(fCos) * fRad2Deg;
}
else
return acos(fCos) * fRad2Deg;
}
//算得Pitch角。算法见文档。
float GetPitch(float *pRealVals)
{
float fNorm = sqrt(pRealVals * pRealVals + pRealVals * pRealVals + pRealVals * pRealVals);
float fNormYZ = sqrt(pRealVals * pRealVals + pRealVals * pRealVals);
float fCos = fNormYZ / fNorm;
if (pRealVals < 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*pRealVals*pRealVals == 0)
return;
norm = sqrt(pRealVals * pRealVals + pRealVals * pRealVals + pRealVals * pRealVals);
vx = 2 * (q1 * q3 - q0 * q2);
vy = 2 * (q0 * q1 - q3 * q2);
vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
ex = pRealVals / norm * vz - pRealVals / norm * vy;
ey = pRealVals / norm * vx - pRealVals / norm * vz;
ez = pRealVals / norm * vy - pRealVals / norm * vx;
exInt = exInt + ex * KI;
eyInt = eyInt + ey * KI;
ezInt = ezInt + ez * KI;
q0 = q0 + (-q1 * pRealVals + KP * ex + exInt - q2 * pRealVals + KP * ey + eyInt - q3 * pRealVals + KP * ez + ezInt) * halfT;
q1 = q1 + (q0 * pRealVals + KP * ex + exInt + q2 * pRealVals + KP * ez + ezInt - q3 * pRealVals + KP * ey + eyInt) * halfT;
q2 = q2 + (q0 * pRealVals + KP * ey + eyInt - q1 * pRealVals + KP * ez + ezInt + q3 * pRealVals + KP * ex + exInt) * halfT;
q3 = q3 + (q0 * pRealVals + KP * ez + ezInt + q1 * pRealVals + KP * ey + eyInt - q2 * pRealVals + 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;
} //servoL.write(Output + servoL_offset);
//servoR.write(Output + servoR_offset);
請問這兩個程式碼要做用它才會做平衡?
還是要加啥下去才會做平衡?
剛開始只是測角度 後來馬達沒有自己做校正
請問它是要調哪裡才會自己做校正呢?
這兩行前面去掉會有錯
我當初拿到這程式碼 只有說調整角度 可式調整完之後電機輪子沒辦法自己做平衡
電壓絕對夠 可是不清楚做用
有大大可以開導一下?
页:
[1]