API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jul 23rd, 2016
254
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 18.48 KB | None | 0 0
raw download clone embed print report
  1. #define MPU_addr 0x68
  2. #define BUTTON_PIN 12
  3. #include <EEPROM.h>
  4. #include "EEPROMAnything.h"
  5. #include <Wire.h>
  6. #include <math.h>
  7. #include <LiquidCrystal_I2C.h>
  8. LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7,3,POSITIVE);
  9. float G = 9.81;
  10. float G_SQR = 96.2361;
  11. int16_t Tmp;
  12. float x[3],y[3],z[3];
  13. float Araw[3]; // acceleration from MPU, for calibration/alignment use
  14. float GyRaw[3]; // gyro readings from MPU, for use in getAraw function
  15. int signS; // either +1 or -1, sign of S based on gyros
  16. float S, ax, ay, az; // side acceleration (m/s^2), accelerations in chassis frame (m/s), theta in radians
  17. float csX = 1, csY = 1, csZ = 1, coX = 0, coY = 0, coZ = 0; // slope and offset calculated by calibrations. This should be stored in EEPROM.
  18. float GyXoffset = 0, GyYoffset = 0, GyZoffset = 0; // offsets for gyro
  19. int theta;
  20. int g;
  21. long thetaTime = 99999;
  22. long gTime = 99999;
  23. int maxTheta = 0;
  24. int maxG = 0;
  25. long timeOut = 4000; // make this user changable
  26. long timeOutArray[] = {5000, 10000, 15000, 25000, 50000, 120000, 100000000};
  27. // strings
  28. char angle[5];
  29. char accel[5];
  30. char maxAngle[5];
  31. char maxAccel[5];
  32.  
  33. // declare functions
  34. void getA();
  35. void getTemp();
  36. void calibration();
  37. void getAraw();
  38. void alignment();
  39. void calibrateAraw();
  40. void getStheta(int average); // averages "average" number of values before it returns
  41. void setups();
  42. void cross(const float m[],const float n[], float p[]);
  43. boolean buttonPressed();
  44.  
  45. void setup() {
  46. setups();
  47. lcd.setCursor(0,0); lcd.print("Now:");
  48. lcd.setCursor(0,1); lcd.print("Max:");
  49. }
  50.  
  51. void loop() {
  52.  
  53. getStheta(250);
  54. g = 100*az/G;
  55.  
  56. if (abs(g) > abs(maxG)) {
  57. maxG = g; gTime = millis();
  58. }
  59.  
  60. if (abs(theta) > abs(maxTheta)) {
  61. maxTheta = theta; thetaTime = millis();
  62. }
  63.  
  64. if (millis() - gTime > timeOut) maxG = 0;
  65. if (millis() - thetaTime > timeOut) maxTheta = 0;
  66.  
  67. sprintf(angle, "%i%c", theta, ((char) 223) );
  68. sprintf(accel, "%i%s", -g, "'g");
  69. sprintf(maxAngle, "%i%c", maxTheta, ((char) 223) );
  70. sprintf(maxAccel, "%i%s", -maxG, "'g");
  71.  
  72. lcd.setCursor(5, 0); lcd.print(angle); lcd.print(" ");
  73. lcd.setCursor(11,0); lcd.print(accel); lcd.print(" ");
  74. lcd.setCursor(5, 1); lcd.print(maxAngle); lcd.print(" ");
  75. lcd.setCursor(11,1); lcd.print(maxAccel); lcd.print(" ");
  76.  
  77. if (buttonPressed()) changeTimeout();
  78.  
  79. }// end loop
  80.  
  81. void alignment() //determines the x[] y[] and z[] which are used to transform from sensor frame (Araw from getAraw) to chassis frame (ax ay az from getA).
  82. {
  83.  
  84. int steps = 1000;
  85. int delaytime = 5*1000/steps; // put in seconds to calibrate, want at least 1 sec or so.
  86. float ax1=0, ay1=0, az1=0, ax2=0, ay2=0, az2=0; // these are values in sensor frame (with calibration applied): ax1 on kick stand, ax2 on rear stand
  87.  
  88. lcd.clear(); lcd.setCursor(0,0);
  89. lcd.print(" Entering"); lcd.setCursor(0,1); lcd.print(" alignment mode");
  90. delay(200);
  91. while(buttonPressed()); // wait for button to be released
  92. delay(1500);
  93. if (buttonPressed()) calibration();
  94.  
  95. lcd.clear(); lcd.setCursor(0,0);
  96. lcd.print(" Place bike flat"); lcd.setCursor(0,1); lcd.print(" on KICKSTAND");
  97. while (!buttonPressed()); //wait for button press
  98. lcd.clear(); lcd.setCursor(1,0); lcd.print("Acquiring data"); lcd.setCursor(2,1); lcd.print("HOLD STEADY!");
  99. delay(1000); // make sure bike isn't being touched
  100. for(int k=0; k<steps ; k++)
  101. {
  102. delay(delaytime);
  103. getAraw(); //update accel values
  104. calibrateAraw(); // apply calibration to Araw[] array
  105. ax2=ax2+Araw[0]; // sum values to compute average (divide later)
  106. ay2=ay2+Araw[1];
  107. az2=az2+Araw[2];
  108. }
  109. ax2=ax2/steps; //divide to give the average
  110. ay2=ay2/steps;
  111. az2=az2/steps;
  112.  
  113. lcd.clear(); lcd.setCursor(0,0);
  114. lcd.print("Place bike flat"); lcd.setCursor(0,1); lcd.print(" on REAR STAND");
  115. while (!buttonPressed()); //wait for button press
  116. lcd.clear(); lcd.print(" Acquiring in");
  117. for(int j=5;j>0;j--) // countdown until it starts taking data. Gives enough time to level rear stand
  118. {lcd.setCursor(14,0); lcd.print(j); delay(1000); }
  119. lcd.clear(); lcd.setCursor(1,0); lcd.print("Acquiring data"); lcd.setCursor(2,1); lcd.print("HOLD STEADY!");
  120.  
  121. for(int k=0; k<steps ; k++)
  122. {
  123. delay(delaytime);
  124. getAraw();
  125. calibrateAraw();
  126. ax1=ax1+Araw[0];
  127. ay1=ay1+Araw[1];
  128. az1=az1+Araw[2];
  129. }
  130. ax1=ax1/steps;
  131. ay1=ay1/steps;
  132. az1=az1/steps;
  133. //***** that's it for data collection
  134.  
  135. // compute vectors which become components of the rotation matrix
  136. float yvec[3];
  137. yvec[0]=ax1/G; yvec[1]=ay1/G; yvec[2]=az1/G;
  138.  
  139. float a2magnitude=sqrt(ax2*ax2+ay2*ay2+az2*az2);
  140. // float a2[]={ax2/a2magnitude , ay2/a2magnitude , az2/a2magnitude};
  141. float a2[3]; a2[0]=ax2; a2[1]=ay2; a2[2]=az2;
  142.  
  143. float zvec[3]; //initiate this so that the cross function can edit it
  144. cross(yvec,a2,zvec); // crosses yvec, a2, and writes it into zvec
  145. float zvecmagnitude=sqrt(zvec[0]*zvec[0]+zvec[1]*zvec[1]+zvec[2]*zvec[2]); // get it's magnitude because it's not a unit vector ( sin(alpha) is the length), alpha is angle of tilt on kickstand
  146. zvec[0]=zvec[0]/zvecmagnitude; zvec[1]=zvec[1]/zvecmagnitude; zvec[2]=zvec[2]/zvecmagnitude; // make zvec a unit vector
  147.  
  148. float xvec[]={0,0,0};
  149. cross(yvec,zvec,xvec);
  150.  
  151. // now update alignment constants which are loaded in memory in setups() and used by getA function
  152. for(int k = 0; k < 3; k++) {
  153. EEPROM_writeAnything(4*k,xvec[k]);
  154. EEPROM_writeAnything(4*k+12,yvec[k]);
  155. EEPROM_writeAnything(4*k+24,zvec[k]);
  156. }//end for loop
  157.  
  158. EEPROM.write(72, B01100100); // sets the alignment check register (72) = to 100 = B01100100 to flag that it's been done.
  159. lcd.clear(); lcd.setCursor(0,0);
  160. lcd.print(" Alignment"); lcd.setCursor(0,1); lcd.print(" finished.");
  161. delay(3000);
  162. lcd.clear();
  163.  
  164. }// end alignment function
  165.  
  166. boolean buttonPressed() { // should include some sort of debouncing
  167. if (!digitalRead(BUTTON_PIN)) {
  168. delay(20);
  169. if (!digitalRead(BUTTON_PIN)) {
  170. return true;
  171. }
  172. }
  173. return false;
  174. }// end buttonPrssed() function
  175.  
  176. void calibrateAraw()
  177. {
  178. // apply calibration to raw sensor values
  179. Araw[0]=Araw[0]*csX+coX;
  180. Araw[1]=Araw[1]*csY+coY;
  181. Araw[2]=Araw[2]*csZ+coZ;
  182.  
  183. GyRaw[0]=GyRaw[0]-GyXoffset;
  184. GyRaw[1]=GyRaw[1]-GyYoffset;
  185. GyRaw[2]=GyRaw[2]-GyZoffset;
  186.  
  187. } // end calibrateAraw function
  188.  
  189. void calibration()
  190. {
  191. float xmin=0,xmax=0,ymin=0,ymax=0,zmin=0,zmax=0,GyroX=0,GyroY=0,GyroZ=0;
  192. int steps = 1000;
  193. int delaytime = 2.5*1000/steps;
  194. lcd.clear(); lcd.setCursor(0,0);
  195. lcd.print(" Entering"); lcd.setCursor(0,1); lcd.print("CALIBRATION mode");
  196. delay(1500);
  197. while(buttonPressed());
  198.  
  199.  
  200.  
  201. lcd.clear(); lcd.setCursor(0,0); lcd.print("Align X vertical"); lcd.setCursor(0,1); lcd.print("& press button.");
  202. while (!buttonPressed());
  203. lcd.clear(); lcd.setCursor(0,0); lcd.print("Calibrating X"); lcd.setCursor(0,1); lcd.print("HOLD STEADY!");
  204. for(int k=0 ; k<steps ; k++)
  205. {
  206. delay(delaytime);
  207. getAraw(); //update accel values
  208. if(Araw[0]<0) xmin=xmin+Araw[0]; // figures out if you have x down or -x down
  209. else xmax=xmax+Araw[0];
  210. }
  211. lcd.clear(); lcd.setCursor(0,0); lcd.print("Flip X"); lcd.setCursor(0,1); lcd.print("& press button.");
  212. while (!buttonPressed()); //wait for button press
  213. lcd.clear(); lcd.setCursor(0,0); lcd.print("Calibrating X"); lcd.setCursor(0,1); lcd.print("HOLD STEADY!");
  214. for(int k=0; k<steps ; k++)
  215. {
  216. delay(delaytime);
  217. getAraw(); //update accel values
  218. if(Araw[0]<0) xmin=xmin+Araw[0]; // figures out if you have x down or -x down
  219. else xmax=xmax+Araw[0];
  220. }
  221. //now scale back
  222. xmax=xmax/steps;
  223. xmin=xmin/steps;
  224.  
  225.  
  226. lcd.clear(); lcd.setCursor(0,0); lcd.print("Align Y vertical"); lcd.setCursor(0,1); lcd.print("& press button.");
  227. while (!buttonPressed()); //wait for button press
  228. lcd.clear(); lcd.setCursor(0,0); lcd.print("Calibrating Y"); lcd.setCursor(0,1); lcd.print("HOLD STEADY!");
  229. for(int k=0; k<steps; k++)
  230. {
  231. delay(delaytime);
  232. getAraw(); //update accel values
  233. if(Araw[1]<0) ymin=ymin+Araw[1]; // figures out if you have y down or -y down
  234. else ymax=ymax+Araw[1];
  235. }
  236. lcd.clear(); lcd.setCursor(0,0); lcd.print("Flip Y"); lcd.setCursor(0,1); lcd.print("& press button.");
  237. while (!buttonPressed()); //wait for button press
  238. lcd.clear(); lcd.setCursor(0,0); lcd.print("Calibrating Y"); lcd.setCursor(0,1); lcd.print("HOLD STEADY!");
  239. for(int k=0; k<steps; k++)
  240. {
  241. delay(delaytime);
  242. getAraw(); //update accel values
  243. if(Araw[1]<0) ymin=ymin+Araw[1]; // figures out if you have y down or -y down
  244. else ymax=ymax+Araw[1];
  245. }
  246.  
  247. ymax=ymax/steps;
  248. ymin=ymin/steps;
  249. // done with y calibration
  250.  
  251. lcd.clear(); lcd.setCursor(0,0); lcd.print("Align Z vertical"); lcd.setCursor(0,1); lcd.print("& press button.");
  252. while (!buttonPressed()); //wait for button press
  253. lcd.clear(); lcd.setCursor(0,0); lcd.print("Calibrating Z"); lcd.setCursor(0,1); lcd.print("HOLD STEADY!");
  254. for(int k=0; k<steps; k++)
  255. {
  256. delay(delaytime);
  257. getAraw(); //update accel values
  258. if(Araw[2]<0) zmin=zmin+Araw[2]; // figures out if you have z down or -z down
  259. else zmax=zmax+Araw[2];
  260. }
  261. lcd.clear(); lcd.setCursor(0,0); lcd.print("Flip Z"); lcd.setCursor(0,1); lcd.print("& press button.");
  262. while (!buttonPressed()); //wait for button press
  263. lcd.clear(); lcd.setCursor(0,0); lcd.print("Calibrating Z"); lcd.setCursor(0,1); lcd.print("HOLD STEADY!");
  264. for(int k=0; k<steps; k++)
  265. {
  266. delay(delaytime);
  267. getAraw(); //update accel values
  268. if(Araw[2]<0) zmin=zmin+Araw[2]; // figures out if you have z down or -z down
  269. else zmax=zmax+Araw[2];
  270. }
  271.  
  272. zmax=zmax/steps;
  273. zmin=zmin/steps;
  274. // done with z calibration
  275.  
  276. // now do the gyro accelerations
  277. lcd.clear(); lcd.setCursor(0,0); lcd.print("Hold STILL"); lcd.setCursor(0,1); lcd.print("& press button.");
  278. while (!buttonPressed()); //wait for button press
  279. lcd.clear(); lcd.setCursor(0,0); lcd.print("Calibrating GYRO"); lcd.setCursor(0,1); lcd.print("HOLD STEADY!");
  280. for(int k=0; k<steps; k++)
  281. {
  282. getAraw();
  283. GyroX=GyroX+GyRaw[0];
  284. GyroY=GyroY+GyRaw[1];
  285. GyroZ=GyroZ+GyRaw[2];
  286. delay(delaytime);
  287. }
  288.  
  289. csX=2*G/(xmax-xmin);
  290. coX=-G*(xmax+xmin)/(xmax-xmin);
  291.  
  292. csY=2*G/(ymax-ymin);
  293. coY=-G*(ymax+ymin)/(ymax-ymin);
  294.  
  295. csZ=2*G/(zmax-zmin);
  296. coZ=-G*(zmax+zmin)/(zmax-zmin);
  297.  
  298. GyXoffset=GyroX/steps;
  299. GyYoffset=GyroY/steps;
  300. GyZoffset=GyroZ/steps;
  301.  
  302. // write coefficients to EEPROM as in excel list
  303. EEPROM_writeAnything(36,GyXoffset);
  304. EEPROM_writeAnything(40,GyYoffset);
  305. EEPROM_writeAnything(44,GyZoffset);
  306.  
  307. EEPROM_writeAnything(48,csX);
  308. EEPROM_writeAnything(52,coX);
  309. EEPROM_writeAnything(56,csY);
  310. EEPROM_writeAnything(60,coY);
  311. EEPROM_writeAnything(64,csZ);
  312. EEPROM_writeAnything(68,coZ);
  313.  
  314. EEPROM.write(73, B01100100); // sets the calibration check register (73) = to 100 to flag that it's been done.
  315.  
  316.  
  317.  
  318. lcd.clear(); lcd.setCursor(0,0); lcd.print("Calibration"); lcd.setCursor(0,1); lcd.print("complete."); delay(2000); lcd.clear();
  319. }// end calibration function
  320.  
  321. void changeTimeout() {
  322.  
  323. lcd.clear();
  324. lcd.setCursor(0,0);
  325. lcd.print("Set timeout:");
  326. while(buttonPressed());
  327. delay(200);
  328. long timer = millis();
  329. byte k = 0;
  330. char timerString[16];
  331.  
  332. while (millis() - timer < 4000) {
  333. delay(150);
  334. if (buttonPressed()) {
  335.  
  336. if ( k > 6 ) k = 0;
  337. timeOut = timeOutArray[k];
  338.  
  339. sprintf(timerString, "%li%s", timeOut/1000, " seconds ");
  340.  
  341. delay(200);
  342. lcd.setCursor(0,1);
  343. lcd.print(timerString);
  344.  
  345. k++;
  346. timer = millis();
  347. }
  348. }
  349.  
  350. EEPROM.write(75, k);
  351. lcd.clear();
  352. lcd.setCursor(0,0); lcd.print("Setting saved!");
  353. delay(2000);
  354. lcd.clear();
  355. lcd.setCursor(0,0); lcd.print("Now:");
  356. lcd.setCursor(0,1); lcd.print("Max:");
  357. }
  358.  
  359. void cross(const float m[],const float n[], float p[]) // pass p[] and change it in the cross function because you can't return array in C. Funciton can change input arrays because they're pointers. Doing const float m[] means the function is not allowed to change m[].
  360. {
  361. p[0]=n[1]*m[2]-m[1]*n[2];
  362. p[1]=m[0]*n[2]-n[0]*m[2];
  363. p[2]=n[0]*m[1]-m[0]*n[1];
  364.  
  365. }//end cross function
  366.  
  367. void getA() // takes Araw[] data and transforms to chassis frame, both accel and gyro, also sets sign of S
  368. {
  369.  
  370. getAraw(); // first, loads raw values straight from MPU6050
  371. calibrateAraw(); // apply calibration to Araw[] array
  372.  
  373. // transorm to chassis frame, x[],y[],z[] computed from alignment
  374. ax=Araw[0]*x[0]+Araw[1]*x[1]+Araw[2]*x[2];
  375. ay=Araw[0]*y[0]+Araw[1]*y[1]+Araw[2]*y[2];
  376. az=Araw[0]*z[0]+Araw[1]*z[1]+Araw[2]*z[2];
  377.  
  378. int16_t gyy=GyRaw[0]*y[0]+GyRaw[1]*y[1]+GyRaw[2]*y[2]; // only care about angular rate about y axis (bike is in a turn)
  379.  
  380.  
  381. //determine sign of S
  382. if(gyy>0) signS=1;
  383. else signS=-1;
  384.  
  385. }// end getA function
  386.  
  387. void getAraw()// gets raw sensor values for calibration and alignment
  388. {
  389. Wire.beginTransmission(MPU_addr);
  390. Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
  391. if (Wire.endTransmission(true)) return;
  392. if(Wire.requestFrom(MPU_addr,14,true) == 14) // request a total of 14 registers
  393. {
  394. Araw[0]=Wire.read()<<8|Wire.read();
  395. Araw[1]=Wire.read()<<8|Wire.read();
  396. Araw[2]=Wire.read()<<8|Wire.read();
  397. Tmp = Wire.read()<<8|Wire.read();
  398. GyRaw[0]=Wire.read()<<8|Wire.read();
  399. GyRaw[1]=Wire.read()<<8|Wire.read();
  400. GyRaw[2]=Wire.read()<<8|Wire.read();
  401. }
  402.  
  403. // note, Araw[] and GyRaw[] are float, but they come off as uint16_t from << thing, it gets re-cast.
  404.  
  405. }// end getAraw
  406.  
  407. void getStheta(int average) // updates the S, theta floats, and accelerations -- "int average" is the number of readings to be averaged
  408. {
  409.  
  410. float axAxPlusAyay = 0; // this gets averaged
  411. float axaveraged=0, ayaveraged=0, azaveraged=0;
  412. int signSaveraged=0;
  413.  
  414. for(int k=0; k < average ; k++) { // loop adds up "int average" number of readings
  415. getA();
  416. axaveraged = axaveraged + ax;
  417. ayaveraged = ayaveraged + ay;
  418. azaveraged = azaveraged + az;
  419.  
  420. signSaveraged = signSaveraged + signS; // signS comes from getA function
  421. }// end for loop which averages readings
  422.  
  423. // now divide by "int average" to get the average except for signS
  424. axaveraged = axaveraged/average;
  425. ayaveraged = ayaveraged/average;
  426. az = azaveraged/average; // this is a bit weird, bad style to overwrite az like this
  427. axAxPlusAyay = axaveraged*axaveraged + ayaveraged*ayaveraged;
  428.  
  429. if (signSaveraged > 0) signSaveraged = 1;
  430. else if (signSaveraged < 0) signSaveraged = -1;
  431. else signSaveraged = 0;
  432. //Serial.print("ax^2 + ay^2= "); Serial.println(axaxplusayay);
  433.  
  434. float absS_squared = axAxPlusAyay - G_SQR; // this should always be >=0 but inaccuracies can make it negative
  435.  
  436. if (absS_squared <=0) S=0; // sometimes the sqrt gets negative argument which returns "nan" (no imaginary). This prevents it.
  437. else S = signSaveraged*sqrt( absS_squared ); // sqrtf is on floats, sqrt() is on doubles
  438. // S = signSaveraged * sqrt( absS_squared );
  439. //Serial.print("S= "); Serial.println(S);
  440.  
  441.  
  442. //theta = round(57.29578*acos( (axaveraged*G+ayaveraged*S)/axAxPlusAyay )-90);
  443. theta = round(57.29578*asin( (axaveraged*G-ayaveraged*S)/axAxPlusAyay ) ); // asin takes double, also converts to degrees
  444. //Serial.print("theta "); Serial.println(theta);
  445.  
  446.  
  447. } //end getStheta function
  448.  
  449. void getTemp() // returns int value of temperature in Fahrengehgiehgeht
  450. {
  451. getAraw(); delay(100); getAraw();
  452. int tempF = Tmp*0.0052941+97.754; // converstion formula
  453. lcd.setCursor(0,0); lcd.print("Chip temp:");
  454. lcd.print(tempF); lcd.print((char) 223); lcd.print("F"); delay(250);
  455.  
  456. } // end getTemp function
  457.  
  458. void setups() { // sets the accelerometer settings, starts LCD
  459. // configure accelerometer registers
  460. Wire.begin();
  461. Wire.beginTransmission(MPU_addr);
  462. Wire.write(0x6B); // PWR_MGMT_1 register, sets clock reference, sleep mode, and temp sensor
  463. Wire.write(0b00000011); // set to zero (wakes up the MPU-6050), use gyro Z for clock ref
  464. Wire.endTransmission(true);
  465.  
  466. Wire.beginTransmission(MPU_addr);
  467. Wire.write(26); // Frame Synchronization and Filtering register
  468. Wire.write(0b00000110); // set to pretty hardcore filtering (last 3 bits chose between 0-6)
  469. Wire.endTransmission(true);
  470.  
  471. Wire.beginTransmission(MPU_addr);
  472. Wire.write(28); // Self-test and sensitivity (g-range) register.
  473. Wire.write(0b00000000); // last 3 bits are nothing. all 0 is +/- 2g
  474. Wire.endTransmission(true);
  475.  
  476.  
  477. // initialize LCD 16x2
  478. lcd.begin(16,2);
  479. lcd.backlight();
  480. lcd.setCursor(0,0); //Start at character 4 on line 0
  481. lcd.print("Starting...");
  482. delay(500);
  483. lcd.clear();
  484.  
  485. pinMode(BUTTON_PIN,INPUT_PULLUP); // button pin default high
  486. //Serial.begin(9600);
  487.  
  488. // load EEPROM, if EEPROM bytes are all 255, message that it needs alignment. Or make byte 72 = 0 if alignment done.
  489. /*byte checkCalibration;
  490. byte checkAlignment;
  491. byte checkCalibration=EEPROM.read(73);
  492. EEPROM.read(72, checkAlignment);*/
  493. if(EEPROM.read(73)!=B01100100){
  494. lcd.setCursor(0,0);
  495. lcd.print("Calibration not");
  496. lcd.setCursor(0,1);
  497. lcd.print("yet done.");
  498. delay(4000); calibration();
  499. }
  500.  
  501. if(EEPROM.read(72) != B01100100){
  502. lcd.print("Alignment not");
  503. lcd.setCursor(0,1);
  504. lcd.print("yet done.");
  505. delay(4000); alignment();
  506. }
  507.  
  508. // show temperature while waiting for any button presses
  509. getTemp();
  510. while (!buttonPressed() && millis() < 2000);
  511. lcd.clear();
  512. // now read calibration constants from EEPROM
  513. EEPROM_readAnything(36,GyXoffset);
  514. EEPROM_readAnything(40,GyYoffset);
  515. EEPROM_readAnything(44,GyZoffset);
  516.  
  517. EEPROM_readAnything(48,csX);
  518. EEPROM_readAnything(52,coX);
  519. EEPROM_readAnything(56,csY);
  520. EEPROM_readAnything(60,coY);
  521. EEPROM_readAnything(64,csZ);
  522. EEPROM_readAnything(68,coZ);
  523.  
  524. // perform alignment if button pressed
  525. if(buttonPressed()) alignment();
  526.  
  527. // now read alignment constants from EEPROM
  528. for(int k=0; k < 3; k++) {
  529. EEPROM_readAnything(4*k,x[k]);
  530. EEPROM_readAnything(4*k+12,y[k]);
  531. EEPROM_readAnything(4*k+24,z[k]);
  532. }//end for loop
  533.  
  534. // read timeOut preference
  535. int w = EEPROM.read(75);
  536. if (w > 6) w = 2;
  537. timeOut = timeOutArray[ w ];
  538.  
  539. }//end setups function
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!