|
如題 小弟最近再測試別人的程式碼 可是在找庫的時候發現了個問題
- #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: [url]http://hobbylogs.me.pn/?p=47[/url]
- 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) [kHz] if DLPF is disabled
- Sample rate = 1/(1 + Sample rate divider) [kHz] 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 [Hz]
- 0 0 0
- 0 0 1 184
- 0 1 0 94
- 0 1 1 44
- 1 0 0 21
- 1 0 1 10
- 1 1 0 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: [url]http://hobbylogs.me.pn/?p=47[/url]
- Location: Pakistan
- Ver: 0.1 beta --- Start
- */
复制代码
雖然複雜 還想請問一下版上 有沒有大能還有其它的gyro_accel.h文字庫 小弟找遍了都沒看到有...
不然gyro_accel.cpp 或gyro_accel.h 程式碼 需要加哪些字 要填到直到可以動為止
目前看一些應該有的東西都有在裡面 可是好像少了啥麼 跟那些少了的算法跟程式碼
怕沒寫好會出事情 麻煩請大人指點讓小弟我完成這程式碼....強行呼叫也是我從朋友那學的
可惜朋友不會寫文字庫
希望大大能伸出援手讓小弟完成他 就算給我gyro_accel.h檔 我也會想辦法做些比較
畢竟我這邊光mpu6050.h寫法就有10幾種寫法 多少都有研究
看完只有"崇拜"兩個字可以形容
雖然有很多大同小異
可是都各有風格...算法最強的就是整個數學系的數學....
小弟束手無策了 麻煩各位大大解惑 |
|