API tools faq
paste
Advertisement
Guest User

Originalen kod

a guest
Jul 16th, 2018
103
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 9.41 KB | None | 0 0
raw download clone embed print report
  1. /*
  2. ===Contact & Support===
  3. Website: http://eeenthusiast.com/
  4. Youtube: https://www.youtube.com/EEEnthusiast
  5. Facebook: https://www.facebook.com/EEEnthusiast/
  6. Patreon: https://www.patreon.com/EE_Enthusiast
  7. Revision: 1.0 (July 13th, 2016)
  8.  
  9. ===Hardware===
  10. - Arduino Uno R3
  11. - MPU-6050 (Available from: http://eeenthusiast.com/product/6dof-mpu-6050-accelerometer-gyroscope-temperature/)
  12.  
  13. ===Software===
  14. - Latest Software: https://github.com/VRomanov89/EEEnthusiast/tree/master/MPU-6050%20Implementation/MPU6050_Implementation
  15. - Arduino IDE v1.6.9
  16. - Arduino Wire library
  17.  
  18. ===Terms of use===
  19. The software is provided by EEEnthusiast without warranty of any kind. In no event shall the authors or
  20. copyright holders be liable for any claim, damages or other liability, whether in an action of contract,
  21. tort or otherwise, arising from, out of or in connection with the software or the use or other dealings in
  22. the software.
  23. */
  24.  
  25. // I2Cdev and MPU6050 must be installed as libraries
  26. #include "I2Cdev.h"
  27. #include "MPU6050.h"
  28. #include <Wire.h>
  29.  
  30.  
  31. /////////////////////////////////// CONFIGURATION /////////////////////////////
  32. //Change this 3 variables if you want to fine tune the skecth to your needs.
  33. int buffersize=1000; //Amount of readings used to average, make it higher to get more precision but sketch will be slower (default:1000)
  34. int acel_deadzone=8; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge (default:8)
  35. int giro_deadzone=1; //Giro error allowed, make it lower to get more precision, but sketch may not converge (default:1)
  36.  
  37. // default I2C address is 0x68
  38. // specific I2C addresses may be passed as a parameter here
  39. // AD0 low = 0x68 (default for InvenSense evaluation board)
  40. // AD0 high = 0x69
  41. //MPU6050 accelgyro;
  42. MPU6050 accelgyro(0x68); // <-- use for AD0 high
  43.  
  44. int16_t ax, ay, az,gx, gy, gz;
  45.  
  46. int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
  47. int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;
  48. int ready=0;
  49.  
  50. long accelX, accelY, accelZ;
  51. float gForceX, gForceY, gForceZ;
  52.  
  53. long gyroX, gyroY, gyroZ;
  54. float rotX, rotY, rotZ;
  55.  
  56. void setup() {
  57.  
  58. // initialize digital pin LED_BUILTIN as an output.
  59. pinMode(LED_BUILTIN, OUTPUT);
  60.  
  61. // initialize serial communication
  62. Serial.begin(9600);
  63.  
  64. // join I2C bus (I2Cdev library doesn't do this automatically)
  65. Wire.begin();
  66.  
  67. // start message
  68. Serial.println("\nMPU6050 Calibration Sketch");
  69. delay(2000);
  70. Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
  71. delay(3000);
  72.  
  73. // verify connection
  74. Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  75. delay(1000);
  76.  
  77. // reset offsets
  78. accelgyro.setXAccelOffset(0);
  79. accelgyro.setYAccelOffset(0);
  80. accelgyro.setZAccelOffset(0);
  81. accelgyro.setXGyroOffset(0);
  82. accelgyro.setYGyroOffset(0);
  83. accelgyro.setZGyroOffset(0);
  84.  
  85. setupMPU();
  86.  
  87. if (state==0){
  88. Serial.println("\nReading sensors for first time...");
  89. meansensors();
  90. state++;
  91. delay(1000);
  92. }
  93.  
  94. if (state==1) {
  95. Serial.println("\nCalculating offsets...");
  96. calibration();
  97. state++;
  98. delay(1000);
  99. }
  100.  
  101. if (state==2) {
  102. meansensors();
  103. Serial.println("\nFINISHED!");
  104. Serial.print("\nSensor readings with offsets:\t");
  105. Serial.print(mean_ax);
  106. Serial.print("\t");
  107. Serial.print(mean_ay);
  108. Serial.print("\t");
  109. Serial.print(mean_az);
  110. Serial.print("\t");
  111. Serial.print(mean_gx);
  112. Serial.print("\t");
  113. Serial.print(mean_gy);
  114. Serial.print("\t");
  115. Serial.println(mean_gz);
  116. Serial.print("Your offsets:\t");
  117. Serial.print(ax_offset);
  118. Serial.print("\t");
  119. Serial.print(ay_offset);
  120. Serial.print("\t");
  121. Serial.print(az_offset);
  122. Serial.print("\t");
  123. Serial.print(gx_offset);
  124. Serial.print("\t");
  125. Serial.print(gy_offset);
  126. Serial.print("\t");
  127. Serial.println(gz_offset);
  128. Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
  129. Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
  130. Serial.println(" ");
  131. }
  132.  
  133. }
  134.  
  135.  
  136. void loop() {
  137. recordAccelRegisters();
  138. recordGyroRegisters();
  139. printData();
  140. delay(100);
  141. }
  142.  
  143.  
  144. /////////////////////////////////// FUNCTIONS ////////////////////////////////////
  145.  
  146. void meansensors(){
  147. long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;
  148.  
  149. while (i<(buffersize+101)){
  150. // read raw accel/gyro measurements from device
  151. accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  152.  
  153. if (i>100 && i<=(buffersize+100)){ //First 100 measures are discarded
  154. buff_ax=buff_ax+ax;
  155. buff_ay=buff_ay+ay;
  156. buff_az=buff_az+az;
  157. buff_gx=buff_gx+gx;
  158. buff_gy=buff_gy+gy;
  159. buff_gz=buff_gz+gz;
  160. }
  161. if (i==(buffersize+100)){
  162. mean_ax=buff_ax/buffersize;
  163. mean_ay=buff_ay/buffersize;
  164. mean_az=buff_az/buffersize;
  165. mean_gx=buff_gx/buffersize;
  166. mean_gy=buff_gy/buffersize;
  167. mean_gz=buff_gz/buffersize;
  168. }
  169. i++;
  170. delay(2); //Needed so we don't get repeated measures
  171. }
  172. }
  173.  
  174. void calibration(){
  175. ax_offset=-mean_ax/8;
  176. ay_offset=-mean_ay/8;
  177. az_offset=(8192-mean_az)/8;
  178.  
  179. gx_offset=-mean_gx/4;
  180. gy_offset=-mean_gy/4;
  181. gz_offset=-mean_gz/4;
  182. while (ready<6){
  183. int ready=0;
  184. accelgyro.setXAccelOffset(ax_offset);
  185. accelgyro.setYAccelOffset(ay_offset);
  186. accelgyro.setZAccelOffset(az_offset);
  187.  
  188. accelgyro.setXGyroOffset(gx_offset);
  189. accelgyro.setYGyroOffset(gy_offset);
  190. accelgyro.setZGyroOffset(gz_offset);
  191.  
  192. meansensors();
  193. Serial.println("...");
  194.  
  195. if (abs(mean_ax)<=acel_deadzone) ready++;
  196. else ax_offset=ax_offset-mean_ax/acel_deadzone;
  197.  
  198. if (abs(mean_ay)<=acel_deadzone) ready++;
  199. else ay_offset=ay_offset-mean_ay/acel_deadzone;
  200.  
  201. if (abs(8192-mean_az)<=acel_deadzone) ready++;
  202. else az_offset=az_offset+(8192-mean_az)/acel_deadzone;
  203.  
  204. if (abs(mean_gx)<=giro_deadzone) ready++;
  205. else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);
  206.  
  207. if (abs(mean_gy)<=giro_deadzone) ready++;
  208. else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);
  209.  
  210. if (abs(mean_gz)<=giro_deadzone) ready++;
  211. else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);
  212.  
  213. if (ready==6) break;
  214. }
  215. }
  216.  
  217. void setupMPU(){
  218. Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
  219. Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
  220. Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
  221. Wire.endTransmission();
  222. Wire.beginTransmission(0b1101000); //I2C address of the MPU
  223. Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
  224. Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s
  225. Wire.endTransmission();
  226. Wire.beginTransmission(0b1101000); //I2C address of the MPU
  227. Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
  228. Wire.write(0b00001000); //Setting the accel to +/- 4g
  229. Wire.endTransmission();
  230. }
  231.  
  232. void recordAccelRegisters() {
  233. Wire.beginTransmission(0b1101000); //I2C address of the MPU
  234. Wire.write(0x3B); //Starting register for Accel Readings
  235. Wire.endTransmission();
  236. Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
  237. while(Wire.available() < 6);
  238. accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  239. accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  240. accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  241. processAccelData();
  242. }
  243.  
  244. void processAccelData(){
  245. gForceX = accelX / 8192.0;
  246. gForceY = accelY / 8192.0;
  247. gForceZ = accelZ / 8192.0;
  248. }
  249.  
  250. void recordGyroRegisters() {
  251. Wire.beginTransmission(0b1101000); //I2C address of the MPU
  252. Wire.write(0x43); //Starting register for Gyro Readings
  253. Wire.endTransmission();
  254. Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
  255. while(Wire.available() < 6);
  256. gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  257. gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  258. gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  259. processGyroData();
  260. }
  261.  
  262. void processGyroData() {
  263. rotX = gyroX / 131.0;
  264. rotY = gyroY / 131.0;
  265. rotZ = gyroZ / 131.0;
  266. }
  267.  
  268. void printData() {
  269. Serial.print("Gyro (deg)");
  270. Serial.print(" X=");
  271. Serial.print(rotX);
  272. if (rotX>=100){
  273. blinkLED();
  274. }
  275. if (rotX<=-90){
  276. blinkLED();
  277. }
  278. Serial.print(" Y=");
  279. Serial.print(rotY);
  280. if (rotY>=90){
  281. blinkLED();
  282. }
  283. if (rotY<=-90){
  284. blinkLED();
  285. }
  286. Serial.print(" Z=");
  287. Serial.print(rotZ);
  288. Serial.print(" Accel (g)");
  289. Serial.print(" X=");
  290. Serial.print(gForceX);
  291. if (gForceX>=0.75){
  292. blinkLED();
  293. }
  294. if (gForceX<=-0.75){
  295. blinkLED();
  296. }
  297. Serial.print(" Y=");
  298. Serial.print(gForceY);
  299. if (gForceY>=0.75){
  300. blinkLED();
  301. }
  302. if (gForceY<=-0.75){
  303. blinkLED();
  304. }
  305. Serial.print(" Z=");
  306. Serial.println(gForceZ);
  307. if (gForceZ>=3.5){
  308. blinkLED();
  309. }
  310. }
  311. void blinkLED(){
  312. digitalWrite(LED_BUILTIN, HIGH);
  313. delay(5000);
  314. digitalWrite(LED_BUILTIN, LOW);
  315. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!