關於gyro_accel文字庫驅動
如題 小弟最近再測試別人的程式碼 可是在找庫的時候發現了個問題#include <Wire.h>
#include "C:\Program Files\Arduino\libraries\gyro_accel.h"
#include "PID_v1.h"
// Defining constants
#define dt 1 // time difference in milli seconds
#define rad2degree 57.3// radian to degree conversion
#define Filter_gain 0.95// e.g. angle = angle_gyro*Filter_gain + angle_accel*(1-Filter_gain)
// Global Variables
unsigned long t = 0; // Time Variables
float angle_x_gyro=0, angle_y_gyro=0, angle_z_gyro=0, angle_x_accel=0, angle_y_accel=0, angle_z_accel=0, angle_x=0, angle_y=0, angle_z=0;
char val ;
int maichong,lefta,leftaa,leftone,righta,rightaa,rightone;
// DC motor driver with L298N
const int motor1PWMPin = 11; // PWM Pin of Motor 1
const int motor1Polarity1 = 10; // Polarity 1 Pin of Motor 1
const int motor1Polarity2 = 9;// Polarity 2 Pin of Motor 1
const int motor2PWMPin = 6;// PWM Pin of Motor 2
const int motor2Polarity1 = 8; // Polarity 1 Pin of Motor 2
const int motor2Polarity2 = 7;// Polarity 2 Pin of Motor 2
const int left=12; //码盘
const int right=5;//码盘
int ValM1 = 255;// Initial Value for PWM Motor 1
int ValM2 = 255;// Initial Value for PWM Motor 2
double Setpoint, Input, Output;
//PID myPID(&Input, &Output, &Setpoint, 1, 0, -10, DIRECT);
PID myPID(&Input, &Output, &Setpoint, 25, 4, -5.5, DIRECT);
void setup(){
// MPU-6050
Serial.begin(115200);
Wire.begin();
MPU6050_ResetWake();
MPU6050_SetGains(0,1);// Setting the lows scale
MPU6050_SetDLPF(0); // Setting the DLPF to inf Bandwidth for calibration
MPU6050_OffsetCal();// very important
MPU6050_SetDLPF(6);// Setting the DLPF to lowest Bandwidth
t = millis();
// DC motor
pinMode(motor1PWMPin, OUTPUT);
pinMode(motor1Polarity1, OUTPUT);
pinMode(motor1Polarity2, OUTPUT);
pinMode(motor2PWMPin, OUTPUT);
pinMode(motor2Polarity1, OUTPUT);
pinMode(motor2Polarity2, OUTPUT);
//0 1 zhongduan
attachInterrupt(left, jishu1, CHANGE);
attachInterrupt(right, jishu, CHANGE);
// set enablePin of motor 1 high so that motor 1 can turn on
digitalWrite(motor1PWMPin, HIGH);
digitalWrite(motor1Polarity1, HIGH);
digitalWrite(motor1Polarity2, LOW);
// set enablePin of motor 2 high so that motor 2 can turn on
digitalWrite(motor2PWMPin, HIGH);
digitalWrite(motor2Polarity1, HIGH);
digitalWrite(motor2Polarity2, LOW);
Input = 0.0;
Setpoint = 10.0;
//myPID.SetSampleTime(100）;
myPID.SetMode(AUTOMATIC);
}
void jishu()
{
lefta++;
if(lefta==45)
{
lefta=0;//mai chong biao zhi qing ling
leftone++;//一圈
}
}
void jishu1()
{
righta++;
if(righta==45)
{
righta=0;//mai chong biao zhi qing ling
rightone++;//一圈
}
}
void rightz()//右轮正转
{
digitalWrite(motor1PWMPin, HIGH);digitalWrite(motor1Polarity1, HIGH);digitalWrite(motor1Polarity2, LOW);
}
void rightf()//右轮反转
{
digitalWrite(motor1PWMPin, HIGH);digitalWrite(motor1Polarity1, LOW);digitalWrite(motor1Polarity2, HIGH);
}
void leftz()
{
digitalWrite(motor2PWMPin, HIGH);digitalWrite(motor2Polarity1, HIGH);digitalWrite(motor2Polarity2, LOW);
}
void leftf()
{
digitalWrite(motor2PWMPin, HIGH);digitalWrite(motor2Polarity1, LOW);digitalWrite(motor2Polarity2, HIGH);
}
void st()//电机停
{
digitalWrite(motor1PWMPin, HIGH);digitalWrite(motor1Polarity1, LOW);digitalWrite(motor1Polarity2, LOW);
digitalWrite(motor2PWMPin, HIGH);digitalWrite(motor2Polarity1, LOW);digitalWrite(motor2Polarity2, LOW);
}
void blue()//蓝牙
{
if (Serial.available() > 0) {
val = Serial.read();
if(val == 'A') {
Serial.println(angle_x); //jiaodu
}
if(val == 'z') {
rightz();
}
if(val == 'x') {
rightf();
}
if(val == 'c') {
leftz();
}
if(val == 'v') {
leftf();
}
if(val == 'b') {
st();
}
}
}
void loop(){
t = millis();
MPU6050_ReadData();
angle_x_gyro = (gyro_x_scalled*((float)dt/1000)+angle_x);
angle_y_gyro = (gyro_y_scalled*((float)dt/1000)+angle_y);
angle_z_gyro = (gyro_z_scalled*((float)dt/1000)+angle_z);
angle_z_accel = atan(accel_z_scalled/(sqrt(accel_y_scalled*accel_y_scalled+accel_x_scalled*accel_x_scalled)))*(float)rad2degree;
angle_y_accel = -atan(accel_x_scalled/(sqrt(accel_y_scalled*accel_y_scalled+accel_z_scalled*accel_z_scalled)))*(float)rad2degree;
angle_x_accel = atan(accel_y_scalled/(sqrt(accel_x_scalled*accel_x_scalled+accel_z_scalled*accel_z_scalled)))*(float)rad2degree;
angle_x = Filter_gain*angle_x_gyro+(1-Filter_gain)*angle_x_accel;
angle_y = Filter_gain*angle_y_gyro+(1-Filter_gain)*angle_y_accel;
angle_z = Filter_gain*angle_z_gyro+(1-Filter_gain)*angle_z_accel;
digitalWrite(motor1PWMPin, HIGH);
digitalWrite(motor2PWMPin, HIGH);
Serial.print("\n");Serial.print(" ");Serial.print("angle_x=");Serial.print(angle_x);Serial.print("");Serial.print("angle_y=");Serial.print(angle_y);Serial.print("");Serial.print("angle_z=");Serial.print(angle_z);
Serial.print("____"); //串口输出
// change direction is very important 平衡
if(angle_x>0)
{
myPID.SetControllerDirection(REVERSE);
// set enablePin of motor 1 high so that motor 1 can turn on
digitalWrite(motor1Polarity1, HIGH);
digitalWrite(motor1Polarity2, LOW);
// set enablePin of motor 2 high so that motor 2 can turn on
digitalWrite(motor2Polarity1, HIGH);
digitalWrite(motor2Polarity2, LOW);
}
else
{
myPID.SetControllerDirection(DIRECT);
// set enablePin of motor 1 high so that motor 1 can turn on
digitalWrite(motor1Polarity1, LOW);
digitalWrite(motor1Polarity2, HIGH);
// set enablePin of motor 2 high so that motor 2 can turn on
digitalWrite(motor2Polarity1, LOW);
digitalWrite(motor2Polarity2, HIGH);
}
Input = angle_x;
Serial.print("Input=");Serial.print(Input);Serial.print("____");
myPID.Compute();
Serial.print("Output=");Serial.print(Output);Serial.print("____"); //Output 怎么会是0？？？？
analogWrite(motor1PWMPin, Output);
analogWrite(motor2PWMPin, Output);
Serial.print("Setpoint=");Serial.print(Setpoint); Serial.print("\t");
Serial.print("left=");Serial.print(lefta);Serial.print("");Serial.print(leftone); Serial.print("\t");
Serial.print("right=");Serial.print(righta);Serial.print("");Serial.print(rightone); Serial.print("\t"); //串口输出
blue();//蓝牙
if(rightone==1) //转动1圈标志
{
rightone=0;
st(); // 停止转动
}
//while((millis()-t) < dt)
{
// Do nothing
}
}
這是那位平衡小車大能做出來的程式碼 小弟最近裝了碼盤想測來看看是否能作用...
PID_v1.h 我已經有了....
至於gyro_accel我去載了GitHub的gyro_accel的代碼
由於沒辦法呼叫 所以就強行呼叫發生了個很麻煩的問題....
好像少了定義
C:\DOCUME~1\pig\LOCALS~1\Temp\build6679638715162739097.tmp\blance_car4.cpp.o: In function `setup':
C:\Program Files\Arduino/blance_car4.ino:36: undefined reference to `MPU6050_ResetWake()'
C:\Program Files\Arduino/blance_car4.ino:37: undefined reference to `MPU6050_SetGains(int, int)'
C:\Program Files\Arduino/blance_car4.ino:38: undefined reference to `MPU6050_SetDLPF(int)'
C:\Program Files\Arduino/blance_car4.ino:39: undefined reference to `MPU6050_OffsetCal()'
C:\Program Files\Arduino/blance_car4.ino:40: undefined reference to `MPU6050_SetDLPF(int)'
C:\DOCUME~1\pig\LOCALS~1\Temp\build6679638715162739097.tmp\blance_car4.cpp.o: In function `loop':
C:\Program Files\Arduino/blance_car4.ino:140: undefined reference to `MPU6050_ReadData()'
C:\Program Files\Arduino/blance_car4.ino:142: undefined reference to `gyro_x_scalled'
C:\Program Files\Arduino/blance_car4.ino:142: undefined reference to `gyro_x_scalled'
C:\Program Files\Arduino/blance_car4.ino:142: undefined reference to `gyro_x_scalled'
C:\Program Files\Arduino/blance_car4.ino:142: undefined reference to `gyro_x_scalled'
C:\Program Files\Arduino/blance_car4.ino:143: undefined reference to `gyro_y_scalled'
C:\Program Files\Arduino/blance_car4.ino:143: undefined reference to `gyro_y_scalled'
C:\Program Files\Arduino/blance_car4.ino:143: undefined reference to `gyro_y_scalled'
C:\Program Files\Arduino/blance_car4.ino:143: undefined reference to `gyro_y_scalled'
C:\Program Files\Arduino/blance_car4.ino:144: undefined reference to `gyro_z_scalled'
C:\Program Files\Arduino/blance_car4.ino:144: undefined reference to `gyro_z_scalled'
C:\Program Files\Arduino/blance_car4.ino:144: undefined reference to `gyro_z_scalled'
C:\Program Files\Arduino/blance_car4.ino:144: undefined reference to `gyro_z_scalled'
C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_z_scalled'
C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_z_scalled'
C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_z_scalled'
C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_z_scalled'
C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_y_scalled'
C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_y_scalled'
C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_y_scalled'
C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_y_scalled'
C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_x_scalled'
C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_x_scalled'
C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_x_scalled'
C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_x_scalled'
collect2.exe: error: ld returned 1 exit status
這是錯誤的訊息
我特別打開了gyro_accel.h和gyro_accel.cpp的程式碼
gyro_accel.h:
#ifndef gyro_accel.h
#define gyro_accel.h
extern int accel_x_OC, accel_y_OC, accel_z_OC, gyro_x_OC ,gyro_y_OC, gyro_z_OC; // offset variables
extern float temp_scalled,accel_x_scalled,accel_y_scalled,accel_z_scalled,gyro_x_scalled,gyro_y_scalled,gyro_z_scalled; //Scalled Data varaibles
void MPU6050_ReadData();
void MPU6050_ResetWake();
void MPU6050_SetDLPF(int BW);
void MPU6050_SetGains(int gyro,int accel);
void MPU6050_OffsetCal();
#endif
/* Author = helscream (Omer Ikram ul Haq)
Last edit date = 2014-06-22
Website: http://hobbylogs.me.pn/?p=47
Location: Pakistan
Ver: 0.1 beta --- Start
*/
gyro_accel.cpp:
#include <Arduino.h>
#include <Wire.h>
#include "gyro_accel.h"
// Defining the important registers
#define MPU6050_address 0x68
// ------ SELF Test Trim Factors ----------------------
#define MPU6050_self_test_x 13 // R/W
#define MPU6050_self_test_y 14 // R/W
#define MPU6050_self_test_z 15 // R/W
#define MPU6050_self_test_A 16 // R/W
// ----- Sample Divider -------------------------------
#define MPU6050_sample_div 25 // R/W
/* Sample Divider Discription
Sample rate = 8/(1 + Sample rate divider) if DLPF is disabled
Sample rate = 1/(1 + Sample rate divider) if DLPF is enabled
*/
// ----- Configration ---------------------------------
#define MPU6050_config 26 // R/W
#define MPU6050_gyro_config 27 // R/W
#define MPU6050_accel_config 28 // R/W
// ----- Data ---------------------------------------------
#define MPU6050_data_start 59
// ----- Power Management ---------------------------------
#define MPU6050_PWR1 107
#define MPU6050_PWR2 108
// ----- Defining Constant --------------------------------
#define g 9.81 // Gravitational accelration
// Variables
int temp=0, accel_x=0, accel_y=0, accel_z=0, gyro_x=0, gyro_y=0, gyro_z=0; // Raw values varaibles
int accel_x_OC=0, accel_y_OC=0, accel_z_OC=0, gyro_x_OC=0 ,gyro_y_OC=0, gyro_z_OC=0; // offset variables
float temp_scalled,accel_x_scalled,accel_y_scalled,accel_z_scalled,gyro_x_scalled,gyro_y_scalled,gyro_z_scalled; //Scalled Data varaibles
float accel_scale_fact = 1, gyro_scale_fact = 1; // Scale factor variables
// **************************************************************************
// Functions for MPU6050
// **************************************************************************
void MPU6050_ReadData(){
Wire.beginTransmission(MPU6050_address);
Wire.write(MPU6050_data_start);
Wire.endTransmission();
int read_bytes = 14;
Wire.requestFrom(MPU6050_address,read_bytes);
if(Wire.available() == read_bytes){
accel_x = Wire.read()<<8 | Wire.read();
accel_y = Wire.read()<<8 | Wire.read();
accel_z = Wire.read()<<8 | Wire.read();
temp = Wire.read()<<8 | Wire.read();
gyro_x = Wire.read()<<8 | Wire.read();
gyro_y = Wire.read()<<8 | Wire.read();
gyro_z = Wire.read()<<8 | Wire.read();
}
accel_x_scalled = (float)(accel_x-accel_x_OC)*accel_scale_fact/1000; // divided by 1000 as the Scale factor is in milli units
accel_y_scalled = (float)(accel_y-accel_y_OC)*accel_scale_fact/1000;
accel_z_scalled = (float)(accel_z-accel_z_OC)*accel_scale_fact/1000;
gyro_x_scalled = (float)(gyro_x-gyro_x_OC)*gyro_scale_fact/1000;
gyro_y_scalled = (float)(gyro_y-gyro_y_OC)*gyro_scale_fact/1000;
gyro_z_scalled = ((float)(gyro_z-gyro_z_OC)*gyro_scale_fact/1000);
temp_scalled = (float)temp/340+36.53;
}
// ------------------------------------------------
// Reset and wake up the MPU6050
// ------------------------------------------------
void MPU6050_ResetWake(){
Serial.println("Resetting MPU6050 and waking it up.....");
Wire.beginTransmission(MPU6050_address);
Wire.write(MPU6050_PWR1);
Wire.write(0b10000000);
Wire.endTransmission();
delay(100); // Waiting for the reset to complete
Wire.beginTransmission(MPU6050_address);
Wire.write(MPU6050_PWR1);
Wire.write(0b00000000);
Wire.endTransmission();
}
// ------------------------------------------------
// Setting up the DLFP
// ------------------------------------------------
void MPU6050_SetDLPF(int BW)
{
if (BW < 0 || BW > 6){
BW = 0;
}
Wire.beginTransmission(MPU6050_address);
Wire.write(MPU6050_config); // Address to the configuration register
/* config Discription ---- x x 0 0 0 F2 F1 F0
I am only intrested in the Digital Low Pass Filter (DLPF)
F2 F1 F0 Bandwidth
000
001 184
010 94
011 44
100 21
101 10
110 5
*/
Wire.write(BW);
Wire.endTransmission();
}
// ------------------------------------------------
// Setting up the Accelrometer and Gyro Gains
// ------------------------------------------------
void MPU6050_SetGains(int gyro,int accel)
{
byte gyro_byte,accel_byte;
// Setting up Gyro
Wire.beginTransmission(MPU6050_address);
Wire.write(MPU6050_gyro_config); // Address to the configuration register
if (gyro==0)
{
gyro_scale_fact =(float)250*0.0305; // each data is of 16 bits that means, 250 is divided along 2^(15)-1 = 32767 so for milli degree/s 0.0305 = 1000/32767
gyro_byte = 0b00000000;
}else if (gyro == 1)
{
gyro_scale_fact = 500*0.0305; // each data is of 16 bits that means, 500 is divided along 2^(15)-1 = 32767 so for milli degree/s 0.0305 = 1000/32767
gyro_byte = 0b00001000;
}else if (gyro == 2)
{
gyro_scale_fact = 1000*0.0305;// each data is of 16 bits that means, 1000 is divided along 2^(15)-1 = 32767 so for milli degree/s 0.0305 = 1000/32767
gyro_byte = 0b00010000;
}else if (gyro == 3)
{
gyro_scale_fact = 2000*0.0305;// each data is of 16 bits that means, 2000 is divided along 2^(15)-1 = 32767 so for milli degree/s 0.0305 = 1000/32767
gyro_byte = 0b00011000;
}else
{
gyro_scale_fact = 1;
}
Wire.write(gyro_byte);
Wire.endTransmission();
Serial.print("The gyro scale is set to ");
Serial.print(gyro_scale_fact);
Serial.println(" milli Degree/s");
// Setting up Accel
Wire.beginTransmission(MPU6050_address);
Wire.write(MPU6050_accel_config); // Address to the configuration register
if (accel==0)
{
accel_scale_fact =(float)2*g*0.0305; // each data is of 16 bits that means, 2g is divided along 2^(15)-1 = 32767 so for milli m/s^2 0.0305 = 1000/32767
accel_byte = 0b00000000;
}else if (accel == 1)
{
accel_scale_fact = 4*g*0.0305; // each data is of 16 bits that means, 4g is divided along 2^(15)-1 = 32767 so for milli m/s^2 0.0305 = 1000/32767
accel_byte = 0b00001000;
}else if (accel == 2)
{
accel_scale_fact = 8*g*0.0305;// each data is of 16 bits that means, 8g is divided along 2^(15)-1 = 32767 so for milli m/s^2 0.0305 = 1000/32767
accel_byte = 0b00010000;
}else if (accel == 3)
{
accel_scale_fact = 16*g*0.0305; // each data is of 16 bits that means, 16g is divided along 2^(15)-1 = 32767 so for milli m/s^2 0.0305 = 1000/32767
accel_byte = 0b00011000;
}else
{
accel_scale_fact = 1;
}
Wire.write(accel_byte);
Wire.endTransmission();
Serial.print("The accel scale is set to ");
Serial.print(accel_scale_fact);
Serial.println(" milli m/s^2");
}
// ------------------------------------------------
// offset calibration
// ------------------------------------------------
void MPU6050_OffsetCal(){
Serial.println("Calibrating gyroscope .... dont move the hardware ..........");
int x=0,y=0,z=0,i;
MPU6050_ReadData();
MPU6050_ReadData();
// Gyro Offset Calculation
x=gyro_x;
y=gyro_y;
z=gyro_z;
for (i=1;i<=1000;i++){
MPU6050_ReadData();
x=(x+gyro_x)/2;
y=(y+gyro_y)/2;
z=(z+gyro_z)/2;
Serial.print(".");
}
Serial.println(".");
gyro_x_OC=x;
gyro_y_OC=y;
gyro_z_OC=z;
Serial.print("gyro_x register offset = ");
Serial.println(x);
Serial.print("gyro_y register offect = ");
Serial.println(y);
Serial.print("gyro_z register offset = ");
Serial.println(z);
// Accel Offset Calculation
Serial.println("Calibrating accelrometer .... dont move the hardware ..........");
x=accel_x;
y=accel_y;
z=accel_z;
for (i=1;i<=1000;i++){
MPU6050_ReadData();
x=(x+accel_x)/2;
y=(y+accel_y)/2;
z=(z+accel_z)/2;
Serial.print(".");
}
Serial.println(".");
accel_x_OC=x;
accel_y_OC=y;
accel_z_OC=z-(float)g*1000/accel_scale_fact;
Serial.print("Accel_x register offset = ");
Serial.println(x);
Serial.print("Accel_y register offect = ");
Serial.println(y);
Serial.print("Accel_z register offset = ");
Serial.println(z);
}
/* Author = helscream (Omer Ikram ul Haq)
Last edit date = 2014-06-22
Website: http://hobbylogs.me.pn/?p=47
Location: Pakistan
Ver: 0.1 beta --- Start
*/
雖然複雜 還想請問一下版上 有沒有大能還有其它的gyro_accel.h文字庫 小弟找遍了都沒看到有...
不然gyro_accel.cpp 或gyro_accel.h 程式碼 需要加哪些字 要填到直到可以動為止
目前看一些應該有的東西都有在裡面 可是好像少了啥麼 跟那些少了的算法跟程式碼
怕沒寫好會出事情 麻煩請大人指點讓小弟我完成這程式碼....強行呼叫也是我從朋友那學的
可惜朋友不會寫文字庫
希望大大能伸出援手讓小弟完成他 就算給我gyro_accel.h檔 我也會想辦法做些比較
畢竟我這邊光mpu6050.h寫法就有10幾種寫法 多少都有研究
看完只有"崇拜"兩個字可以形容
雖然有很多大同小異
可是都各有風格...算法最強的就是整個數學系的數學....
小弟束手無策了 麻煩各位大大解惑
页:
[1]