API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jun 16th, 2019
83
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 38.35 KB | None | 0 0
raw download clone embed print report
  1. //imports Library
  2. #include <Wire.h>
  3. //sets LED as pin 13
  4. int LED = 10;
  5.  
  6. //sets addr as 0x76(I2C adress for barometer)
  7. #define Addr 0x76
  8.  
  9. //the variables for the barometer
  10. unsigned long Coff[6], Ti = 0, offi = 0, sensi = 0;
  11. unsigned int data[3];
  12.  
  13. //the variables for the barometer 2
  14. unsigned long Coff2[6], Ti2 = 0, offi2 = 0, sensi2 = 0;
  15. unsigned int data2[3];
  16.  
  17. //The variables for the gyroscope
  18. float elapsedTime, time, timePrev; // these are time
  19. int gyro_error =0;// used to calculate error
  20. float Gyro_rawX, Gyro_rawY, Gyro_rawZ; //the raw angle data
  21. float angleX, angleY;
  22. float Gyro_raw_errorX, Gyro_raw_errorY;
  23. float Total_angle_x, Total_angle_y;
  24.  
  25. //The variables for the gyroscope 2
  26.  
  27. int gyro_error2 =0;// used to calculate error
  28. float Gyro_rawX2, Gyro_rawY2, Gyro_rawZ2; //the raw angle data2
  29. float angleX2, angleY2;
  30. float Gyro_raw_errorX2, Gyro_raw_errorY2;
  31. float Total_angle_x2, Total_angle_y2;
  32.  
  33. //The variables for the acceleromter
  34. int accel_error=0;
  35. float rad_to_degree = 180/3.14159264;//If you have been in math class recently you should know this
  36. float Accel_rawX, Accel_rawY, Accel_rawZ; //The raw accelerometer data
  37. float Accel_angleX, Accel_angleY; //This is the angle data that you get from the accelerometers data
  38. float Accel_angle_errorX, Accel_angle_errorY;//This is
  39. float Total_angleX, Total_angleY;
  40. float prev_accelX = 1000;
  41.  
  42. //The variables for the acceleromter 2
  43. int accel_error2=0;
  44. float Accel_rawX2, Accel_rawY2, Accel_rawZ2; //The raw accelerometer data2
  45. float Accel_angleX2, Accel_angleY2; //This is the angle data2 that you get from the accelerometers data2
  46. float Accel_angle_errorX2, Accel_angle_errorY2;//This is
  47. float Total_angleX2, Total_angleY2;
  48. float prev_accelX2= 1000;
  49.  
  50. float height, height2;
  51. float hi, hi2, hi3, hi4;
  52. float pressure, pressure2;
  53. float pressure_at_sealevel = 1012.8; //mbar;
  54. float change;
  55.  
  56. //the variables for the barometer
  57. unsigned long Coff3[6], Ti3 = 0, offi3 = 0, sensi3 = 0;
  58. unsigned int data3[3];
  59.  
  60. //the variables for the barometer 2
  61. unsigned long Coff4[6], Ti4 = 0, offi4 = 0, sensi4 = 0;
  62. unsigned int data4[3];
  63.  
  64. //The variables for the gyroscope
  65.  
  66. int gyro_error3 =0;// used to calculate error
  67. float Gyro_rawX3, Gyro_rawY3, Gyro_rawZ3; //the raw angle data
  68. float angleX3, angleY3;
  69. float Gyro_raw_errorX3, Gyro_raw_errorY3;
  70. float Total_angle_x3, Total_angle_y3;
  71.  
  72. //The variables for the gyroscope 4
  73.  
  74. int gyro_error4 =0;// used to calculate error
  75. float Gyro_rawX4, Gyro_rawY4, Gyro_rawZ4; //the raw angle data4
  76. float angleX4, angleY4;
  77. float Gyro_raw_errorX4, Gyro_raw_errorY4;
  78. float Total_angle_x4, Total_angle_y4;
  79.  
  80. //The variables for the acceleromter
  81. int accel_error3=0;
  82. float Accel_rawX3, Accel_rawY3, Accel_rawZ3; //The raw accelerometer data
  83. float Accel_angleX3, Accel_angleY3; //This is the angle data that you get from the accelerometers data
  84. float Accel_angle_errorX3, Accel_angle_errorY3;//This is
  85. float Total_angleX3, Total_angleY3;
  86.  
  87. //The variables for the acceleromter 4
  88. int accel_error4=0;
  89. float Accel_rawX4, Accel_rawY4, Accel_rawZ4; //The raw accelerometer data4
  90. float Accel_angleX4, Accel_angleY4; //This is the angle data4 that you get from the accelerometers data4
  91. float Accel_angle_errorX4, Accel_angle_errorY4;//This is
  92. float Total_angleX4, Total_angleY4;
  93. float prev_accelX4= 1000;
  94.  
  95. float height3, height4;
  96. float h3, h4;
  97. float pressure3, pressure4;
  98.  
  99.  
  100. void setup(){
  101.  
  102. //starting up the I2C device
  103. Wire.begin();
  104.  
  105. Wire.beginTransmission(0x69);//This starts the transmission to the I2C device, while most things online will tell you that it's 0x68 it's not
  106. Wire.write(0x6B);
  107. Wire.write(0x00);//reset
  108. Wire.endTransmission(true);//end Transmission
  109.  
  110. //Gyroscope configuration
  111. Wire.beginTransmission(0x69);
  112. Wire.write(0x1B);
  113. Wire.write(0x10);//for the gyroscope use 0x10 for the accelerometer use 0x18
  114. Wire.endTransmission(true);
  115.  
  116. //Accelerometer configuration
  117. Wire.beginTransmission(0x69);
  118. Wire.write(0x1C);
  119. Wire.write(0x18);//this is +/- 16g if you want +/- 8g then use 0x10
  120. Wire.endTransmission(true);
  121.  
  122. //starting up the serial monitor
  123. Serial.begin(9600);//for this we are using a 9600 baud rate
  124. time = millis();
  125.  
  126. //This gets the data for the accelerometer
  127. if(accel_error==0)
  128. {
  129. for(int a=0; a<200; a++)
  130. {
  131. Wire.beginTransmission(0x69);
  132. Wire.write(0x3B); //acceleration in the X-axis
  133. Wire.endTransmission(false);
  134. Wire.requestFrom(0x69,6,true);
  135.  
  136. Accel_angle_errorX = Accel_angle_errorX +((atan((Accel_rawY)/sqrt(pow((Accel_rawX),2)+pow((Accel_rawZ),2)))*rad_to_degree));
  137. Accel_angle_errorY = Accel_angle_errorY +((atan(-1*(Accel_rawX)/sqrt(pow((Accel_rawY),2)+pow((Accel_rawZ),2)))*rad_to_degree));
  138.  
  139. if(a==199)
  140. {
  141. Accel_angle_errorX = Accel_angle_errorX/200;
  142. Accel_angle_errorY = Accel_angle_errorY/200;
  143. accel_error=1;
  144. }
  145. }
  146. }
  147.  
  148. if(accel_error2==0)
  149. {
  150. for(int a2=0; a2<200; a2++)
  151. {
  152. Wire.beginTransmission(0x69);
  153. Wire.write(0x3B); //acceleraTi2on in the X-axis
  154. Wire.endTransmission(false);
  155. Wire.requestFrom(0x69,6,true);
  156.  
  157. Accel_angle_errorX2= Accel_angle_errorX2+((atan((Accel_rawY2)/sqrt(pow((Accel_rawX2),2)+pow((Accel_rawZ2),2)))*rad_to_degree));
  158. Accel_angle_errorY2= Accel_angle_errorY2+((atan(-1*(Accel_rawX2)/sqrt(pow((Accel_rawY2),2)+pow((Accel_rawZ2),2)))*rad_to_degree));
  159.  
  160. if(a2==199)
  161. {
  162. Accel_angle_errorX2= Accel_angle_errorX2/200;
  163. Accel_angle_errorY2= Accel_angle_errorY2/200;
  164. accel_error2=1;
  165. }
  166. }
  167. }
  168.  
  169.  
  170. //This gets the data for the gyroscope
  171. if(gyro_error==0)
  172. {
  173. for(int i=0; i<200; i++)
  174. {
  175. Wire.beginTransmission(0x69); //begin, Send the slave adress (in this case 68)
  176. Wire.write(0x43); //First adress of the Gyro data
  177. Wire.endTransmission(false);
  178. Wire.requestFrom(0x69,4,true); //We ask for just 4 registers
  179.  
  180. Gyro_rawX=Wire.read()<<8|Wire.read(); //Once again we shif and sum
  181. Gyro_rawY=Wire.read()<<8|Wire.read();
  182.  
  183. /*---X---*/
  184. Gyro_raw_errorX = Gyro_raw_errorX + (Gyro_rawX/32.8);
  185. /*---Y---*/
  186. Gyro_raw_errorY = Gyro_raw_errorY + (Gyro_rawY/32.8);
  187.  
  188. if(i==199)
  189. {
  190. Gyro_raw_errorX = Gyro_raw_errorX/200;
  191. Gyro_raw_errorY = Gyro_raw_errorY/200;
  192. gyro_error=1;
  193. }
  194. }
  195. }
  196.  
  197. if(gyro_error2==0)
  198. {
  199. for(int i2=0; i2<200; i2++)
  200. {
  201. Wire.beginTransmission(0x69); //begin, Send the slave adress (in this case 68)
  202. Wire.write(0x43); //First adress of the Gyro data2
  203. Wire.endTransmission(false);
  204. Wire.requestFrom(0x69,4,true); //We ask for just 4 registers
  205.  
  206. Gyro_rawX2=Wire.read()<<8|Wire.read(); //Once again we shif and sum
  207. Gyro_rawY2=Wire.read()<<8|Wire.read();
  208.  
  209. /*---X---*/
  210. Gyro_raw_errorX2= Gyro_raw_errorX2+ (Gyro_rawX2/32.8);
  211. /*---Y---*/
  212. Gyro_raw_errorY2= Gyro_raw_errorY2+ (Gyro_rawY2/32.8);
  213.  
  214. if(i2==199)
  215. {
  216. Gyro_raw_errorX2= Gyro_raw_errorX2/200;
  217. Gyro_raw_errorY2= Gyro_raw_errorY2/200;
  218. gyro_error2=1;
  219. }
  220. }
  221. }
  222.  
  223. //This gets the data for the barometer (includes temperature and pressure)
  224. for(int i = 0; i < 6; i++)
  225. {
  226. // Start I2C Transmission
  227. Wire.beginTransmission(Addr);
  228. // Select data register
  229. Wire.write(0xA2 + (2 * i));
  230. // Stop I2C Transmission
  231. Wire.endTransmission();
  232.  
  233. // Request 2 bytes of data
  234. Wire.requestFrom(Addr, 2);
  235.  
  236. // Read 2 bytes of data
  237. // Coff msb, Coff lsb
  238. if(Wire.available() == 2)
  239. {
  240. data[0] = Wire.read();
  241. data[1] = Wire.read();
  242. }
  243.  
  244. // Convert the data
  245. Coff[i] = ((data[0] * 256) + data[1]);
  246. }
  247.  
  248. //This gets the data2 for the barometer (includes temperature and pressure)
  249. for(int i = 0; i < 6; i++)
  250. {
  251. // Start I2C Transmission
  252. Wire.beginTransmission(Addr);
  253. // Select data2 register
  254. Wire.write(0xA2 + (2 * i));
  255. // Stop I2C Transmission
  256. Wire.endTransmission();
  257.  
  258. // Request 2 bytes of data2
  259. Wire.requestFrom(Addr, 2);
  260.  
  261. // Read 2 bytes of data2
  262. // Coff2 msb, Coff2 lsb
  263. if(Wire.available() == 2)
  264. {
  265. data2[0] = Wire.read();
  266. data2[1] = Wire.read();
  267. }
  268.  
  269. // Convert the data2
  270. Coff2[i] = ((data2[0] * 256) + data2[1]);
  271. }
  272.  
  273. //This gets the data for the accelerometer
  274. if(accel_error3==0)
  275. {
  276. for(int a3=0; a3<200; a3++)
  277. {
  278. Wire.beginTransmission(0x69);
  279. Wire.write(0x3B); //acceleration in the X-axis
  280. Wire.endTransmission(false);
  281. Wire.requestFrom(0x69,6,true);
  282.  
  283. Accel_angle_errorX3= Accel_angle_errorX3+((atan((Accel_rawY3)/sqrt(pow((Accel_rawX3),2)+pow((Accel_rawZ3),2)))*rad_to_degree));
  284. Accel_angle_errorY3 = Accel_angle_errorY3 +((atan(-1*(Accel_rawX3)/sqrt(pow((Accel_rawY3),2)+pow((Accel_rawZ3),2)))*rad_to_degree));
  285.  
  286. if(a3==199)
  287. {
  288. Accel_angle_errorX3= Accel_angle_errorX3/200;
  289. Accel_angle_errorY3 = Accel_angle_errorY3/200;
  290. accel_error3=1;
  291. }
  292. }
  293. }
  294.  
  295. if(accel_error4==0)
  296. {
  297. for(int a4=0; a4<200; a4++)
  298. {
  299. Wire.beginTransmission(0x69);
  300. Wire.write(0x3B); //acceleraTi2on in the X-axis
  301. Wire.endTransmission(false);
  302. Wire.requestFrom(0x69,6,true);
  303.  
  304. Accel_angle_errorX4= Accel_angle_errorX4+((atan((Accel_rawY4)/sqrt(pow((Accel_rawX4),2)+pow((Accel_rawZ4),2)))*rad_to_degree));
  305. Accel_angle_errorY4= Accel_angle_errorY4+((atan(-1*(Accel_rawX4)/sqrt(pow((Accel_rawY4),2)+pow((Accel_rawZ4),2)))*rad_to_degree));
  306.  
  307. if(a4==199)
  308. {
  309. Accel_angle_errorX4= Accel_angle_errorX4/200;
  310. Accel_angle_errorY4= Accel_angle_errorY4/200;
  311. accel_error4=1;
  312. }
  313. }
  314. }
  315.  
  316.  
  317. //This gets the data for the gyroscope
  318. if(gyro_error3==0)
  319. {
  320. for(int i3=0; i3<200; i3++)
  321. {
  322. Wire.beginTransmission(0x69); //begin, Send the slave adress (in this case 68)
  323. Wire.write(0x43); //First adress of the Gyro data
  324. Wire.endTransmission(false);
  325. Wire.requestFrom(0x69,4,true); //We ask for just 4 registers
  326.  
  327. Gyro_rawX3=Wire.read()<<8|Wire.read(); //Once again we shif and sum
  328. Gyro_rawY3=Wire.read()<<8|Wire.read();
  329.  
  330. /*---X---*/
  331. Gyro_raw_errorX3= Gyro_raw_errorX3+ (Gyro_rawX3/32.8);
  332. /*---Y---*/
  333. Gyro_raw_errorY3 = Gyro_raw_errorY3 + (Gyro_rawY3/32.8);
  334.  
  335. if(i3==199)
  336. {
  337. Gyro_raw_errorX3= Gyro_raw_errorX3/200;
  338. Gyro_raw_errorY3 = Gyro_raw_errorY3/200;
  339. gyro_error3=1;
  340. }
  341. }
  342. }
  343.  
  344. if(gyro_error4==0)
  345. {
  346. for(int i4=0; i4<200; i4++)
  347. {
  348. Wire.beginTransmission(0x69); //begin, Send the slave adress (in this case 68)
  349. Wire.write(0x43); //First adress of the Gyro data4
  350. Wire.endTransmission(false);
  351. Wire.requestFrom(0x69,4,true); //We ask for just 4 registers
  352.  
  353. Gyro_rawX4=Wire.read()<<8|Wire.read(); //Once again we shif and sum
  354. Gyro_rawY4=Wire.read()<<8|Wire.read();
  355.  
  356. /*---X---*/
  357. Gyro_raw_errorX4= Gyro_raw_errorX4+ (Gyro_rawX4/32.8);
  358. /*---Y---*/
  359. Gyro_raw_errorY4= Gyro_raw_errorY4+ (Gyro_rawY4/32.8);
  360.  
  361. if(i4==199)
  362. {
  363. Gyro_raw_errorX4= Gyro_raw_errorX4/200;
  364. Gyro_raw_errorY4= Gyro_raw_errorY4/200;
  365. gyro_error4=1;
  366. }
  367. }
  368. }
  369.  
  370. //This gets the data for the barometer (includes temperature and pressure)
  371. for(int i3 = 0; i3 < 6; i3++)
  372. {
  373. // Start I2C Transmission
  374. Wire.beginTransmission(Addr);
  375. // Select data register
  376. Wire.write(0xA2 + (2 * i3));
  377. // Stop I2C Transmission
  378. Wire.endTransmission();
  379.  
  380. // Request 2 bytes of data
  381. Wire.requestFrom(Addr, 2);
  382.  
  383. // Read 2 bytes of data
  384. // Coff msb, Coff lsb
  385. if(Wire.available() == 2)
  386. {
  387. data3[0] = Wire.read();
  388. data3[1] = Wire.read();
  389. }
  390.  
  391. // Convert the data
  392. Coff3[i3] = ((data3[0] * 256) + data3[1]);
  393. }
  394.  
  395. //This gets the data2 for the barometer (includes temperature and pressure)
  396. for(int i3 = 0; i3 < 6; i3++)
  397. {
  398. // Start I2C Transmission
  399. Wire.beginTransmission(Addr);
  400. // Select data2 register
  401. Wire.write(0xA2 + (2 * i3));
  402. // Stop I2C Transmission
  403. Wire.endTransmission();
  404.  
  405. // Request 2 bytes of data2
  406. Wire.requestFrom(Addr, 2);
  407.  
  408. // Read 2 bytes of data2
  409. // Coff2 msb, Coff2 lsb
  410. if(Wire.available() == 2)
  411. {
  412. data4[0] = Wire.read();
  413. data4[1] = Wire.read();
  414. }
  415.  
  416. // Convert the data2
  417. Coff4[i3] = ((data4[0] * 256) + data4[1]);
  418. }
  419.  
  420. //beeper just to know if it's working
  421. pinMode(LED, OUTPUT);
  422. digitalWrite(LED, HIGH);
  423. delay(400);
  424. digitalWrite(LED, LOW);
  425. delay(400);
  426.  
  427.  
  428.  
  429.  
  430. }
  431.  
  432.  
  433. void loop(){
  434. timePrev =time;
  435. time= millis();
  436. elapsedTime = (time - timePrev)/1000;
  437.  
  438. //Acceleration
  439. Wire.beginTransmission(0x69);
  440. Wire.write(0x3B);
  441. Wire.endTransmission(false);
  442. Wire.requestFrom(0x69,6,true);
  443.  
  444. Accel_rawX=(Wire.read()<<8|Wire.read())/2048.0;
  445. Accel_rawY=(Wire.read()<<8|Wire.read())/2048.0;
  446. Accel_rawZ=(Wire.read()<<8|Wire.read())/2048.0;
  447.  
  448. //Gyroscope
  449. Wire.beginTransmission(0x69);
  450. Wire.write(0x43);
  451. Wire.endTransmission(false);
  452. Wire.requestFrom(0x69,6,true);
  453.  
  454. Gyro_rawX=Wire.read()<<8|Wire.read();
  455. Gyro_rawY=Wire.read()<<8|Wire.read();
  456.  
  457. Gyro_rawX = (Gyro_rawX/32.8) - Gyro_raw_errorX;
  458.  
  459. Gyro_rawY = (Gyro_rawY/32.8) - Gyro_raw_errorY;
  460.  
  461. angleX = Gyro_rawX * elapsedTime;
  462.  
  463. angleY = Gyro_rawX * elapsedTime;
  464.  
  465. Total_angle_x = 0.98 *(Total_angle_x + angleX) + 0.02*Accel_angleX;
  466.  
  467. Total_angle_y = 0.98 *(Total_angle_y + angleY) + 0.02*Accel_angleY;
  468.  
  469.  
  470.  
  471. // Start I2C Transmission
  472. Wire.beginTransmission(Addr);
  473. // Send reset command
  474. Wire.write(0x1E);
  475. // Stop I2C Transmission
  476. Wire.endTransmission();
  477. delay(500);
  478.  
  479. // Start I2C Transmission
  480. Wire.beginTransmission(Addr);
  481. // Refresh pressure with the OSR = 256
  482. Wire.write(0x40);
  483. // Stop I2C Transmission
  484. Wire.endTransmission();
  485. delay(500);
  486.  
  487. // Start I2C Transmission
  488. Wire.beginTransmission(Addr);
  489. // Select data register
  490. Wire.write(0x00);
  491. // Stop I2C Transmission
  492. Wire.endTransmission();
  493.  
  494. // Request 3 bytes of data
  495. Wire.requestFrom(Addr, 3);
  496.  
  497. // Read 3 bytes of data
  498. // ptemp_msb1, ptemp_msb, ptemp_lsb
  499. if(Wire.available() == 3)
  500. {
  501. data[0] = Wire.read();
  502. data[1] = Wire.read();
  503. data[2] = Wire.read();
  504. }
  505.  
  506. // Convert the data
  507. unsigned long ptemp = ((data[0] * 65536.0) + (data[1] * 256.0) + data[2]);
  508.  
  509. // Start I2C Transmission
  510. Wire.beginTransmission(Addr);
  511. // Refresh temperature with the OSR = 256
  512. Wire.write(0x50);
  513. // Stop I2C Transmission
  514. Wire.endTransmission();
  515. delay(500);
  516.  
  517. // Start I2C Transmission
  518. Wire.beginTransmission(Addr);
  519. // Select data register
  520. Wire.write(0x00);
  521. // Stop I2C Transmission
  522. Wire.endTransmission();
  523.  
  524. // Request 3 bytes of data
  525. Wire.requestFrom(Addr, 3);
  526.  
  527. // Read 3 bytes of data
  528. // temp_msb1, temp_msb, temp_lsb
  529. if(Wire.available() == 3)
  530. {
  531. data[0] = Wire.read();
  532. data[1] = Wire.read();
  533. data[2] = Wire.read();
  534. }
  535.  
  536. // Convert the data
  537. unsigned long temp = ((data[0] * 65536.0) + (data[1] * 256.0) + data[2]);
  538.  
  539. // Pressure and Temperature Calculations
  540. // 1st order temperature and pressure compensation
  541. // Difference between actual and reference temperature
  542. unsigned long dT = temp - ((Coff[4] * 256));
  543. temp = 2000 + (dT * (Coff[5] / pow(2, 23)));
  544.  
  545. // Offset and Sensitivity calculation
  546. unsigned long long off = Coff[1] * 65536 + (Coff[3] * dT) / 128;
  547. unsigned long long sens = Coff[0] * 32768 + (Coff[2] * dT) / 256;
  548.  
  549. // 2nd order temperature and pressure compensation
  550. if(temp >= 2000)
  551. {
  552. Ti = 0;
  553. offi = 0;
  554. sensi = 0;
  555. }
  556. else if(temp < 2000)
  557. {
  558. Ti = (dT * dT) / (pow(2,31));
  559. offi = 5 * ((pow((temp - 2000), 2))) / 2;
  560. sensi = 5 * ((pow((temp - 2000), 2))) / 4;
  561. if(temp < -1500)
  562. {
  563. offi = offi + 7 * ((pow((temp + 1500), 2)));
  564. sensi = sensi + 11 * ((pow((temp + 1500), 2))) / 2;
  565. }
  566. }
  567.  
  568. // Adjust temp, off, sens based on 2nd order compensation
  569. temp -= Ti;
  570. off -= offi;
  571. sens -= sensi;
  572.  
  573. // Convert the final data
  574. ptemp = (ptemp * sens / 2097152 - off);
  575. ptemp /= 32768;
  576. float pressure = ptemp / 100.0;
  577. float ctemp = temp / 100.0;
  578. float fTemp = ctemp * 1.8 + 32.0;
  579.  
  580. //This prints out stuff in the serial moniter
  581.  
  582. height = ((pow(pressure_at_sealevel/pressure, (1/5.257))-1)*(ctemp+273.15))/0.0065;
  583.  
  584.  
  585. hi = 44330.0f * (1.0f - pow((double)pressure / (double)pressure_at_sealevel, 0.1902949f));
  586.  
  587. change = hi2- hi;
  588.  
  589. Serial.print("HI: ");
  590. Serial.println(hi,4);
  591.  
  592. //This prints out time
  593. Serial.print("time: ");
  594. Serial.println(time/1000,4);
  595.  
  596. //This prints out acceleration
  597. Serial.print("X accel: ");//in the x axis
  598. Serial.print(Accel_rawX, 4);
  599. Serial.print(" | ");
  600. Serial.print("Y accel: ");//in the y axis
  601. Serial.print(Accel_rawY, 4);
  602. Serial.print(" | ");
  603. Serial.print("Z accel: ");//in the z axis
  604. Serial.println(Accel_rawZ, 4);
  605.  
  606. //This prints out rotation
  607. Serial.print("X rotation");//in the x axis
  608. Serial.println(angleX,4);
  609. Serial.print("Y rotation");//in the y axis
  610. Serial.println(angleY,4);
  611.  
  612. //This prints out the temperature
  613. Serial.print("Temperature in Fahrenheit : ");
  614. Serial.print(fTemp,4);
  615. Serial.println(" F");
  616.  
  617. Serial.print("Temperature in not Fahrenheit : ");
  618. Serial.print(ctemp,4);
  619. Serial.println(" F");
  620.  
  621. //This prints out the presssure
  622. Serial.print("Pressure : ");
  623. Serial.print(pressure,4);
  624. Serial.println(" mbar");
  625.  
  626. Serial.print("Xºgggg: ");
  627. Serial.print(Total_angle_x,4);
  628. Serial.print(" | ");
  629. Serial.print("Yºgggg: ");
  630. Serial.print(Total_angle_y,4);
  631. Serial.println(" ");
  632.  
  633. Serial.print("height: ");
  634. Serial.println(height,4);
  635.  
  636.  
  637.  
  638. delay(1000);
  639.  
  640. //AcceleraTi2on b2
  641. Wire.beginTransmission(0x69);
  642. Wire.write(0x3B);
  643. Wire.endTransmission(false);
  644. Wire.requestFrom(0x69,6,true);
  645.  
  646. Accel_rawX2=(Wire.read()<<8|Wire.read())/2048.0;
  647. Accel_rawY2=(Wire.read()<<8|Wire.read())/2048.0;
  648. Accel_rawZ2=(Wire.read()<<8|Wire.read())/2048.0;
  649.  
  650. //Gyroscope 2
  651. Wire.beginTransmission(0x69);
  652. Wire.write(0x43);
  653. Wire.endTransmission(false);
  654. Wire.requestFrom(0x69,6,true);
  655.  
  656. Gyro_rawX2=Wire.read()<<8|Wire.read();
  657. Gyro_rawY2=Wire.read()<<8|Wire.read();
  658.  
  659. Gyro_rawX2 = (Gyro_rawX2/32.8) - Gyro_raw_errorX2;
  660.  
  661. Gyro_rawY2 = (Gyro_rawY2/32.8) - Gyro_raw_errorY2;
  662.  
  663. angleX2= Gyro_rawX2 * elapsedTime;
  664.  
  665. angleY2= Gyro_rawX2 * elapsedTime;
  666.  
  667. Total_angleX2= 0.98 *(Total_angleX2+ angleX2) + 0.02*Accel_angleX2;
  668.  
  669. Total_angleY2= 0.98 *(Total_angleY2+ angleY2) + 0.02*Accel_angleY2;
  670.  
  671.  
  672.  
  673.  
  674.  
  675. // Start I2C Transmission
  676. Wire.beginTransmission(Addr);
  677. // Send reset command
  678. Wire.write(0x1E);
  679. // Stop I2C Transmission
  680. Wire.endTransmission();
  681. delay(500);
  682.  
  683. // Start I2C Transmission
  684. Wire.beginTransmission(Addr);
  685. // Refresh pressure with the OSR = 256
  686. Wire.write(0x40);
  687. // Stop I2C Transmission
  688. Wire.endTransmission();
  689. delay(500);
  690.  
  691. // Start I2C Transmission
  692. Wire.beginTransmission(Addr);
  693. // Select data2 register
  694. Wire.write(0x00);
  695. // Stop I2C Transmission
  696. Wire.endTransmission();
  697.  
  698. // Request 3 bytes of data2
  699. Wire.requestFrom(Addr, 3);
  700.  
  701. // Read 3 bytes of data2
  702. // ptemp_msb1, ptemp_msb, ptemp_lsb
  703. if(Wire.available() == 3)
  704. {
  705. data2[0] = Wire.read();
  706. data2[1] = Wire.read();
  707. data2[2] = Wire.read();
  708. }
  709.  
  710. // Convert the data2
  711. unsigned long ptemp2 = ((data2[0] * 65536.0) + (data2[1] * 256.0) + data2[2]);
  712.  
  713. // Start I2C Transmission
  714. Wire.beginTransmission(Addr);
  715. // Refresh temperature with the OSR = 256
  716. Wire.write(0x50);
  717. // Stop I2C Transmission
  718. Wire.endTransmission();
  719. delay(500);
  720.  
  721. // Start I2C Transmission
  722. Wire.beginTransmission(Addr);
  723. // Select data2 register
  724. Wire.write(0x00);
  725. // Stop I2C Transmission
  726. Wire.endTransmission();
  727.  
  728. // Request 3 bytes of data2
  729. Wire.requestFrom(Addr, 3);
  730.  
  731. // Read 3 bytes of data2
  732. // temp_msb1, temp_msb, temp_lsb
  733. if(Wire.available() == 3)
  734. {
  735. data2[0] = Wire.read();
  736. data2[1] = Wire.read();
  737. data2[2] = Wire.read();
  738. }
  739.  
  740. // Convert the data2
  741. unsigned long temp2 = ((data2[0] * 65536.0) + (data2[1] * 256.0) + data2[2]);
  742.  
  743. // Pressure and Temperature CalculaTi2ons
  744. // 1st order temperature and pressure compensaTi2on
  745. // Difference between actual and reference temperature
  746. unsigned long dT2 = temp2- ((Coff2[4] * 256));
  747. temp2 = 2000 + (dT2 * (Coff2[5] / pow(2, 23)));
  748.  
  749. // Offset and SensiTi2vitY2calculaTi2on
  750. unsigned long long off2 = Coff2[1] * 65536 + (Coff2[3] * dT2) / 128;
  751. unsigned long long sens2 = Coff2[0] * 32768 + (Coff2[2] * dT2) / 256;
  752.  
  753. // 2nd order temperature and pressure compensaTi2on
  754. if(temp2 >= 2000)
  755. {
  756. Ti2 = 0;
  757. offi2 = 0;
  758. sensi2 = 0;
  759. }
  760. else if(temp2 < 2000)
  761. {
  762. Ti2 = (dT2 * dT2) / (pow(2,31));
  763. offi2 = 5 * ((pow((temp2 - 2000), 2))) / 2;
  764. sensi2 = 5 * ((pow((temp2 - 2000), 2))) / 4;
  765. if(temp2 < -1500)
  766. {
  767. offi2 = offi2 + 7 * ((pow((temp2 + 1500), 2)));
  768. sensi2 = sensi2 + 11 * ((pow((temp2 + 1500), 2))) / 2;
  769. }
  770. }
  771.  
  772. // Adjust temp, off, sens based on 2nd order compensaTi2on
  773. temp2 -= Ti2;
  774. off2 -= offi2;
  775. sens2 -= sensi2;
  776.  
  777. // Convert the final data2
  778. ptemp2 = (ptemp2 * sens2 / 2097152 - off2);
  779. ptemp2 /= 32768;
  780. float pressure2= ptemp2 / 100.0;
  781. float cTemp2 = temp2 / 100.0;
  782. float fTemp2 = cTemp2 * 1.8 + 32.0;
  783.  
  784. //This prints out stuff in the serial moniter
  785.  
  786. //This prints out Time
  787. height2= ((pow(pressure_at_sealevel/pressure2, (1/5.257))-1)*(cTemp2+273.15))/0.0065;
  788.  
  789. hi2 = 44330.0f * (1.0f - pow((double)pressure2 / (double)pressure_at_sealevel, 0.1902949f));
  790.  
  791.  
  792.  
  793. Serial.print("HI two: ");
  794. Serial.println(hi2,4);
  795.  
  796. Serial.print("CHANGE IN HEIGHT: ");
  797. Serial.println(hi2-hi,4);
  798.  
  799. //This prints out acceleraTi2on
  800. Serial.print("X2accel: ");//in the X2axis
  801. Serial.print(Accel_rawX2, 4);
  802. Serial.print(" | ");
  803. Serial.print("Y accel: ");//in the Y2axis
  804. Serial.print(Accel_rawY2, 4);
  805. Serial.print(" | ");
  806. Serial.print("Z accel: ");//in the Z2axis
  807. Serial.println(Accel_rawZ2, 4);
  808.  
  809. //This prints out rotation
  810. Serial.print("X rotation: ");//in the X2axis
  811. Serial.println(angleX2,4);
  812. Serial.print("Y rotation: ");//in the Y2axis
  813. Serial.println(angleY2,4);
  814.  
  815. //This prints out the temperature
  816. Serial.print("Fahrenheit two : ");
  817. Serial.print(fTemp2,4);
  818. Serial.println(" Fº");
  819. Serial.print("not Fahrenheit two : ");
  820. Serial.print(cTemp2,4);
  821. Serial.println(" Cº");
  822.  
  823. //This prints out the presssure
  824. Serial.print("Pressure two : ");
  825. Serial.print(pressure2,4);
  826. Serial.println(" mbar");
  827.  
  828. Serial.print("Xº: ");
  829. Serial.print(Total_angleX2,4);
  830. Serial.print(" | ");
  831. Serial.print("Yº: ");
  832. Serial.print(Total_angleY2,4);
  833. Serial.println(" ");
  834.  
  835. Serial.print("height two: ");
  836. Serial.println(height2,4);
  837.  
  838.  
  839. delay(1000);
  840.  
  841.  
  842. //Acceleration
  843. Wire.beginTransmission(0x69);
  844. Wire.write(0x3B);
  845. Wire.endTransmission(false);
  846. Wire.requestFrom(0x69,6,true);
  847.  
  848. Accel_rawX3=(Wire.read()<<8|Wire.read())/2048.0;
  849. Accel_rawY3=(Wire.read()<<8|Wire.read())/2048.0;
  850. Accel_rawZ3=(Wire.read()<<8|Wire.read())/2048.0;
  851.  
  852. //Gyroscope
  853. Wire.beginTransmission(0x69);
  854. Wire.write(0x43);
  855. Wire.endTransmission(false);
  856. Wire.requestFrom(0x69,6,true);
  857.  
  858. Gyro_rawX3=Wire.read()<<8|Wire.read();
  859. Gyro_rawY3=Wire.read()<<8|Wire.read();
  860.  
  861. Gyro_rawX3= (Gyro_rawX3/32.8) - Gyro_raw_errorX3;
  862.  
  863. Gyro_rawY3 = (Gyro_rawY3/32.8) - Gyro_raw_errorY3;
  864.  
  865. angleX3= Gyro_rawX3* elapsedTime;
  866.  
  867. angleY3 = Gyro_rawX3* elapsedTime;
  868.  
  869. Total_angle_x3= 0.98 *(Total_angle_x3+ angleX3) + 0.02*Accel_angleX3;
  870.  
  871. Total_angle_y3 = 0.98 *(Total_angle_y3 + angleY3) + 0.02*Accel_angleY3;
  872.  
  873.  
  874.  
  875. // Start I2C Transmission
  876. Wire.beginTransmission(Addr);
  877. // Send reset command
  878. Wire.write(0x1E);
  879. // Stop I2C Transmission
  880. Wire.endTransmission();
  881. delay(500);
  882.  
  883. // Start I2C Transmission
  884. Wire.beginTransmission(Addr);
  885. // Refresh pressure with the OSR = 256
  886. Wire.write(0x40);
  887. // Stop I2C Transmission
  888. Wire.endTransmission();
  889. delay(500);
  890.  
  891. // Start I2C Transmission
  892. Wire.beginTransmission(Addr);
  893. // Select data register
  894. Wire.write(0x00);
  895. // Stop I2C Transmission
  896. Wire.endTransmission();
  897.  
  898. // Request 3 bytes of data
  899. Wire.requestFrom(Addr, 3);
  900.  
  901. // Read 3 bytes of data
  902. // ptemp_msb1, ptemp_msb, ptemp_lsb
  903. if(Wire.available() == 3)
  904. {
  905. data3[0] = Wire.read();
  906. data3[1] = Wire.read();
  907. data3[2] = Wire.read();
  908. }
  909.  
  910. // Convert the data
  911. unsigned long ptemp3 = ((data3[0] * 65536.0) + (data3[1] * 256.0) + data3[2]);
  912.  
  913. // Start I2C Transmission
  914. Wire.beginTransmission(Addr);
  915. // Refresh temperature with the OSR = 256
  916. Wire.write(0x50);
  917. // Stop I2C Transmission
  918. Wire.endTransmission();
  919. delay(500);
  920.  
  921. // Start I2C Transmission
  922. Wire.beginTransmission(Addr);
  923. // Select data register
  924. Wire.write(0x00);
  925. // Stop I2C Transmission
  926. Wire.endTransmission();
  927.  
  928. // Request 3 bytes of data
  929. Wire.requestFrom(Addr, 3);
  930.  
  931. // Read 3 bytes of data
  932. // temp_msb1, temp_msb, temp_lsb
  933. if(Wire.available() == 3)
  934. {
  935. data3[0] = Wire.read();
  936. data3[1] = Wire.read();
  937. data3[2] = Wire.read();
  938. }
  939.  
  940. // Convert the data
  941. unsigned long temp3 = ((data3[0] * 65536.0) + (data3[1] * 256.0) + data3[2]);
  942.  
  943. // Pressure and Temperature Calculations
  944. // 1st order temperature and pressure compensation
  945. // Difference between actual and reference temperature
  946. unsigned long dT3 = temp3 - ((Coff3[4] * 256));
  947. temp3 = 2000 + (dT3 * (Coff3[5] / pow(2, 23)));
  948.  
  949. // Offset and Sensitivity calculation
  950. unsigned long long off3 = Coff3[1] * 65536 + (Coff3[3] * dT3) / 128;
  951. unsigned long long sens3 = Coff3[0] * 32768 + (Coff3[2] * dT3) / 256;
  952.  
  953. // 2nd order temperature and pressure compensation
  954. if(temp3 >= 2000)
  955. {
  956. Ti3 = 0;
  957. offi3 = 0;
  958. sensi3 = 0;
  959. }
  960. else if(temp3 < 2000)
  961. {
  962. Ti3 = (dT3 * dT3) / (pow(2,31));
  963. offi3 = 5 * ((pow((temp3 - 2000), 2))) / 2;
  964. sensi3 = 5 * ((pow((temp3 - 2000), 2))) / 4;
  965. if(temp3 < -1500)
  966. {
  967. offi3 = offi3 + 7 * ((pow((temp3 + 1500), 2)));
  968. sensi3 = sensi3 + 11 * ((pow((temp3 + 1500), 2))) / 2;
  969. }
  970. }
  971.  
  972. // Adjust temp, off, sens based on 2nd order compensation
  973. temp3 -= Ti3;
  974. off3 -= offi3;
  975. sens3 -= sensi3;
  976.  
  977. // Convert the final data
  978. ptemp3 = (ptemp3 * sens3 / 2097152 - off3);
  979. ptemp3 /= 32768;
  980. float pressure3 = ptemp3 / 100.0;
  981. float ctemp3 = temp3 / 100.0;
  982. float fTemp3 = ctemp3 * 1.8 + 32.0;
  983.  
  984. //This prints out stuff in the serial moniter
  985.  
  986. height3 = ((pow(pressure_at_sealevel/pressure3, (1/5.257))-1)*(ctemp3+273.15))/0.0065;
  987.  
  988. hi3 = 44330.0f * (1.0f - pow((double)pressure3 / (double)pressure_at_sealevel, 0.1902949f));
  989.  
  990.  
  991.  
  992. //This prints out time
  993. Serial.print("time: ");
  994. Serial.println(time/1000,4);
  995.  
  996. Serial.print("change two: ");
  997. Serial.println(hi3-hi2,4);
  998.  
  999. //This prints out acceleration
  1000. Serial.print("X3accel: ");//in the x axis
  1001. Serial.print(Accel_rawX3, 4);
  1002. Serial.print(" | ");
  1003. Serial.print("Y accel: ");//in the y axis
  1004. Serial.print(Accel_rawY3, 4);
  1005. Serial.print(" | ");
  1006. Serial.print("Z accel: ");//in the z axis
  1007. Serial.println(Accel_rawZ3, 4);
  1008.  
  1009. //This prints out rotation
  1010. Serial.print("X3rotation");//in the x axis
  1011. Serial.println(angleX3,4);
  1012. Serial.print("Y rotation");//in the y axis
  1013. Serial.println(angleY3,4);
  1014.  
  1015. //This prints out the temperature
  1016. Serial.print("Temperature in Fahrenheit : ");
  1017. Serial.print(fTemp3,4);
  1018. Serial.println(" F");
  1019.  
  1020. //This prints out the presssure
  1021. Serial.print("Pressure Three: ");
  1022. Serial.print(pressure3,4);
  1023. Serial.println(" mbar");
  1024.  
  1025. Serial.print("Xº: ");
  1026. Serial.print(Total_angle_x3,4);
  1027. Serial.print(" | ");
  1028. Serial.print("Yº: ");
  1029. Serial.print(Total_angle_y3,4);
  1030. Serial.println(" ");
  1031.  
  1032. Serial.print("height three: ");
  1033. Serial.println(height3,4);
  1034.  
  1035. Serial.print ("hi three: ");
  1036. Serial.println(hi3,4);
  1037.  
  1038.  
  1039. delay(1000);
  1040.  
  1041. //AcceleraTi2on b2
  1042. Wire.beginTransmission(0x69);
  1043. Wire.write(0x3B);
  1044. Wire.endTransmission(false);
  1045. Wire.requestFrom(0x69,6,true);
  1046.  
  1047. Accel_rawX4=(Wire.read()<<8|Wire.read())/2048.0;
  1048. Accel_rawY4=(Wire.read()<<8|Wire.read())/2048.0;
  1049. Accel_rawZ4=(Wire.read()<<8|Wire.read())/2048.0;
  1050.  
  1051. //Gyroscope 4
  1052. Wire.beginTransmission(0x69);
  1053. Wire.write(0x43);
  1054. Wire.endTransmission(false);
  1055. Wire.requestFrom(0x69,6,true);
  1056.  
  1057. Gyro_rawX4=Wire.read()<<8|Wire.read();
  1058. Gyro_rawY4=Wire.read()<<8|Wire.read();
  1059.  
  1060. Gyro_rawX4 = (Gyro_rawX4/32.8) - Gyro_raw_errorX4;
  1061.  
  1062. Gyro_rawY4 = (Gyro_rawY4/32.8) - Gyro_raw_errorY4;
  1063.  
  1064. angleX4= Gyro_rawX4 * elapsedTime;
  1065.  
  1066. angleY4= Gyro_rawX4 * elapsedTime;
  1067.  
  1068. Total_angleX4= 0.98 *(Total_angleX4+ angleX4) + 0.02*Accel_angleX4;
  1069.  
  1070. Total_angleY4= 0.98 *(Total_angleY4+ angleY4) + 0.02*Accel_angleY4;
  1071.  
  1072.  
  1073.  
  1074. // Start I2C Transmission
  1075. Wire.beginTransmission(Addr);
  1076. // Send reset command
  1077. Wire.write(0x1E);
  1078. // Stop I2C Transmission
  1079. Wire.endTransmission();
  1080. delay(500);
  1081.  
  1082. // Start I2C Transmission
  1083. Wire.beginTransmission(Addr);
  1084. // Refresh pressure with the OSR = 256
  1085. Wire.write(0x40);
  1086. // Stop I2C Transmission
  1087. Wire.endTransmission();
  1088. delay(500);
  1089.  
  1090. // Start I2C Transmission
  1091. Wire.beginTransmission(Addr);
  1092. // Select data2 register
  1093. Wire.write(0x00);
  1094. // Stop I2C Transmission
  1095. Wire.endTransmission();
  1096.  
  1097. // Request 3 bytes of data2
  1098. Wire.requestFrom(Addr, 3);
  1099.  
  1100. // Read 3 bytes of data2
  1101. // ptemp_msb1, ptemp_msb, ptemp_lsb
  1102. if(Wire.available() == 3)
  1103. {
  1104. data4[0] = Wire.read();
  1105. data4[1] = Wire.read();
  1106. data4[2] = Wire.read();
  1107. }
  1108.  
  1109. // Convert the data2
  1110. unsigned long ptemp4 = ((data4[0] * 65536.0) + (data4[1] * 256.0) + data4[2]);
  1111.  
  1112. // Start I2C Transmission
  1113. Wire.beginTransmission(Addr);
  1114. // Refresh temperature with the OSR = 256
  1115. Wire.write(0x50);
  1116. // Stop I2C Transmission
  1117. Wire.endTransmission();
  1118. delay(500);
  1119.  
  1120. // Start I2C Transmission
  1121. Wire.beginTransmission(Addr);
  1122. // Select data2 register
  1123. Wire.write(0x00);
  1124. // Stop I2C Transmission
  1125. Wire.endTransmission();
  1126.  
  1127. // Request 3 bytes of data2
  1128. Wire.requestFrom(Addr, 3);
  1129.  
  1130. // Read 3 bytes of data2
  1131. // temp_msb1, temp_msb, temp_lsb
  1132. if(Wire.available() == 3)
  1133. {
  1134. data4[0] = Wire.read();
  1135. data4[1] = Wire.read();
  1136. data4[2] = Wire.read();
  1137. }
  1138.  
  1139. // Convert the data2
  1140. unsigned long temp4 = ((data4[0] * 65536.0) + (data4[1] * 256.0) + data4[2]);
  1141.  
  1142. // Pressure and Temperature CalculaTi2ons
  1143. // 1st order temperature and pressure compensaTi2on
  1144. // Difference between actual and reference temperature
  1145. unsigned long dT4 = temp4- ((Coff4[4] * 256));
  1146. temp4 = 2000 + (dT4 * (Coff4[5] / pow(2, 23)));
  1147.  
  1148. // Offset and SensiTi2vitY2calculaTi2on
  1149. unsigned long long off4 = Coff4[1] * 65536 + (Coff4[3] * dT4) / 128;
  1150. unsigned long long sens4 = Coff4[0] * 32768 + (Coff4[2] * dT4) / 256;
  1151.  
  1152. // 2nd order temperature and pressure compensaTi2on
  1153. if(temp4 >= 2000)
  1154. {
  1155. Ti4 = 0;
  1156. offi4 = 0;
  1157. sensi4 = 0;
  1158. }
  1159. else if(temp4 < 2000)
  1160. {
  1161. Ti4 = (dT4 * dT4) / (pow(2,31));
  1162. offi4 = 5 * ((pow((temp4 - 2000), 2))) / 2;
  1163. sensi4 = 5 * ((pow((temp4 - 2000), 2))) / 4;
  1164. if(temp4 < -1500)
  1165. {
  1166. offi4 = offi4 + 7 * ((pow((temp4 + 1500), 2)));
  1167. sensi4 = sensi4 + 11 * ((pow((temp4 + 1500), 2))) / 2;
  1168. }
  1169. }
  1170.  
  1171. // Adjust temp, off, sens based on 2nd order compensaTi2on
  1172. temp4 -= Ti4;
  1173. off4 -= offi4;
  1174. sens4 -= sensi4;
  1175.  
  1176. // Convert the final data2
  1177. ptemp4 = (ptemp4 * sens4 / 2097152 - off4);
  1178. ptemp4 /= 32768;
  1179. float pressure4= ptemp4 / 100.0;
  1180. float cTemp4 = temp4 / 100.0;
  1181. float fTemp4 = cTemp4 * 1.8 + 32.0;
  1182.  
  1183. //This prints out stuff in the serial moniter
  1184.  
  1185. //This prints out Time
  1186. height4= ((pow(pressure_at_sealevel/pressure4, (1/5.457))-1)*(cTemp4+273.15))/0.0065;
  1187. hi4 = 44330.0f * (1.0f - pow((double)pressure4 / (double)pressure_at_sealevel, 0.1902949f));
  1188.  
  1189. Serial.print("change 4: ");
  1190. Serial.println(hi4-hi3,4);
  1191. Serial.print("1-4 change: ");
  1192. Serial.println(hi4-hi,4);
  1193. //This prints out acceleraTi2on
  1194. Serial.print("X accel four: ");//in the X2axis
  1195. Serial.print(Accel_rawX4, 4);
  1196. Serial.print(" | ");
  1197. Serial.print("Y accel: ");//in the Y2axis
  1198. Serial.print(Accel_rawY4, 4);
  1199. Serial.print(" | ");
  1200. Serial.print("Z accel: ");//in the Z2axis
  1201. Serial.println(Accel_rawZ4, 4);
  1202.  
  1203. //This prints out rotation
  1204. Serial.print("X3rotation: ");//in the X2axis
  1205. Serial.println(angleX4,4);
  1206. Serial.print("Y rotation: ");//in the Y2axis
  1207. Serial.println(angleY4,4);
  1208.  
  1209. //This prints out the temperature
  1210. Serial.print("Fahrenheit two : ");
  1211. Serial.print(fTemp4,4);
  1212. Serial.println(" Fº");
  1213. Serial.print("not Fahrenheit two : ");
  1214. Serial.print(cTemp4,4);
  1215. Serial.println(" Cº");
  1216.  
  1217. //This prints out the presssure
  1218. Serial.print("Pressure four : ");
  1219. Serial.print(pressure4,4);
  1220. Serial.println(" mbar");
  1221.  
  1222. Serial.print("Xº: ");
  1223. Serial.print(Total_angleX4,4);
  1224. Serial.print(" | ");
  1225. Serial.print("Yº: ");
  1226. Serial.print(Total_angleY4,4);
  1227. Serial.println(" ");
  1228.  
  1229. Serial.print("height four: ");
  1230. Serial.println(height4,4);
  1231. Serial.print("hi four: ");
  1232. Serial.println(hi4,4);
  1233.  
  1234.  
  1235. delay(1000);
  1236.  
  1237.  
  1238.  
  1239. if(height<200){
  1240. if((hi2-hi) >-20){
  1241. digitalWrite(LED, HIGH);
  1242. delay(200);
  1243. digitalWrite(LED, LOW);
  1244. delay(200);
  1245. }
  1246. if((hi3-hi2) > -20){
  1247. digitalWrite(LED, HIGH);
  1248. delay(200);
  1249. digitalWrite(LED, LOW);
  1250. delay(200);
  1251. }
  1252. if(hi3-hi2 >-20){
  1253. digitalWrite(LED, HIGH);
  1254. delay(200);
  1255. digitalWrite(LED, LOW);
  1256. delay(200);
  1257. }
  1258. if(hi4-hi3 >20){
  1259. digitalWrite(LED, HIGH);
  1260. delay(200);
  1261. digitalWrite(LED, LOW);
  1262. delay(200);
  1263. }
  1264. if(hi4-hi >20){
  1265. digitalWrite(LED, HIGH);
  1266. delay(200);
  1267. digitalWrite(LED, LOW);
  1268. delay(200);
  1269. }
  1270. }
  1271.  
  1272.  
  1273.  
  1274. if(400>height>200){
  1275. if((hi2-hi) >15){
  1276. digitalWrite(LED, HIGH);
  1277. delay(200);
  1278. digitalWrite(LED, LOW);
  1279. delay(200);
  1280. }
  1281.  
  1282.  
  1283. if((hi3-hi2) > 15){
  1284. digitalWrite(LED, HIGH);
  1285. delay(200);
  1286. digitalWrite(LED, LOW);
  1287. delay(200);
  1288. }
  1289. if(hi3-hi2 >15){
  1290. digitalWrite(LED, HIGH);
  1291. delay(200);
  1292. digitalWrite(LED, LOW);
  1293. delay(200);
  1294. }
  1295. if(hi4-hi3 >15){
  1296. digitalWrite(LED, HIGH);
  1297. delay(200);
  1298. digitalWrite(LED, LOW);
  1299. delay(200);
  1300. }
  1301. if(hi4-hi >15){
  1302. digitalWrite(LED, HIGH);
  1303. delay(200);
  1304. digitalWrite(LED, LOW);
  1305. delay(200);
  1306. }
  1307. }
  1308.  
  1309.  
  1310. if(600>height>400){
  1311. if((hi2-hi) >12){
  1312. digitalWrite(LED, HIGH);
  1313. delay(200);
  1314. digitalWrite(LED, LOW);
  1315. delay(200);
  1316. }
  1317. if((hi3-hi2) > 12){
  1318. digitalWrite(LED, HIGH);
  1319. delay(200);
  1320. digitalWrite(LED, LOW);
  1321. delay(200);
  1322. }
  1323. if(hi3-hi2 >12){
  1324. digitalWrite(LED, HIGH);
  1325. delay(200);
  1326. digitalWrite(LED, LOW);
  1327. delay(200);
  1328. }
  1329. if(hi4-hi3 >12){
  1330. digitalWrite(LED, HIGH);
  1331. delay(200);
  1332. digitalWrite(LED, LOW);
  1333. delay(200);
  1334. }
  1335. if(hi4-hi >12){
  1336. digitalWrite(LED, HIGH);
  1337. delay(200);
  1338. digitalWrite(LED, LOW);
  1339. delay(200);
  1340. }
  1341. }
  1342.  
  1343. if(800>height>600){
  1344. if((hi2-hi) >8){
  1345. digitalWrite(LED, HIGH);
  1346. delay(200);
  1347. digitalWrite(LED, LOW);
  1348. delay(200);
  1349. }
  1350. if((hi3-hi2) > 8){
  1351. digitalWrite(LED, HIGH);
  1352. delay(200);
  1353. digitalWrite(LED, LOW);
  1354. delay(200);
  1355. }
  1356. if(hi3-hi2 >8){
  1357. digitalWrite(LED, HIGH);
  1358. delay(200);
  1359. digitalWrite(LED, LOW);
  1360. delay(200);
  1361. }
  1362. if(hi4-hi3 >8){
  1363. digitalWrite(LED, HIGH);
  1364. delay(200);
  1365. digitalWrite(LED, LOW);
  1366. delay(200);
  1367. }
  1368. if(hi4-hi >8){
  1369. digitalWrite(LED, HIGH);
  1370. delay(200);
  1371. digitalWrite(LED, LOW);
  1372. delay(200);
  1373. }
  1374. }
  1375.  
  1376. if(1000>height>800){
  1377. if((hi2-hi) >5){
  1378. digitalWrite(LED, HIGH);
  1379. delay(200);
  1380. digitalWrite(LED, LOW);
  1381. delay(200);
  1382. }
  1383. if((hi3-hi2) > 5){
  1384. digitalWrite(LED, HIGH);
  1385. delay(200);
  1386. digitalWrite(LED, LOW);
  1387. delay(200);
  1388. }
  1389. if(hi3-hi2 >5){
  1390. digitalWrite(LED, HIGH);
  1391. delay(200);
  1392. digitalWrite(LED, LOW);
  1393. delay(200);
  1394. }
  1395. if(hi4-hi3 >5){
  1396. digitalWrite(LED, HIGH);
  1397. delay(200);
  1398. digitalWrite(LED, LOW);
  1399. delay(200);
  1400. }
  1401. if(hi4-hi >5){
  1402. digitalWrite(LED, HIGH);
  1403. delay(200);
  1404. digitalWrite(LED, LOW);
  1405. delay(200);
  1406. }
  1407. }
  1408.  
  1409. if(17167.25>height>1000){
  1410. if((hi2-hi) >5){
  1411. digitalWrite(LED, HIGH);
  1412. delay(200);
  1413. digitalWrite(LED, LOW);
  1414. delay(200);
  1415. }
  1416. if((hi3-hi2) > 5){
  1417. digitalWrite(LED, HIGH);
  1418. delay(200);
  1419. digitalWrite(LED, LOW);
  1420. delay(200);
  1421. }
  1422. if(hi3-hi2 >5){
  1423. digitalWrite(LED, HIGH);
  1424. delay(200);
  1425. digitalWrite(LED, LOW);
  1426. delay(200);
  1427. }
  1428. if(hi4-hi3 >5){
  1429. digitalWrite(LED, HIGH);
  1430. delay(200);
  1431. digitalWrite(LED, LOW);
  1432. delay(200);
  1433. }
  1434. if(hi4-hi >5){
  1435. digitalWrite(LED, HIGH);
  1436. delay(200);
  1437. digitalWrite(LED, LOW);
  1438. delay(200);
  1439. }
  1440. }
  1441.  
  1442. if(height>17167.25) {
  1443. if((hi2-hi) >1){
  1444. digitalWrite(LED, HIGH);
  1445. delay(200);
  1446. digitalWrite(LED, LOW);
  1447. delay(200);
  1448. }
  1449. if((hi3-hi2) > 1){
  1450. digitalWrite(LED, HIGH);
  1451. delay(200);
  1452. digitalWrite(LED, LOW);
  1453. delay(200);
  1454. }
  1455. if(hi3-hi2 >1){
  1456. digitalWrite(LED, HIGH);
  1457. delay(200);
  1458. digitalWrite(LED, LOW);
  1459. delay(200);
  1460. }
  1461. if(hi4-hi3 >1){
  1462. digitalWrite(LED, HIGH);
  1463. delay(200);
  1464. digitalWrite(LED, LOW);
  1465. delay(200);
  1466. }
  1467. if(hi4-hi >1){
  1468. digitalWrite(LED, HIGH);
  1469. delay(200);
  1470. digitalWrite(LED, LOW);
  1471. delay(200);
  1472. }
  1473. }
  1474.  
  1475. }
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!