關於gyro_accel文字庫驅動

lieak59

如題 小弟最近再測試別人的程式碼 可是在找庫的時候發現了個問題

1. 
   
2. #include <Wire.h>
   
3. #include "C:\Program Files\Arduino\libraries\gyro_accel.h"
   
4. #include "PID_v1.h"
   
5. 
   
6. // Defining constants
   
7. #define dt 1            // time difference in milli seconds
   
8. #define rad2degree 57.3  // radian to degree conversion
   
9. #define Filter_gain 0.95  // e.g. angle = angle_gyro*Filter_gain + angle_accel*(1-Filter_gain)
   
10. 
    
11. // Global Variables
    
12. unsigned long t = 0;      // Time Variables
    
13. 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;
    
14. char val ;
    
15. int maichong,lefta,leftaa,leftone,righta,rightaa,rightone;
    
16. // DC motor driver with L298N
    
17. const int motor1PWMPin = 11; // PWM Pin of Motor 1
    
18. const int motor1Polarity1 = 10; // Polarity 1 Pin of Motor 1
    
19. const int motor1Polarity2 = 9;  // Polarity 2 Pin of Motor 1
    
20. const int motor2PWMPin = 6;  // PWM Pin of Motor 2
    
21. const int motor2Polarity1 = 8; // Polarity 1 Pin of Motor 2
    
22. const int motor2Polarity2 = 7;  // Polarity 2 Pin of Motor 2
    
23. const int left=12; //码盘
    
24. const int right=5;//码盘
    
25. int ValM1 = 255;  // Initial Value for PWM Motor 1
    
26. int ValM2 = 255;  // Initial Value for PWM Motor 2
    
27. 
    
28. double Setpoint, Input, Output;
    
29. 
    
30. //PID myPID(&Input, &Output, &Setpoint, 1, 0, -10, DIRECT);
    
31. PID myPID(&Input, &Output, &Setpoint, 25, 4, -5.5, DIRECT);
    
32. 
    
33. void setup(){
    
34.   // MPU-6050
    
35.   Serial.begin(115200);
    
36.   Wire.begin();
    
37.   MPU6050_ResetWake();
    
38.   MPU6050_SetGains(0,1);  // Setting the lows scale
    
39.   MPU6050_SetDLPF(0);    // Setting the DLPF to inf Bandwidth for calibration
    
40.   MPU6050_OffsetCal();  // very important
    
41.   MPU6050_SetDLPF(6);  // Setting the DLPF to lowest Bandwidth
    
42.    
    
43.   t = millis();
    
44.    
    
45.   // DC motor
    
46.   pinMode(motor1PWMPin, OUTPUT);
    
47.   pinMode(motor1Polarity1, OUTPUT);
    
48.   pinMode(motor1Polarity2, OUTPUT);
    
49.    
    
50.   pinMode(motor2PWMPin, OUTPUT);
    
51.   pinMode(motor2Polarity1, OUTPUT);
    
52.   pinMode(motor2Polarity2, OUTPUT);
    
53.   //0 1 zhongduan
    
54.   attachInterrupt(left, jishu1, CHANGE);
    
55.   attachInterrupt(right, jishu, CHANGE);
    
56.   
    
57.    
    
58.   // set enablePin of motor 1 high so that motor 1 can turn on
    
59.   digitalWrite(motor1PWMPin, HIGH);
    
60.   digitalWrite(motor1Polarity1, HIGH);
    
61.   digitalWrite(motor1Polarity2, LOW);
    
62. 
    
63.   // set enablePin of motor 2 high so that motor 2 can turn on  
    
64.   digitalWrite(motor2PWMPin, HIGH);
    
65.   digitalWrite(motor2Polarity1, HIGH);
    
66.   digitalWrite(motor2Polarity2, LOW);
    
67.    
    
68.   Input = 0.0;
    
69.   Setpoint = 10.0;
    
70.   
    
71.   //myPID.SetSampleTime(100）;
    
72.   myPID.SetMode(AUTOMATIC);
    
73.   
    
74. }
    
75. void jishu()
    
76. {  
    
77.     lefta++;
    
78.     if(lefta==45)
    
79.     {  
    
80.       lefta=0;  //mai chong biao zhi qing ling
    
81.       leftone++;//一圈
    
82.     }  
    
83. }
    
84. void jishu1()
    
85. {  
    
86.     righta++;
    
87.     if(righta==45)
    
88.     {  
    
89.       righta=0;  //mai chong biao zhi qing ling
    
90.       rightone++;//一圈
    
91.     }  
    
92. }
    
93. void rightz()//右轮正转
    
94. {
    
95.   digitalWrite(motor1PWMPin, HIGH);digitalWrite(motor1Polarity1, HIGH);digitalWrite(motor1Polarity2, LOW);
    
96. }
    
97. void rightf()//右轮反转
    
98. {
    
99.   digitalWrite(motor1PWMPin, HIGH);digitalWrite(motor1Polarity1, LOW);digitalWrite(motor1Polarity2, HIGH);
    
100. }
     
101. void leftz()
     
102. {
     
103.   digitalWrite(motor2PWMPin, HIGH);digitalWrite(motor2Polarity1, HIGH);digitalWrite(motor2Polarity2, LOW);
     
104. }
     
105. void leftf()
     
106. {
     
107.   digitalWrite(motor2PWMPin, HIGH);digitalWrite(motor2Polarity1, LOW);digitalWrite(motor2Polarity2, HIGH);
     
108. }
     
109. void st()//电机停
     
110. {
     
111.   digitalWrite(motor1PWMPin, HIGH);digitalWrite(motor1Polarity1, LOW);digitalWrite(motor1Polarity2, LOW);
     
112.   digitalWrite(motor2PWMPin, HIGH);digitalWrite(motor2Polarity1, LOW);digitalWrite(motor2Polarity2, LOW);
     
113. }
     
114. void blue()//蓝牙
     
115. {
     
116.   if (Serial.available() > 0) {
     
117.       val = Serial.read();
     
118.      if(val == 'A') {
     
119.         Serial.println(angle_x); //jiaodu
     
120.       }
     
121.       if(val == 'z') {
     
122.        rightz();
     
123.       }
     
124.       if(val == 'x') {
     
125.       rightf();
     
126.       }
     
127.       if(val == 'c') {
     
128.        leftz();
     
129.       }
     
130.       if(val == 'v') {
     
131.        leftf();
     
132.       }
     
133.       if(val == 'b') {
     
134.        st();
     
135.       }
     
136. }
     
137. }
     
138. void loop(){
     
139.   t = millis();
     
140.   
     
141.   MPU6050_ReadData();
     
142.    
     
143.   angle_x_gyro = (gyro_x_scalled*((float)dt/1000)+angle_x);
     
144.   angle_y_gyro = (gyro_y_scalled*((float)dt/1000)+angle_y);
     
145.   angle_z_gyro = (gyro_z_scalled*((float)dt/1000)+angle_z);
     
146. 
     
147.   angle_z_accel = atan(accel_z_scalled/(sqrt(accel_y_scalled*accel_y_scalled+accel_x_scalled*accel_x_scalled)))*(float)rad2degree;
     
148.   angle_y_accel = -atan(accel_x_scalled/(sqrt(accel_y_scalled*accel_y_scalled+accel_z_scalled*accel_z_scalled)))*(float)rad2degree;
     
149.   angle_x_accel = atan(accel_y_scalled/(sqrt(accel_x_scalled*accel_x_scalled+accel_z_scalled*accel_z_scalled)))*(float)rad2degree;
     
150. 
     
151.   angle_x = Filter_gain*angle_x_gyro+(1-Filter_gain)*angle_x_accel;
     
152.   angle_y = Filter_gain*angle_y_gyro+(1-Filter_gain)*angle_y_accel;
     
153.   angle_z = Filter_gain*angle_z_gyro+(1-Filter_gain)*angle_z_accel;
     
154.    
     
155.   digitalWrite(motor1PWMPin, HIGH);
     
156.   digitalWrite(motor2PWMPin, HIGH);
     
157. 
     
158. 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);
     
159. Serial.print("____");           //串口输出
     
160.    
     
161.   // change direction is very important   平衡
     
162.   if(angle_x>0)
     
163.   {
     
164.     myPID.SetControllerDirection(REVERSE);
     
165.      
     
166.     // set enablePin of motor 1 high so that motor 1 can turn on
     
167.     digitalWrite(motor1Polarity1, HIGH);
     
168.     digitalWrite(motor1Polarity2, LOW);
     
169. 
     
170.     // set enablePin of motor 2 high so that motor 2 can turn on  
     
171.     digitalWrite(motor2Polarity1, HIGH);
     
172.     digitalWrite(motor2Polarity2, LOW);
     
173.   }
     
174.   else
     
175.   {
     
176.    myPID.SetControllerDirection(DIRECT);
     
177.     // set enablePin of motor 1 high so that motor 1 can turn on
     
178.     digitalWrite(motor1Polarity1, LOW);
     
179.     digitalWrite(motor1Polarity2, HIGH);
     
180. 
     
181.     // set enablePin of motor 2 high so that motor 2 can turn on  
     
182.     digitalWrite(motor2Polarity1, LOW);
     
183.     digitalWrite(motor2Polarity2, HIGH);
     
184.   }
     
185.    
     
186.   Input = angle_x;
     
187.   Serial.print("Input=");Serial.print(Input);Serial.print("____");
     
188.    
     
189.   myPID.Compute();
     
190.   Serial.print("Output=");Serial.print(Output);Serial.print("____");                        //Output 怎么会是0？？？？
     
191.    
     
192.   analogWrite(motor1PWMPin, Output);
     
193.   analogWrite(motor2PWMPin, Output);
     
194.   
     
195.   Serial.print("Setpoint=");Serial.print(Setpoint); Serial.print("\t");
     
196.   Serial.print("left=");Serial.print(lefta);Serial.print("  ");Serial.print(leftone); Serial.print("\t");
     
197.   Serial.print("right=");Serial.print(righta);Serial.print("  ");Serial.print(rightone); Serial.print("\t");   //串口输出
     
198.    blue();//蓝牙
     
199.    
     
200.    if(rightone==1) //转动1圈标志
     
201.        {
     
202.          rightone=0;   
     
203.          st();   // 停止转动
     
204.        }
     
205.       
     
206.    
     
207. //while((millis()-t) < dt)
     
208.   {
     
209.     // Do nothing
     
210.   }
     
211. }

复制代码

這是那位平衡小車大能做出來的程式碼 小弟最近裝了碼盤想測來看看是否能作用...

PID_v1.h 我已經有了....
至於gyro_accel我去載了GitHub的gyro_accel的代碼
由於沒辦法呼叫 所以就強行呼叫發生了個很麻煩的問題....

好像少了定義

1. 
   
2. C:\DOCUME~1\pig\LOCALS~1\Temp\build6679638715162739097.tmp\blance_car4.cpp.o: In function `setup':
   
3. C:\Program Files\Arduino/blance_car4.ino:36: undefined reference to `MPU6050_ResetWake()'
   
4. C:\Program Files\Arduino/blance_car4.ino:37: undefined reference to `MPU6050_SetGains(int, int)'
   
5. C:\Program Files\Arduino/blance_car4.ino:38: undefined reference to `MPU6050_SetDLPF(int)'
   
6. C:\Program Files\Arduino/blance_car4.ino:39: undefined reference to `MPU6050_OffsetCal()'
   
7. C:\Program Files\Arduino/blance_car4.ino:40: undefined reference to `MPU6050_SetDLPF(int)'
   
8. C:\DOCUME~1\pig\LOCALS~1\Temp\build6679638715162739097.tmp\blance_car4.cpp.o: In function `loop':
   
9. C:\Program Files\Arduino/blance_car4.ino:140: undefined reference to `MPU6050_ReadData()'
   
10. C:\Program Files\Arduino/blance_car4.ino:142: undefined reference to `gyro_x_scalled'
    
11. C:\Program Files\Arduino/blance_car4.ino:142: undefined reference to `gyro_x_scalled'
    
12. C:\Program Files\Arduino/blance_car4.ino:142: undefined reference to `gyro_x_scalled'
    
13. C:\Program Files\Arduino/blance_car4.ino:142: undefined reference to `gyro_x_scalled'
    
14. C:\Program Files\Arduino/blance_car4.ino:143: undefined reference to `gyro_y_scalled'
    
15. C:\Program Files\Arduino/blance_car4.ino:143: undefined reference to `gyro_y_scalled'
    
16. C:\Program Files\Arduino/blance_car4.ino:143: undefined reference to `gyro_y_scalled'
    
17. C:\Program Files\Arduino/blance_car4.ino:143: undefined reference to `gyro_y_scalled'
    
18. C:\Program Files\Arduino/blance_car4.ino:144: undefined reference to `gyro_z_scalled'
    
19. C:\Program Files\Arduino/blance_car4.ino:144: undefined reference to `gyro_z_scalled'
    
20. C:\Program Files\Arduino/blance_car4.ino:144: undefined reference to `gyro_z_scalled'
    
21. C:\Program Files\Arduino/blance_car4.ino:144: undefined reference to `gyro_z_scalled'
    
22. C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_z_scalled'
    
23. C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_z_scalled'
    
24. C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_z_scalled'
    
25. C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_z_scalled'
    
26. C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_y_scalled'
    
27. C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_y_scalled'
    
28. C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_y_scalled'
    
29. C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_y_scalled'
    
30. C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_x_scalled'
    
31. C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_x_scalled'
    
32. C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_x_scalled'
    
33. C:\Program Files\Arduino/blance_car4.ino:146: undefined reference to `accel_x_scalled'
    
34. collect2.exe: error: ld returned 1 exit status
    

复制代码

這是錯誤的訊息

我特別打開了gyro_accel.h和gyro_accel.cpp的程式碼

gyro_accel.h:

1. 
   
2. #ifndef gyro_accel.h
   
3.   #define gyro_accel.h
   
4.   
   
5.   extern int accel_x_OC, accel_y_OC, accel_z_OC, gyro_x_OC ,gyro_y_OC, gyro_z_OC; // offset variables
   
6.   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
   
7. 
   
8.   
   
9.   void MPU6050_ReadData();
   
10.   void MPU6050_ResetWake();
    
11.   void MPU6050_SetDLPF(int BW);
    
12.   void MPU6050_SetGains(int gyro,int accel);
    
13.   void MPU6050_OffsetCal();
    
14. 
    
15. #endif
    
16. 
    
17. /* Author = helscream (Omer Ikram ul Haq)
    
18. Last edit date = 2014-06-22
    
19. Website: [url]http://hobbylogs.me.pn/?p=47[/url]
    
20. Location: Pakistan
    
21. Ver: 0.1 beta --- Start
    
22. */
    

复制代码

gyro_accel.cpp:

1. 
   
2. #include <Arduino.h>
   
3. #include <Wire.h>
   
4. #include "gyro_accel.h"
   
5. 
   
6. 
   
7. // Defining the important registers
   
8. 
   
9. #define MPU6050_address 0x68
   
10. 
    
11. // ------ SELF Test Trim Factors ----------------------
    
12. #define MPU6050_self_test_x 13       // R/W
    
13. #define MPU6050_self_test_y 14       // R/W
    
14. #define MPU6050_self_test_z 15       // R/W
    
15. #define MPU6050_self_test_A 16       // R/W
    
16. 
    
17. // ----- Sample Divider -------------------------------
    
18. #define MPU6050_sample_div 25        // R/W
    
19. /*       Sample Divider Discription
    
20.   Sample rate = 8/(1 + Sample rate divider) [kHz]  if DLPF is disabled
    
21.   Sample rate = 1/(1 + Sample rate divider) [kHz]  if DLPF is enabled
    
22. */
    
23. 
    
24. // ----- Configration ---------------------------------
    
25. #define MPU6050_config 26            // R/W
    
26. #define MPU6050_gyro_config 27       // R/W
    
27. #define MPU6050_accel_config 28      // R/W
    
28. 
    
29. // ----- Data ---------------------------------------------
    
30. #define MPU6050_data_start 59
    
31. 
    
32. 
    
33. 
    
34. // ----- Power Management ---------------------------------
    
35. #define MPU6050_PWR1 107
    
36. #define MPU6050_PWR2 108
    
37. 
    
38. // ----- Defining Constant --------------------------------
    
39. #define g 9.81                       // Gravitational accelration
    
40. 
    
41. 
    
42. 
    
43. 
    
44. // Variables
    
45. int temp=0, accel_x=0, accel_y=0, accel_z=0, gyro_x=0, gyro_y=0, gyro_z=0; // Raw values varaibles
    
46. 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
    
47. float temp_scalled,accel_x_scalled,accel_y_scalled,accel_z_scalled,gyro_x_scalled,gyro_y_scalled,gyro_z_scalled; //Scalled Data varaibles
    
48. float accel_scale_fact = 1, gyro_scale_fact = 1; // Scale factor variables
    
49. 
    
50. 
    
51. // **************************************************************************
    
52. //          Functions for MPU6050
    
53. // **************************************************************************
    
54. 
    
55. 
    
56. void MPU6050_ReadData(){
    
57.   
    
58.   
    
59.   Wire.beginTransmission(MPU6050_address);
    
60.   Wire.write(MPU6050_data_start);
    
61.   Wire.endTransmission();
    
62. 
    
63.   int read_bytes = 14;
    
64. 
    
65.   Wire.requestFrom(MPU6050_address,read_bytes);
    
66. 
    
67.   if(Wire.available() == read_bytes){
    
68.   
    
69.     accel_x = Wire.read()<<8 | Wire.read();
    
70.     accel_y = Wire.read()<<8 | Wire.read();
    
71.     accel_z = Wire.read()<<8 | Wire.read();
    
72.   
    
73.     temp = Wire.read()<<8 | Wire.read();
    
74.   
    
75.     gyro_x = Wire.read()<<8 | Wire.read();
    
76.     gyro_y = Wire.read()<<8 | Wire.read();
    
77.     gyro_z = Wire.read()<<8 | Wire.read();
    
78.   
    
79.   }
    
80.   
    
81.   
    
82.   
    
83.   accel_x_scalled = (float)(accel_x-accel_x_OC)*accel_scale_fact/1000; // divided by 1000 as the Scale factor is in milli units
    
84.   accel_y_scalled = (float)(accel_y-accel_y_OC)*accel_scale_fact/1000;
    
85.   accel_z_scalled = (float)(accel_z-accel_z_OC)*accel_scale_fact/1000;
    
86.   
    
87.   gyro_x_scalled = (float)(gyro_x-gyro_x_OC)*gyro_scale_fact/1000;
    
88.   gyro_y_scalled = (float)(gyro_y-gyro_y_OC)*gyro_scale_fact/1000;
    
89.   gyro_z_scalled = ((float)(gyro_z-gyro_z_OC)*gyro_scale_fact/1000);
    
90.   
    
91.   temp_scalled = (float)temp/340+36.53;
    
92. }
    
93. 
    
94. 
    
95. 
    
96. // ------------------------------------------------
    
97. //       Reset and wake up the MPU6050
    
98. // ------------------------------------------------
    
99. void MPU6050_ResetWake(){
    
100.   
     
101.   Serial.println("Resetting MPU6050 and waking it up.....");
     
102.   Wire.beginTransmission(MPU6050_address);
     
103.   Wire.write(MPU6050_PWR1);
     
104.   Wire.write(0b10000000);
     
105.   Wire.endTransmission();
     
106.   
     
107.   delay(100); // Waiting for the reset to complete
     
108.    
     
109.   Wire.beginTransmission(MPU6050_address);
     
110.   Wire.write(MPU6050_PWR1);
     
111. 
     
112.   Wire.write(0b00000000);
     
113.   Wire.endTransmission();
     
114.   
     
115. }
     
116. 
     
117. 
     
118. 
     
119. // ------------------------------------------------
     
120. //       Setting up the DLFP
     
121. // ------------------------------------------------
     
122. 
     
123. void MPU6050_SetDLPF(int BW)
     
124.   {
     
125.     if (BW < 0 || BW > 6){
     
126.       BW = 0;
     
127.     }
     
128.   Wire.beginTransmission(MPU6050_address);
     
129.   Wire.write(MPU6050_config); // Address to the configuration register
     
130. /*       config Discription ---- x x 0 0 0 F2 F1 F0
     
131.   I am only intrested in the Digital Low Pass Filter (DLPF)
     
132.   F2 F1 F0    Bandwidth [Hz]
     
133.   0  0  0        
     
134.   0  0  1      184
     
135.   0  1  0      94
     
136.   0  1  1      44
     
137.   1  0  0      21  
     
138.   1  0  1      10  
     
139.   1  1  0      5
     
140. */
     
141. 
     
142.   Wire.write(BW);
     
143.   Wire.endTransmission();
     
144.   }
     
145.   
     
146. // ------------------------------------------------
     
147. //       Setting up the Accelrometer and Gyro Gains
     
148. // ------------------------------------------------  
     
149. 
     
150. void MPU6050_SetGains(int gyro,int accel)
     
151.   {
     
152.     byte gyro_byte,accel_byte;
     
153.    
     
154.     // Setting up Gyro
     
155.     Wire.beginTransmission(MPU6050_address);
     
156.     Wire.write(MPU6050_gyro_config); // Address to the configuration register
     
157.     if (gyro==0)
     
158.     {
     
159.       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
     
160.       gyro_byte = 0b00000000;
     
161.     }else if (gyro == 1)
     
162.     {
     
163.       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
     
164.       gyro_byte = 0b00001000;
     
165.     }else if (gyro == 2)
     
166.     {
     
167.       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
     
168.       gyro_byte = 0b00010000;
     
169.     }else if (gyro == 3)
     
170.     {
     
171.       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
     
172.       gyro_byte = 0b00011000;
     
173.     }else
     
174.     {
     
175.       gyro_scale_fact = 1;
     
176.     }  
     
177.       
     
178.     Wire.write(gyro_byte);
     
179.     Wire.endTransmission();
     
180.     Serial.print("The gyro scale is set to ");
     
181.     Serial.print(gyro_scale_fact);
     
182.     Serial.println(" milli Degree/s");
     
183.    
     
184.    
     
185.     // Setting up Accel
     
186.     Wire.beginTransmission(MPU6050_address);
     
187.     Wire.write(MPU6050_accel_config); // Address to the configuration register
     
188.     if (accel==0)
     
189.     {
     
190.       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      
     
191.       accel_byte = 0b00000000;
     
192.     }else if (accel == 1)
     
193.     {
     
194.       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
     
195.       accel_byte = 0b00001000;
     
196.     }else if (accel == 2)
     
197.     {
     
198.       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
     
199.       accel_byte = 0b00010000;
     
200.     }else if (accel == 3)
     
201.     {
     
202.       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
     
203.       accel_byte = 0b00011000;
     
204.     }else
     
205.     {
     
206.       accel_scale_fact = 1;
     
207.     }  
     
208.       
     
209.     Wire.write(accel_byte);
     
210.     Wire.endTransmission();
     
211.     Serial.print("The accel scale is set to ");
     
212.     Serial.print(accel_scale_fact);
     
213.     Serial.println(" milli m/s^2");
     
214.    
     
215.   }
     
216.   
     
217.   
     
218. // ------------------------------------------------
     
219. //       offset calibration
     
220. // ------------------------------------------------  
     
221. 
     
222. void MPU6050_OffsetCal(){
     
223.   Serial.println("Calibrating gyroscope .... dont move the hardware ..........");
     
224.   
     
225.   int x=0,y=0,z=0,i;
     
226.   
     
227.   MPU6050_ReadData();
     
228.   MPU6050_ReadData();
     
229.   
     
230.   // Gyro Offset Calculation
     
231.   x=gyro_x;
     
232.   y=gyro_y;
     
233.   z=gyro_z;
     
234.    
     
235.   for (i=1;i<=1000;i++){
     
236.     MPU6050_ReadData();
     
237.     x=(x+gyro_x)/2;
     
238.     y=(y+gyro_y)/2;
     
239.     z=(z+gyro_z)/2;
     
240.     Serial.print(".");
     
241.   }
     
242.   Serial.println(".");
     
243.   gyro_x_OC=x;
     
244.   gyro_y_OC=y;
     
245.   gyro_z_OC=z;
     
246.   
     
247.   Serial.print("gyro_x register offset = ");
     
248.   Serial.println(x);
     
249. 
     
250.   
     
251.   Serial.print("gyro_y register offect = ");
     
252.   Serial.println(y);
     
253. 
     
254.   
     
255.   Serial.print("gyro_z register offset = ");
     
256.   Serial.println(z);
     
257. 
     
258.   
     
259.   
     
260.   // Accel Offset Calculation
     
261.   Serial.println("Calibrating accelrometer .... dont move the hardware ..........");
     
262.   x=accel_x;
     
263.   y=accel_y;
     
264.   z=accel_z;
     
265.    
     
266.   for (i=1;i<=1000;i++){
     
267.     MPU6050_ReadData();
     
268.     x=(x+accel_x)/2;
     
269.     y=(y+accel_y)/2;
     
270.     z=(z+accel_z)/2;
     
271.     Serial.print(".");
     
272.   }
     
273.   Serial.println(".");
     
274.   accel_x_OC=x;
     
275.   accel_y_OC=y;
     
276.   accel_z_OC=z-(float)g*1000/accel_scale_fact;
     
277.   
     
278.   Serial.print("Accel_x register offset = ");
     
279.   Serial.println(x);
     
280.   
     
281.   
     
282.   Serial.print("Accel_y register offect = ");
     
283.   Serial.println(y);
     
284.   
     
285.   
     
286.   Serial.print("Accel_z register offset = ");
     
287.   Serial.println(z);
     
288.   
     
289.   
     
290.   
     
291. 
     
292. }
     
293. /* Author = helscream (Omer Ikram ul Haq)
     
294. Last edit date = 2014-06-22
     
295. Website: [url]http://hobbylogs.me.pn/?p=47[/url]
     
296. Location: Pakistan
     
297. Ver: 0.1 beta --- Start
     
298. */
     
299. 
     

复制代码

雖然複雜 還想請問一下版上 有沒有大能還有其它的gyro_accel.h文字庫 小弟找遍了都沒看到有...

不然gyro_accel.cpp 或gyro_accel.h 程式碼 需要加哪些字 要填到直到可以動為止

目前看一些應該有的東西都有在裡面 可是好像少了啥麼 跟那些少了的算法跟程式碼

怕沒寫好會出事情 麻煩請大人指點讓小弟我完成這程式碼....強行呼叫也是我從朋友那學的

可惜朋友不會寫文字庫

希望大大能伸出援手讓小弟完成他 就算給我gyro_accel.h檔 我也會想辦法做些比較

畢竟我這邊光mpu6050.h寫法就有10幾種寫法 多少都有研究

看完只有"崇拜"兩個字可以形容
雖然有很多大同小異
可是都各有風格...算法最強的就是整個數學系的數學....

小弟束手無策了 麻煩各位大大解惑