API tools faq
paste
Advertisement
Halfrican911

Untitled

Halfrican911
Jun 28th, 2021
165
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 55.15 KB | None | 0 0
raw download clone embed print report
  1. #include <Wire.h>
  2. #include <Adafruit_Sensor.h>
  3. #include <Adafruit_BNO055.h>
  4. #include <utility/imumaths.h>
  5. // include the SD library:
  6. #include <SPI.h>
  7. #include <SD.h>
  8. #include "RTClib.h"
  9. #include <LiquidCrystal_I2C.h>
  10.  
  11.  
  12. #define NAMECHEST "chest"
  13. #define NAMEBICEP_R "upper_arm.R"
  14. #define NAMEFOREARM_R "forearm.R"
  15. #define NAMEHAND_R "hand.R"
  16. #define NAMEBICEP_L "upper_arm.L"
  17. #define NAMEFOREARM_L "forearm.L"
  18. #define NAMEHAND_L "hand.L"
  19. #define NAMEHEAD "head"
  20. #define NAMEWAIST "spine"
  21. #define NAMETHIGH_R "thigh.R"
  22. #define NAMETHIGH_L "thigh.L"
  23. #define NAMEKNEE_R "shin.R"
  24. #define NAMEKNEE_L "shin.L"
  25. #define NAMEFOOT_R "foot.R"
  26. #define NAMEFOOT_L "foot.L"
  27. /* This driver reads raw data from the BNO055
  28.  
  29. Connections
  30. ===========
  31. Connect SCL to analog 5
  32. Connect SDA to analog 4
  33. Connect VDD to 3.3V DC
  34. Connect GROUND to common ground
  35.  
  36. History
  37. =======
  38. 2015/MAR/03 - First release (KTOWN)
  39. */
  40. /**************************************************
  41. * VERSION
  42. *************************************************/
  43. float Version = 1.01;
  44. String VersionString = String(Version,2);
  45. /**************************************************
  46. *************************************************/
  47.  
  48.  
  49. // all of the types of sensors we will have.
  50. typedef enum
  51. {
  52. CHEST,
  53. BICEP_R,
  54. FOREARM_R,
  55. HAND_R,
  56. BICEP_L,
  57. FOREARM_L,
  58. HAND_L,
  59. HEAD,
  60. WAIST,
  61. THIGH_R,
  62. KNEE_R,
  63. FOOT_R,
  64. THIGH_L,
  65. KNEE_L,
  66. FOOT_L
  67. } sensor_id;
  68.  
  69. /* Set the delay between fresh samples */
  70. #define BNO055_SAMPLERATE_DELAY_MS (100)
  71.  
  72. /*
  73. syntax for this is first item is
  74. device id, not sure what this is,
  75. device addres either 0x29 or 0x28 for BNO055
  76. mux 1 port:the sensor is on for the mux (unused if next value is not -1)
  77. mux 2 port is the port on the second mux daisy chained off of the first mux at a port defined in the header file.
  78.  
  79. */
  80. /*Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address, int32_t mux1port, int32_t mux2port)*/
  81. Adafruit_BNO055 chest = Adafruit_BNO055(-1,BNO055_ADDRESS_B, 6, -1);
  82. bool chestPresent;
  83.  
  84. /* Head */
  85. Adafruit_BNO055 head = Adafruit_BNO055(-1,BNO055_ADDRESS_A, 4, -1);
  86. bool headPresent;
  87.  
  88. /* Right ARm*/
  89. Adafruit_BNO055 forearmR = Adafruit_BNO055(-1,BNO055_ADDRESS_A, 0, -1);
  90. bool forearmRPresent;
  91. Adafruit_BNO055 bicepR = Adafruit_BNO055(-1,BNO055_ADDRESS_B, 0, -1);
  92. bool bicepRPresent;
  93. Adafruit_BNO055 handR = Adafruit_BNO055(-1,BNO055_ADDRESS_B, 1, -1);
  94. bool handRPresent;
  95.  
  96. /* Left ARm*/
  97. Adafruit_BNO055 forearmL = Adafruit_BNO055(-1,BNO055_ADDRESS_A, 2, -1);
  98. bool forearmLPresent;
  99. Adafruit_BNO055 bicepL = Adafruit_BNO055(-1,BNO055_ADDRESS_B, 2, -1);
  100. bool bicepLPresent;
  101. Adafruit_BNO055 handL = Adafruit_BNO055(-1,BNO055_ADDRESS_B, 3, -1);
  102. bool handLPresent;
  103.  
  104. /* waist */
  105. Adafruit_BNO055 waist = Adafruit_BNO055(-1,BNO055_ADDRESS_B, -1, 0);
  106. bool waistPresent;
  107.  
  108.  
  109. /* Right Leg*/
  110. Adafruit_BNO055 thighR = Adafruit_BNO055(-1,BNO055_ADDRESS_A, -1, 1);
  111. bool thighRPresent;
  112. Adafruit_BNO055 kneeR = Adafruit_BNO055(-1,BNO055_ADDRESS_B, -1, 1);
  113. bool kneeRPresent;
  114. Adafruit_BNO055 footR = Adafruit_BNO055(-1,BNO055_ADDRESS_B, -1, 2);
  115. bool footRPresent;
  116.  
  117.  
  118. /* Left Leg*/
  119. Adafruit_BNO055 thighL = Adafruit_BNO055(-1,BNO055_ADDRESS_A, -1, 3);
  120. bool thighLPresent;
  121. Adafruit_BNO055 kneeL = Adafruit_BNO055(-1,BNO055_ADDRESS_B, -1, 3);
  122. bool kneeLPresent;
  123. Adafruit_BNO055 footL = Adafruit_BNO055(-1,BNO055_ADDRESS_B, -1, 4);
  124. bool footLPresent;
  125.  
  126. bool QuatMode;
  127. int TakeNumber = 1;
  128. bool IMUsInitialized = false;
  129. bool IMUsCalibrated = false;
  130.  
  131. // set up variables using the SD utility library functions:
  132. Sd2Card card;
  133. SdVolume volume;
  134. SdFile root;
  135. const int chipSelect = 10;
  136.  
  137. // real time clock.
  138. RTC_PCF8523 rtc;
  139. char daysOfTheWeek[7][12] = {"Sunday", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday"};
  140.  
  141. bool sdCardPresentandIntialized = false;
  142. const int buttonPin = 5; // the number of the pushbutton pin
  143. //LEDs
  144. const int ledGreenPin = 7;// the number of the LED pin
  145. const int ledRedPin = 6;// the number of the LED pin
  146. unsigned long CaptureDelay = 0;
  147.  
  148. //LCD Display
  149. #define LCD_DISPLAY_ADDR 0x27
  150. LiquidCrystal_I2C lcd(0x27, 16, 2);
  151. bool DisplayFound = false;
  152.  
  153. /**************************************************************************/
  154. /*
  155. LED Function- all Good
  156. */
  157. /**************************************************************************/
  158. bool CheckForLCDConnect(void)
  159. {
  160. byte error;
  161.  
  162. Wire.beginTransmission(LCD_DISPLAY_ADDR);
  163. error = Wire.endTransmission(); // stop transmitting
  164. if (error == 0)
  165. {
  166. DisplayFound = true;
  167. lcd.begin();
  168. Serial.println("LCD Display Found at 0x27...");
  169. } else
  170. {
  171. Serial.println("No LCD Display found...");
  172. }
  173.  
  174. }
  175. void ClearRowOfDisplay(int RowNum){
  176. if(RowNum == 1)
  177. {
  178. lcd.setCursor(0, 0);
  179. lcd.print(" ");
  180. } else
  181. {
  182. lcd.setCursor(0, 1);
  183. lcd.print(" ");
  184. }
  185. }
  186. void PrintLCDLine1(String c)
  187. {
  188. if(DisplayFound)
  189. {
  190. ClearRowOfDisplay(1);
  191. lcd.setCursor(0, 0);
  192. lcd.print(c);
  193. }
  194. }
  195. void PrintLCDLine2(String c)
  196. {
  197. if(DisplayFound)
  198. {
  199. ClearRowOfDisplay(2);
  200. lcd.setCursor(0, 1);
  201. lcd.print(c);
  202. }
  203. }
  204. /**************************************************************************/
  205. /*
  206. LED Function- all Good
  207. */
  208. /**************************************************************************/
  209. void SetupLEDs(){
  210. pinMode(ledGreenPin, OUTPUT);
  211. pinMode(ledRedPin, OUTPUT);
  212. }
  213. void StatusAllGood(){
  214. digitalWrite(ledGreenPin, HIGH);
  215. digitalWrite(ledRedPin, LOW);
  216. }
  217. void Busy(){
  218. digitalWrite(ledGreenPin, !digitalRead(ledGreenPin));
  219. digitalWrite(ledRedPin, LOW);
  220. }
  221. void Recording(){
  222. digitalWrite(ledRedPin, !digitalRead(ledRedPin));
  223. digitalWrite(ledGreenPin, LOW);
  224. }
  225. void StatusAllBad(){
  226. digitalWrite(ledGreenPin, LOW);
  227. digitalWrite(ledRedPin, HIGH);
  228. }
  229. /**************************************************************************/
  230. /*
  231. Set Capture Speed
  232. */
  233. /**************************************************************************/
  234. unsigned long SetSpeed( unsigned long CurrentDelay ){
  235. unsigned long SpeedDelayOut = 0;
  236. long int userInput;
  237.  
  238. Serial.print("Current Delay is Set To: ");
  239. Serial.println(CurrentDelay, DEC);
  240. Serial.println("Please Enter the speed you want to capture at:");
  241. Serial.println("0 As Fast As Possible");
  242. Serial.println("1 is Super Fast");
  243. Serial.println("2 Fast");
  244. Serial.println("3 Medium");
  245. Serial.println("4 Slow");
  246. Serial.println("5 Super Slow");
  247. while (Serial.available()==0) { }
  248. userInput = Serial.parseInt();
  249.  
  250. switch(userInput){
  251. case 0:
  252. SpeedDelayOut = 0;
  253. break;
  254. case 1:
  255. SpeedDelayOut = 10;
  256. break;
  257. case 2:
  258. SpeedDelayOut = 50;
  259. break;
  260. case 3:
  261. SpeedDelayOut = 100;
  262. break;
  263. case 4:
  264. SpeedDelayOut = 200;
  265. break;
  266. case 5:
  267. SpeedDelayOut = 500;
  268. break;
  269. default:
  270. SpeedDelayOut = BNO055_SAMPLERATE_DELAY_MS;
  271.  
  272. }
  273. Serial.print("Delay is Set To: ");
  274. Serial.println(SpeedDelayOut, DEC);
  275. return SpeedDelayOut;
  276. }
  277.  
  278. /**************************************************************************/
  279. /*
  280. Set the Clock
  281. */
  282. /**************************************************************************/
  283. void GetTimeFromUserAndSet(){
  284. long int userYear;
  285. long int userMonth = 1;
  286. long int userDay = 21;
  287. long int userHour = 8;
  288. long int userMinute = 30;
  289. long int userSecond = 0;
  290. DateTime now = rtc.now();
  291.  
  292. Serial.print("Please Enter The Year: (i.e. ");
  293. Serial.print(now.year(), DEC);
  294. Serial.println(")"); //Prompt User for input
  295. while (Serial.available()==0) { }
  296. userYear = Serial.parseInt();
  297.  
  298. Serial.print("Please Enter The Month: (i.e. ");
  299. Serial.print(now.month(), DEC);
  300. Serial.println(")"); //Prompt User for input
  301. while (Serial.available()==0) { }
  302. userMonth = Serial.parseInt();
  303.  
  304. Serial.print("Please Enter The Day: (i.e. ");
  305. Serial.print(now.day(), DEC);
  306. Serial.println(")"); //Prompt User for input
  307. while (Serial.available()==0) { }
  308. userDay = Serial.parseInt();
  309.  
  310. Serial.print("Please Enter The Hour: (i.e. ");
  311. Serial.print(now.hour(), DEC);
  312. Serial.println(")"); //Prompt User for input
  313. while (Serial.available()==0) { }
  314. userHour = Serial.parseInt();
  315.  
  316. Serial.print("Please Enter The Minute: (i.e. ");
  317. Serial.print(now.minute(), DEC);
  318. Serial.println(")"); //Prompt User for input
  319. while (Serial.available()==0) { }
  320. userMinute = Serial.parseInt();
  321.  
  322. Serial.print("Please Enter The Second: (i.e. ");
  323. Serial.print(now.second(), DEC);
  324. Serial.println(") When you press enter the time will be committed."); //Prompt User for input
  325. while (Serial.available()==0) { }
  326. userSecond = Serial.parseInt();
  327.  
  328.  
  329. rtc.adjust(DateTime(userYear,userMonth, userDay, userHour, userMinute, userSecond));
  330.  
  331. Serial.println("Time Set To:" + GetTime()); //Prompt User for input
  332. }
  333. /**************************************************************************/
  334. /*
  335. SDCard Info Display
  336. */
  337. /**************************************************************************/
  338. bool DetectAndDisplaySDCardInfo(){
  339.  
  340. bool cardPassedInit = true;
  341. Serial.print("\nInitializing SD card...");
  342. Busy();
  343. // we'll use the initialization code from the utility libraries
  344. // since we're just testing if the card is working!
  345. if (!card.init(SPI_HALF_SPEED, chipSelect)) {
  346. Serial.println("initialization failed. Things to check:");
  347. Serial.println("* is a card inserted?");
  348. Serial.println("* is your wiring correct?");
  349. Serial.println("* did you change the chipSelect pin to match your shield or module?");
  350. cardPassedInit = false;
  351. StatusAllBad();
  352. } else {
  353. Serial.println("Wiring is correct and a card is present.");
  354. Busy();
  355. }
  356.  
  357. if(cardPassedInit){
  358. // print the type of card
  359. Serial.print("\nCard type: ");
  360. switch (card.type()) {
  361. case SD_CARD_TYPE_SD1:
  362. Serial.println("SD1");
  363. break;
  364. case SD_CARD_TYPE_SD2:
  365. Serial.println("SD2");
  366. break;
  367. case SD_CARD_TYPE_SDHC:
  368. Serial.println("SDHC");
  369. break;
  370. default:
  371. Serial.println("Unknown");
  372. Busy();
  373. }
  374.  
  375. // Now we will try to open the 'volume'/'partition' - it should be FAT16 or FAT32
  376. if (!volume.init(card)) {
  377. Serial.println("Could not find FAT16/FAT32 partition.\nMake sure you've formatted the card");
  378. cardPassedInit = false;
  379. return cardPassedInit;
  380. Busy();
  381. }
  382.  
  383.  
  384. // print the type and size of the first FAT-type volume
  385. uint32_t volumesize;
  386. Serial.print("\nVolume type is FAT");
  387. Serial.println(volume.fatType(), DEC);
  388. Serial.println();
  389. Busy();
  390.  
  391. volumesize = volume.blocksPerCluster(); // clusters are collections of blocks
  392. volumesize *= volume.clusterCount(); // we'll have a lot of clusters
  393. volumesize *= 512; // SD card blocks are always 512 bytes
  394. Serial.print("Volume size (bytes): ");
  395. Serial.println(volumesize);
  396. Serial.print("Volume size (Kbytes): ");
  397. volumesize /= 1024;
  398. Serial.println(volumesize);
  399. Serial.print("Volume size (Mbytes): ");
  400. volumesize /= 1024;
  401. Serial.println(volumesize);
  402. Busy();
  403.  
  404.  
  405. Serial.println("\nFiles found on the card (name, date and size in bytes): ");
  406. root.openRoot(volume);
  407.  
  408. // list all files in the card with date and size
  409. root.ls(LS_R | LS_DATE | LS_SIZE);
  410. Busy();
  411. }
  412.  
  413. return cardPassedInit;
  414. }
  415. /**************************************************************************/
  416. /*
  417. Get Time String
  418. */
  419. /**************************************************************************/
  420. String GetTime(){
  421.  
  422. DateTime now = rtc.now();
  423. String TimeString = String(now.year(), DEC);
  424. TimeString += '/';
  425. TimeString += String(now.month(), DEC);
  426. TimeString += '/';
  427. TimeString += String(now.day(), DEC);
  428. TimeString += " (";
  429. TimeString += String(daysOfTheWeek[now.dayOfTheWeek()]);
  430. TimeString += ") ";
  431. TimeString += String(now.hour(), DEC);
  432. TimeString += ':';
  433. TimeString += String(now.minute(), DEC);
  434. TimeString += ':';
  435. TimeString += String(now.second(), DEC);
  436. return TimeString;
  437. }
  438. /**************************************************************************/
  439. /*
  440. SDCard Init
  441. */
  442. /**************************************************************************/
  443. void initializeSDCardForDataLogging(){
  444. // see if the card is present and can be initialized:
  445. if (!SD.begin(chipSelect)) {
  446. Serial.println("Card failed, or not present");
  447. // don't do anything more:
  448. return;
  449. } else{
  450. sdCardPresentandIntialized = true;
  451. Serial.println("card initialized.");
  452. }
  453. }
  454. /**************************************************************************/
  455. /*
  456. Get FileName for datalogging
  457. */
  458. /**************************************************************************/
  459. String DataLoggingName(){
  460. DateTime now = rtc.now();
  461. int filenum = 1;
  462. String FinalString = "";
  463. String Temp = String(now.year(), DEC);
  464. String TimeString = Temp.substring(Temp.length() -1);
  465. TimeString += String(now.month(), DEC);
  466. TimeString += String(now.day(), DEC);
  467. TimeString += "_";
  468. FinalString = TimeString + String(filenum, DEC) + ".TXT";
  469. while(SD.exists(FinalString)){
  470. filenum = filenum + 1;
  471. FinalString = TimeString + String(filenum, DEC) + ".TXT";
  472. }
  473. return FinalString;
  474. }
  475. /**************************************************************************/
  476. /*
  477. Print Line to File
  478. */
  479. /**************************************************************************/
  480. void PrintLine2File(String Line, File dataFile){
  481. if (dataFile) {
  482. dataFile.println(Line);
  483. }
  484. }
  485. /**************************************************************************/
  486. /*
  487. Display sensor calibration status
  488. */
  489. /**************************************************************************/
  490. bool displayCalStatus(sensor_id senseID)
  491. {
  492. /* Get the four calibration values (0..3) */
  493. /* Any sensor data reporting 0 should be ignored, */
  494. /* 3 means 'fully calibrated" */
  495. uint8_t system, gyro, accel, mag;
  496. system = gyro = accel = mag = 0;
  497. bool sensorExists = true;
  498. bool calibrated;
  499. String SensorName;
  500. Serial.print("~~");
  501. switch (senseID)
  502. {
  503. case CHEST:
  504. if(chestPresent)
  505. {
  506. SensorName = "NAMECHEST";
  507. chest.getCalibration(&system, &gyro, &accel, &mag);
  508. };
  509. break;
  510. case BICEP_R:
  511. if(bicepRPresent)
  512. {
  513. SensorName = "NAMEBICEP_R";
  514. bicepR.getCalibration(&system, &gyro, &accel, &mag);
  515. };
  516. break;
  517. case FOREARM_R:
  518. if(forearmRPresent)
  519. {
  520. SensorName = "NAMEFOREARM_R";
  521. forearmR.getCalibration(&system, &gyro, &accel, &mag);
  522. };
  523. break;
  524. case HAND_R:
  525. if(handRPresent)
  526. {
  527. SensorName = "NAMEHAND_R";
  528. handR.getCalibration(&system, &gyro, &accel, &mag);
  529. };
  530. case HEAD:
  531. if(headPresent)
  532. {
  533. SensorName = "HEAD";
  534. head.getCalibration(&system, &gyro, &accel, &mag);
  535. };
  536. case WAIST:
  537. if(waistPresent)
  538. {
  539. SensorName = "WAIST";
  540. waist.getCalibration(&system, &gyro, &accel, &mag);
  541. };
  542. case THIGH_R:
  543. if(thighRPresent)
  544. {
  545. SensorName = "THIGH_R";
  546. thighR.getCalibration(&system, &gyro, &accel, &mag);
  547. };
  548. case KNEE_R:
  549. if(kneeRPresent)
  550. {
  551. SensorName = "KNEE_R";
  552. kneeR.getCalibration(&system, &gyro, &accel, &mag);
  553. };
  554. case FOOT_R:
  555. if(footRPresent)
  556. {
  557. SensorName = "FOOT_R";
  558. footR.getCalibration(&system, &gyro, &accel, &mag);
  559. };
  560. case THIGH_L:
  561. if(thighLPresent)
  562. {
  563. SensorName = "THIGH_L";
  564. thighL.getCalibration(&system, &gyro, &accel, &mag);
  565. };
  566. case KNEE_L:
  567. if(kneeLPresent)
  568. {
  569. SensorName = "KNEE_L";
  570. kneeL.getCalibration(&system, &gyro, &accel, &mag);
  571. };
  572. case FOOT_L:
  573. if(footLPresent)
  574. {
  575. SensorName = "FOOT_L";
  576. footL.getCalibration(&system, &gyro, &accel, &mag);
  577. };
  578. break;
  579. default:
  580. sensorExists = false;
  581. break;
  582. }
  583.  
  584. if(sensorExists)
  585. {
  586. /* The data should be ignored until the system calibration is > 0 */
  587. //Serial.print("\t");
  588. if (!system)
  589. {
  590. Serial.print("~~");
  591. Serial.print(SensorName);
  592. Serial.print("NOCAL");
  593. calibrated = false;
  594. }else {
  595. calibrated = true;
  596. }
  597.  
  598. /* Display the individual values */
  599. // Serial.print(system, DEC);
  600. // Serial.print("G:");
  601. // Serial.print(gyro, DEC);
  602. // Serial.print("A:");
  603. // Serial.print(accel, DEC);
  604. // Serial.print("M:");
  605. // Serial.print(mag, DEC);
  606.  
  607. } else {
  608. calibrated = true;;
  609. }
  610. return calibrated;
  611. }
  612. /**************************************************************************/
  613. /*
  614. Enumerate I2C bus
  615. */
  616. /**************************************************************************/
  617. void enumerateI2CBus(void)
  618. { byte error, address, muxport;
  619. int nDevices;
  620. boolean mux1Found;
  621. boolean mux2Found;
  622. boolean RTCFound;
  623.  
  624. mux1Found = false;
  625. mux2Found = false;
  626. RTCFound = false;
  627.  
  628. Serial.println("Scanning...");
  629.  
  630. CheckForLCDConnect();
  631. PrintLCDLine1("Enumerating I2C..");
  632. PrintLCDLine2("");
  633. Wire.beginTransmission(0x70);
  634. error = Wire.endTransmission(); // stop transmitting
  635. if (error == 0)
  636. {
  637. mux1Found = true;
  638. Serial.println("Chest I2C Mux found at address 0x70...");
  639. } else
  640. {
  641. Serial.println("No I2C Mux in chest found...");
  642. }
  643.  
  644. if(mux1Found){
  645. //set mux1 to mux2 port
  646. Wire.beginTransmission(0x70);
  647. Wire.write(0x1 << MUX2_MUX1_PORT);
  648. error = Wire.endTransmission(); // stop transmitting
  649. Wire.beginTransmission(0x71);
  650. error = Wire.endTransmission(); // stop transmitting
  651. if (error == 0)
  652. {
  653. mux2Found = true;
  654. Serial.println("Waist I2C Mux found at address 0x71...");
  655. } else
  656. {
  657. Serial.println("No I2C Mux in waist found...");
  658. }
  659. } else
  660. {
  661. Serial.println("Didn't even look for second Mux because first mux missing...");
  662. }
  663.  
  664. Wire.beginTransmission(0x68);
  665. error = Wire.endTransmission(); // stop transmitting
  666. if (error == 0)
  667. {
  668. RTCFound = true;
  669. Serial.println("Real Time Clock found at address 0x68...");
  670. } else
  671. {
  672. Serial.println("No Real Time Clock found in chest...");
  673. }
  674.  
  675. nDevices = 0;
  676.  
  677. if (mux1Found)
  678. {
  679. for(muxport = 0; muxport <= 7; muxport++ )
  680. {
  681. Serial.print("Checking Mux1 Port ");
  682. Serial.print(muxport,HEX);
  683. Serial.println(".");
  684. Wire.beginTransmission(112); // transmit to device #112 (0x70)
  685. // device address is specified in datasheet
  686. Wire.write(0x1 << muxport); // sends value byte
  687. Wire.endTransmission(); // stop transmitting
  688.  
  689. for(address = 1; address < 127; address++ )
  690. {
  691. // The i2c_scanner uses the return value of
  692. // the Write.endTransmisstion to see if
  693. // a device did acknowledge to the address.
  694. Wire.beginTransmission(address);
  695. error = Wire.endTransmission();
  696.  
  697. if (error == 0 && address != 0x71 && address != 0x70 && address != 0x68 && address != LCD_DISPLAY_ADDR)
  698. {
  699. Serial.print("I2C device found at address 0x");
  700. if (address<16)
  701. Serial.print("0");
  702. Serial.print(address,HEX);
  703. Serial.println(" !");
  704.  
  705. nDevices++;
  706. }
  707. else if (error==4)
  708. {
  709. Serial.print("Unknown error at address 0x");
  710. if (address<16)
  711. Serial.print("0");
  712. Serial.println(address,HEX);
  713. }
  714. }
  715. }
  716. if(mux2Found)
  717. {
  718. //set mux1 to mux2 port
  719. Wire.beginTransmission(0x70);
  720. Wire.write(0x1 << MUX2_MUX1_PORT);
  721. error = Wire.endTransmission(); // stop transmitting
  722. for(muxport = 0; muxport <= 7; muxport++ )
  723. {
  724. Serial.print("Checking Mux2 Port ");
  725. Serial.print(muxport,HEX);
  726. Serial.println(".");
  727. Wire.beginTransmission(0x71); // transmit to device #112 (0x71)
  728. // device address is specified in datasheet
  729. Wire.write(0x1 << muxport); // sends value byte
  730. Wire.endTransmission(); // stop transmitting
  731.  
  732. for(address = 1; address < 127; address++ )
  733. {
  734. // The i2c_scanner uses the return value of
  735. // the Write.endTransmisstion to see if
  736. // a device did acknowledge to the address.
  737. Wire.beginTransmission(address);
  738. error = Wire.endTransmission();
  739.  
  740. if (error == 0 && address != 0x71 && address != 0x70 && address != 0x68 && address != LCD_DISPLAY_ADDR)
  741. {
  742. Serial.print("I2C device found at address 0x");
  743. if (address<16)
  744. Serial.print("0");
  745. Serial.print(address,HEX);
  746. Serial.println(" !");
  747.  
  748. nDevices++;
  749. }
  750. else if (error==4)
  751. {
  752. Serial.print("Unknown error at address 0x");
  753. if (address<16)
  754. Serial.print("0");
  755. Serial.println(address,HEX);
  756. }
  757. }
  758. }
  759. }
  760. } else
  761. {
  762. for(address = 1; address < 127; address++ )
  763. {
  764. // The i2c_scanner uses the return value of
  765. // the Write.endTransmisstion to see if
  766. // a device did acknowledge to the address.
  767. Wire.beginTransmission(address);
  768. error = Wire.endTransmission();
  769.  
  770. if (error == 0)
  771. {
  772. Serial.print("I2C device found at address 0x");
  773. if (address<16)
  774. Serial.print("0");
  775. Serial.print(address,HEX);
  776. Serial.println(" !");
  777.  
  778. nDevices++;
  779. }
  780. else if (error==4)
  781. {
  782. Serial.print("Unknown error at address 0x");
  783. if (address<16)
  784. Serial.print("0");
  785. Serial.println(address,HEX);
  786. }
  787. }
  788. }
  789. if (nDevices == 0)
  790. Serial.println("No I2C devices found\n");
  791. else
  792. Serial.println("done\n");
  793. PrintLCDLine1("Done 12C");
  794. PrintLCDLine2("Enumeration.");
  795. }
  796. /**************************************************************************/
  797. /*
  798. Initialize Chest
  799. */
  800. /**************************************************************************/
  801. void InitChest(void)
  802. {
  803. /* Initialise the sensor */
  804. if(!chest.begin())
  805. {
  806. /* There was a problem detecting the BNO055 ... check your connections */
  807. Serial.println("Chest Not Found.");
  808. PrintLCDLine1("Chest");
  809. PrintLCDLine2("NOT FOUND!!");
  810. chestPresent = false;
  811. StatusAllBad();
  812. } else
  813. {
  814. Serial.println("Chest Initialized..");
  815. PrintLCDLine1("Chest");
  816. PrintLCDLine2("Initialized");
  817. chestPresent = true;
  818. chest.setExtCrystalUse(true);
  819. StatusAllGood();
  820. }
  821. /* Display the current temperature */
  822. //int8_t temp = chest.getTemp();
  823. //Serial.print("Current Temperature: ");
  824. //Serial.print(temp);
  825. //Serial.println(" C");
  826. }
  827.  
  828. /**************************************************************************/
  829. /*
  830. Initialize Bicep Left
  831. */
  832. /**************************************************************************/
  833. void InitbicepL(void)
  834. {
  835. /* Initialise the sensor */
  836. if(!bicepL.begin())
  837. {
  838. /* There was a problem detecting the BNO055 ... check your connections */
  839. Serial.println("Left Bicep Not Found.");
  840. PrintLCDLine1("Left Bicep");
  841. PrintLCDLine2("NOT FOUND!!");
  842. bicepLPresent = false;
  843. StatusAllBad();
  844. } else
  845. {
  846. Serial.println("Left Bicep Initialized..");
  847. PrintLCDLine1("Left Bicep");
  848. PrintLCDLine2("Initialized");
  849. bicepLPresent = true;
  850. bicepL.setExtCrystalUse(true);
  851. StatusAllGood();
  852. }
  853. }
  854. /**************************************************************************/
  855. /*
  856. Initialize ForeARm Left
  857. */
  858. /**************************************************************************/
  859. void InitForeARmL(void)
  860. {
  861. /* Initialise the sensor */
  862. if(!forearmL.begin())
  863. {
  864. /* There was a problem detecting the BNO055 ... check your connections */
  865. Serial.println("Left Fore Arm Not Found.");
  866. PrintLCDLine1("Left ForeArm");
  867. PrintLCDLine2("NOT FOUND!!");
  868. forearmLPresent = false;
  869. StatusAllBad();
  870. } else
  871. {
  872. Serial.println("Left Fore Arm Initialized..");
  873. PrintLCDLine1("Left ForeArm");
  874. PrintLCDLine2("Initialized");
  875. forearmLPresent = true;
  876. forearmL.setExtCrystalUse(true);
  877. StatusAllGood();
  878. }
  879. }
  880. /**************************************************************************/
  881. /*
  882. Initialize Hand Left
  883. */
  884. /**************************************************************************/
  885. void InitHandL(void)
  886. {
  887. /* Initialise the sensor */
  888. if(!handL.begin())
  889. {
  890. /* There was a problem detecting the BNO055 ... check your connections */
  891. Serial.println("Left Hand Not Found.");
  892. PrintLCDLine1("Left Hand");
  893. PrintLCDLine2("NOT FOUND!!");
  894. handLPresent = false;
  895. StatusAllBad();
  896. } else
  897. {
  898. Serial.println("Left Hand Initialized..");
  899. PrintLCDLine1("Left Hand");
  900. PrintLCDLine2("Initialized");
  901. handLPresent = true;
  902. forearmL.setExtCrystalUse(true);
  903. StatusAllGood();
  904. }
  905. }
  906. /**************************************************************************/
  907. /*
  908. Initialize Bicep Right
  909. */
  910. /**************************************************************************/
  911. void InitbicepR(void)
  912. {
  913. /* Initialise the sensor */
  914. if(!bicepR.begin())
  915. {
  916. /* There was a problem detecting the BNO055 ... check your connections */
  917. Serial.println("Right Bicep Not Found.");
  918. PrintLCDLine1("Right Bicep");
  919. PrintLCDLine2("NOT FOUND!!");
  920. bicepRPresent = false;
  921. StatusAllBad();
  922. } else
  923. {
  924. Serial.println("Right Bicep Initialized..");
  925. PrintLCDLine1("Right Bicep");
  926. PrintLCDLine2("Initialized");
  927. bicepRPresent = true;
  928. bicepR.setExtCrystalUse(true);
  929. StatusAllGood();
  930. }
  931. }
  932. /**************************************************************************/
  933. /*
  934. Initialize ForeARm Right
  935. */
  936. /**************************************************************************/
  937. void InitForeARmR(void)
  938. {
  939. /* Initialise the sensor */
  940. if(!forearmR.begin())
  941. {
  942. /* There was a problem detecting the BNO055 ... check your connections */
  943. Serial.println("Right Fore Arm Not Found.");
  944. PrintLCDLine1("Right Fore Arm");
  945. PrintLCDLine2("NOT FOUND!!");
  946. forearmRPresent = false;
  947. StatusAllBad();
  948. } else
  949. {
  950. Serial.println("Right Fore Arm Initialized..");
  951. PrintLCDLine1("Right Fore Arm");
  952. PrintLCDLine2("Initialized");
  953. forearmRPresent = true;
  954. forearmR.setExtCrystalUse(true);
  955. StatusAllGood();
  956. }
  957. }
  958. /**************************************************************************/
  959. /*
  960. Initialize Hand Right
  961. */
  962. /**************************************************************************/
  963. void InitHandR(void)
  964. {
  965. /* Initialise the sensor */
  966. if(!handR.begin())
  967. {
  968. /* There was a problem detecting the BNO055 ... check your connections */
  969. Serial.println("Right Hand Not Found.");
  970. PrintLCDLine1("Right Hand");
  971. PrintLCDLine2("NOT FOUND!!");
  972. handRPresent = false;
  973. StatusAllBad();
  974. } else
  975. {
  976. Serial.println("Right Hand Initialized..");
  977. PrintLCDLine1("Right Hand");
  978. PrintLCDLine2("Initialized");
  979. handRPresent = true;
  980. forearmR.setExtCrystalUse(true);
  981. StatusAllGood();
  982. }
  983. }
  984. /**************************************************************************/
  985. /*
  986. Initialize Head
  987. */
  988. /**************************************************************************/
  989. void Inithead(void)
  990. {
  991. /* Initialise the sensor */
  992. if(!head.begin())
  993. {
  994. /* There was a problem detecting the BNO055 ... check your connections */
  995. Serial.println("head Not Found.");
  996. PrintLCDLine1("Head");
  997. PrintLCDLine2("NOT FOUND!!");
  998. headPresent = false;
  999. StatusAllBad();
  1000. } else
  1001. {
  1002. Serial.println("head Initialized..");
  1003. PrintLCDLine1("Head");
  1004. PrintLCDLine2("Initialized");
  1005. headPresent = true;
  1006. head.setExtCrystalUse(true);
  1007. StatusAllGood();
  1008. }
  1009. }
  1010. /**************************************************************************/
  1011. /*
  1012. Initialize Waist
  1013. */
  1014. /**************************************************************************/
  1015. void Initwaist(void)
  1016. {
  1017. /* Initialise the sensor */
  1018. if(!waist.begin())
  1019. {
  1020. /* There was a problem detecting the BNO055 ... check your connections */
  1021. Serial.println("waist Not Found.");
  1022. PrintLCDLine1("Waist");
  1023. PrintLCDLine2("NOT FOUND!!");
  1024. waistPresent = false;
  1025. StatusAllBad();
  1026. } else
  1027. {
  1028. Serial.println("waist Initialized..");
  1029. PrintLCDLine1("Waist");
  1030. PrintLCDLine2("Initialized");
  1031. waistPresent = true;
  1032. waist.setExtCrystalUse(true);
  1033. StatusAllGood();
  1034. }
  1035. }
  1036. /**************************************************************************/
  1037. /*
  1038. Initialize Right Thigh
  1039. */
  1040. /**************************************************************************/
  1041. void InitthighR(void)
  1042. {
  1043. /* Initialise the sensor */
  1044. if(!thighR.begin())
  1045. {
  1046. /* There was a problem detecting the BNO055 ... check your connections */
  1047. Serial.println("Right Thigh Not Found.");
  1048. PrintLCDLine1("Right Thigh");
  1049. PrintLCDLine2("NOT FOUND!!");
  1050. thighRPresent = false;
  1051. StatusAllBad();
  1052. } else
  1053. {
  1054. Serial.println("Right Thigh Initialized..");
  1055. PrintLCDLine1("Right Thigh");
  1056. PrintLCDLine2("Initialized");
  1057. thighRPresent = true;
  1058. thighR.setExtCrystalUse(true);
  1059. StatusAllGood();
  1060. }
  1061. }
  1062. /**************************************************************************/
  1063. /*
  1064. Initialize Right Knee
  1065. */
  1066. /**************************************************************************/
  1067. void InitkneeR(void)
  1068. {
  1069. /* Initialise the sensor */
  1070. if(!kneeR.begin())
  1071. {
  1072. /* There was a problem detecting the BNO055 ... check your connections */
  1073. Serial.println("Right Knee Not Found.");
  1074. PrintLCDLine1("Righ Knee");
  1075. PrintLCDLine2("NOT FOUND!!");
  1076. kneeRPresent = false;
  1077. StatusAllBad();
  1078. } else
  1079. {
  1080. Serial.println("Right Knee Initialized..");
  1081. PrintLCDLine1("Righ Knee");
  1082. PrintLCDLine2("Initialized");
  1083. kneeRPresent = true;
  1084. kneeR.setExtCrystalUse(true);
  1085. StatusAllGood();
  1086. }
  1087. }
  1088. /**************************************************************************/
  1089. /*
  1090. Initialize Right foot
  1091. */
  1092. /**************************************************************************/
  1093. void InitfootR(void)
  1094. {
  1095. /* Initialise the sensor */
  1096. if(!footR.begin())
  1097. {
  1098. /* There was a problem detecting the BNO055 ... check your connections */
  1099. Serial.println("Right Foot Not Found.");
  1100. PrintLCDLine1("Right Foot");
  1101. PrintLCDLine2("NOT FOUND!!");
  1102. footRPresent = false;
  1103. StatusAllBad();
  1104. } else
  1105. {
  1106. Serial.println("Right Foot Initialized..");
  1107. PrintLCDLine1("Right Foot");
  1108. PrintLCDLine2("Initialized");
  1109. footRPresent = true;
  1110. footR.setExtCrystalUse(true);
  1111. StatusAllGood();
  1112. }
  1113. }
  1114. /**************************************************************************/
  1115. /*
  1116. Initialize Left Thigh
  1117. */
  1118. /**************************************************************************/
  1119. void InitthighL(void)
  1120. {
  1121. /* Initialise the sensor */
  1122. if(!thighL.begin())
  1123. {
  1124. /* There was a problem detecting the BNO055 ... check your connections */
  1125. Serial.println("Left Thigh Not Found.");
  1126. PrintLCDLine1("Left Thigh");
  1127. PrintLCDLine2("NOT FOUND!!");
  1128. thighLPresent = false;
  1129. StatusAllBad();
  1130. } else
  1131. {
  1132. Serial.println("Left Thigh Initialized..");
  1133. PrintLCDLine1("Left Thigh");
  1134. PrintLCDLine2("Initialized");
  1135. thighLPresent = true;
  1136. thighL.setExtCrystalUse(true);
  1137. StatusAllGood();
  1138. }
  1139. }
  1140. /**************************************************************************/
  1141. /*
  1142. Initialize Left Knee
  1143. */
  1144. /**************************************************************************/
  1145. void InitkneeL(void)
  1146. {
  1147. /* Initialise the sensor */
  1148. if(!kneeL.begin())
  1149. {
  1150. /* There was a problem detecting the BNO055 ... check your connections */
  1151. Serial.println("Left Knee Not Found.");
  1152. PrintLCDLine1("Left Knee");
  1153. PrintLCDLine2("NOT FOUND!!");
  1154. kneeLPresent = false;
  1155. StatusAllBad();
  1156. } else
  1157. {
  1158. Serial.println("Left Knee Initialized..");
  1159. PrintLCDLine1("Left Knee");
  1160. PrintLCDLine2("Initialized");
  1161. kneeLPresent = true;
  1162. kneeL.setExtCrystalUse(true);
  1163. StatusAllGood();
  1164. }
  1165. }
  1166. /**************************************************************************/
  1167. /*
  1168. Initialize Right foot
  1169. */
  1170. /**************************************************************************/
  1171. void InitfootL(void)
  1172. {
  1173. /* Initialise the sensor */
  1174. if(!footL.begin())
  1175. {
  1176. /* There was a problem detecting the BNO055 ... check your connections */
  1177. Serial.println("Left Foot Not Found.");
  1178. PrintLCDLine1("Left Foot");
  1179. PrintLCDLine2("NOT FOUND!!");
  1180. footLPresent = false;
  1181. StatusAllBad();
  1182. } else
  1183. {
  1184. Serial.println("Left Foot Initialized..");
  1185.  
  1186. PrintLCDLine1("Left Foot");
  1187. PrintLCDLine2("Initialized");
  1188. footLPresent = true;
  1189. footL.setExtCrystalUse(true);
  1190. StatusAllGood();
  1191. }
  1192. }
  1193. /**************************************************************************/
  1194. /*
  1195. Print Quaternion Data
  1196. */
  1197. /**************************************************************************/
  1198. String printQuat(imu::Quaternion &quat, String SensorName){
  1199. String ReturnString = "~~";
  1200. ReturnString += SensorName;
  1201. ReturnString += "~qW:";
  1202. ReturnString += String(quat.w(), 4);
  1203. ReturnString += "~qX:";
  1204. ReturnString += String(quat.y(), 4);
  1205. ReturnString += "~qY:";
  1206. ReturnString += String(quat.x(), 4);
  1207. ReturnString += "~qZ:";
  1208. ReturnString += String(quat.z(), 4);
  1209. ReturnString += "~";
  1210. return ReturnString;
  1211. }
  1212. /**************************************************************************/
  1213. /*
  1214. Print Euler Data
  1215. */
  1216. /**************************************************************************/
  1217. String printEuler(imu::Vector<3> &euler, String SensorName){
  1218. String ReturnString = "~~";
  1219. ReturnString += SensorName;
  1220. // Display the floating point data
  1221. ReturnString += "~X:";
  1222. ReturnString += String(euler.x());
  1223. ReturnString += "~Y:";
  1224. ReturnString += String(euler.y());
  1225. ReturnString += "~Z:";
  1226. ReturnString += String(euler.z());
  1227. ReturnString += "~";
  1228.  
  1229. return ReturnString;
  1230. }
  1231. /**************************************************************************/
  1232. /*
  1233. Initialize Chest
  1234. */
  1235. /**************************************************************************/
  1236. void InitAllIMUs(void)
  1237. {
  1238. Inithead();
  1239.  
  1240. InitChest();
  1241.  
  1242. InitbicepR();
  1243. InitForeARmR();
  1244. InitHandR();
  1245.  
  1246. InitbicepL();
  1247. InitForeARmL();
  1248. InitHandL();
  1249.  
  1250. Initwaist();
  1251.  
  1252. InitthighR();
  1253. InitkneeR();
  1254. InitfootR();
  1255.  
  1256. InitthighL();
  1257. InitkneeL();
  1258. InitfootL();
  1259.  
  1260. PrintLCDLine1("DoneInitializing");
  1261. PrintLCDLine2("Button to Cal.");
  1262. }
  1263. /**************************************************************************/
  1264. /*
  1265. Read a Line of Calibration Data
  1266. */
  1267. /**************************************************************************/
  1268. bool ReadLineCalData(String LineStartText)
  1269. {
  1270. bool AllCalibrated = true;
  1271. Serial.print(LineStartText);
  1272. if(chestPresent)
  1273. {
  1274. if(!displayCalStatus(CHEST))
  1275. {
  1276. AllCalibrated = false;
  1277. PrintLCDLine1("Chest");
  1278. PrintLCDLine2("NOT CALIBRATED!");
  1279. }
  1280. }
  1281. if(handRPresent)
  1282. {
  1283. if(!displayCalStatus(HAND_R))
  1284. {
  1285. AllCalibrated = false;
  1286. PrintLCDLine1("Right Hand");
  1287. PrintLCDLine2("NOT CALIBRATED!");
  1288. }
  1289. }
  1290. if(bicepRPresent)
  1291. {
  1292. if(!displayCalStatus(BICEP_R))
  1293. {
  1294. AllCalibrated = false;
  1295. PrintLCDLine1("Right Bicep");
  1296. PrintLCDLine2("NOT CALIBRATED!");
  1297. }
  1298. }
  1299. if(forearmRPresent)
  1300. {
  1301. if(!displayCalStatus(FOREARM_R))
  1302. {
  1303. AllCalibrated = false;
  1304. PrintLCDLine1("Right ForeArm");
  1305. PrintLCDLine2("NOT CALIBRATED!");
  1306. }
  1307. }
  1308. if(headPresent)
  1309. {
  1310. if(!displayCalStatus(HEAD))
  1311. {
  1312. AllCalibrated = false;
  1313. PrintLCDLine1("Head");
  1314. PrintLCDLine2("NOT CALIBRATED!");
  1315. }
  1316. }
  1317. if(waistPresent)
  1318. {
  1319. if(!displayCalStatus(WAIST))
  1320. {
  1321. AllCalibrated = false;
  1322. PrintLCDLine1("Waist");
  1323. PrintLCDLine2("NOT CALIBRATED!");
  1324. }
  1325. }
  1326. if(thighRPresent)
  1327. {
  1328. if(!displayCalStatus(THIGH_R))
  1329. {
  1330. AllCalibrated = false;
  1331. PrintLCDLine1("Right Thigh");
  1332. PrintLCDLine2("NOT CALIBRATED!");
  1333. }
  1334. }
  1335. if(kneeRPresent)
  1336. {
  1337. if(!displayCalStatus(KNEE_R))
  1338. {
  1339. AllCalibrated = false;
  1340. PrintLCDLine1("Right Knee");
  1341. PrintLCDLine2("NOT CALIBRATED!");
  1342. }
  1343. }
  1344. if(footRPresent)
  1345. {
  1346. if(!displayCalStatus(FOOT_R))
  1347. {
  1348. AllCalibrated = false;
  1349. PrintLCDLine1("Right Foot");
  1350. PrintLCDLine2("NOT CALIBRATED!");
  1351. }
  1352. }
  1353. if(thighLPresent)
  1354. {
  1355. if(!displayCalStatus(THIGH_L))
  1356. {
  1357. AllCalibrated = false;
  1358. PrintLCDLine1("Right Thigh");
  1359. PrintLCDLine2("NOT CALIBRATED!");
  1360. }
  1361. }
  1362. if(kneeLPresent)
  1363. {
  1364. if(!displayCalStatus(KNEE_L))
  1365. {
  1366. AllCalibrated = false;
  1367. PrintLCDLine1("Left Knee");
  1368. PrintLCDLine2("NOT CALIBRATED!");
  1369. }
  1370. }
  1371. if(footLPresent)
  1372. {
  1373. if(!displayCalStatus(FOOT_L))
  1374. {
  1375. AllCalibrated = false;
  1376. PrintLCDLine1("Left Foot");
  1377. PrintLCDLine2("NOT CALIBRATED!");
  1378. }
  1379. }
  1380. if(AllCalibrated){
  1381. Serial.println("All Sensors Calibrated");
  1382. PrintLCDLine1("All SensorsCal'd");
  1383. PrintLCDLine2("Button to Record");
  1384. } else {
  1385. Serial.println("");
  1386. }
  1387. return AllCalibrated;
  1388. }
  1389. /**************************************************************************/
  1390. /*
  1391. Read a Line of Sensor Data
  1392. */
  1393. /**************************************************************************/
  1394. void ReadLineOfData(String LineStartText, File dataFile, unsigned long refTime)
  1395. {
  1396.  
  1397. String FinalTxtLine = LineStartText;
  1398. FinalTxtLine += " ~Take";
  1399. FinalTxtLine += String(TakeNumber, DEC);
  1400. FinalTxtLine += "~ ";
  1401.  
  1402. FinalTxtLine += " ~Time";
  1403. FinalTxtLine += String(refTime, DEC);
  1404. FinalTxtLine += "~ ";
  1405. if(chestPresent)
  1406. {
  1407. // Possible vector values can be:
  1408. // - VECTOR_ACCELEROMETER - m/s^2
  1409. // - VECTOR_MAGNETOMETER - uT
  1410. // - VECTOR_GYROSCOPE - rad/s
  1411. // - VECTOR_EULER - degrees
  1412. // - VECTOR_LINEARACCEL - m/s^2
  1413. // - VECTOR_GRAVITY - m/s^2
  1414. if (QuatMode){
  1415. // Quaternion data
  1416. imu::Quaternion quat = chest.getQuat();
  1417. FinalTxtLine += printQuat(quat,NAMECHEST);
  1418. }else
  1419. {
  1420. imu::Vector<3> euler = chest.getVector(Adafruit_BNO055::VECTOR_EULER);
  1421. FinalTxtLine += printEuler(euler,NAMECHEST);
  1422. }
  1423. FinalTxtLine += " ";
  1424.  
  1425. }
  1426. if(handRPresent)
  1427. {
  1428. if (QuatMode){
  1429. // Quaternion data
  1430. imu::Quaternion quathandr = handR.getQuat();
  1431. FinalTxtLine += printQuat(quathandr,NAMEHAND_R);
  1432. }else
  1433. {
  1434. imu::Vector<3> euler = handR.getVector(Adafruit_BNO055::VECTOR_EULER);
  1435. // Display the floating point data
  1436. FinalTxtLine += printEuler(euler,NAMEHAND_R);
  1437. }
  1438. FinalTxtLine += " ";
  1439. }
  1440. if(bicepRPresent)
  1441. {
  1442. if (QuatMode){
  1443. // Quaternion data
  1444. imu::Quaternion quatbicepr = bicepR.getQuat();
  1445. FinalTxtLine += printQuat(quatbicepr,NAMEBICEP_R);
  1446. }else
  1447. {
  1448. imu::Vector<3> euler = bicepR.getVector(Adafruit_BNO055::VECTOR_EULER);
  1449. // Display the floating point data
  1450. FinalTxtLine += printEuler(euler,NAMEBICEP_R);
  1451. }
  1452. FinalTxtLine += " ";
  1453. }
  1454.  
  1455. if(forearmRPresent)
  1456. {
  1457. if (QuatMode){
  1458. // Quaternion data
  1459. imu::Quaternion quatforearmR = forearmR.getQuat();
  1460. FinalTxtLine += printQuat(quatforearmR,NAMEFOREARM_R);
  1461. }else
  1462. {
  1463. imu::Vector<3> euler = forearmR.getVector(Adafruit_BNO055::VECTOR_EULER);
  1464. // Display the floating point data
  1465. FinalTxtLine += printEuler(euler,NAMEFOREARM_R);
  1466. }
  1467. FinalTxtLine += " ";
  1468. }
  1469.  
  1470. if(forearmLPresent)
  1471. {
  1472. if (QuatMode){
  1473. // Quaternion data
  1474. imu::Quaternion quatforearmL = forearmL.getQuat();
  1475. FinalTxtLine += printQuat(quatforearmL,NAMEFOREARM_L);
  1476. }else
  1477. {
  1478. imu::Vector<3> euler = forearmL.getVector(Adafruit_BNO055::VECTOR_EULER);
  1479. // Display the floating point data
  1480. FinalTxtLine += printEuler(euler,NAMEFOREARM_L);
  1481. }
  1482. FinalTxtLine += " ";
  1483. }
  1484. if(bicepLPresent)
  1485. {
  1486. if (QuatMode){
  1487. // Quaternion data
  1488. imu::Quaternion quatbicepL = bicepL.getQuat();
  1489. FinalTxtLine += printQuat(quatbicepL,NAMEBICEP_L);
  1490. }else
  1491. {
  1492. imu::Vector<3> euler = bicepL.getVector(Adafruit_BNO055::VECTOR_EULER);
  1493. // Display the floating point data
  1494. FinalTxtLine += printEuler(euler,NAMEBICEP_L);
  1495. }
  1496. FinalTxtLine += " ";
  1497. }
  1498. if(handLPresent)
  1499. {
  1500. if (QuatMode){
  1501. // Quaternion data
  1502. imu::Quaternion quathandL = handL.getQuat();
  1503. FinalTxtLine += printQuat(quathandL,NAMEHAND_L);
  1504. }else
  1505. {
  1506. imu::Vector<3> euler = handL.getVector(Adafruit_BNO055::VECTOR_EULER);
  1507. // Display the floating point data
  1508. FinalTxtLine += printEuler(euler,NAMEHAND_L);
  1509. }
  1510. FinalTxtLine += " ";
  1511. }
  1512. if(headPresent)
  1513. {
  1514. if (QuatMode){
  1515. // Quaternion data
  1516. imu::Quaternion quathead = head.getQuat();
  1517. FinalTxtLine += printQuat(quathead,NAMEHEAD);
  1518. }else
  1519. {
  1520. imu::Vector<3> euler = head.getVector(Adafruit_BNO055::VECTOR_EULER);
  1521. // Display the floating point data
  1522. FinalTxtLine += printEuler(euler,NAMEHEAD);
  1523. }
  1524. FinalTxtLine += " ";
  1525. }
  1526. if(waistPresent)
  1527. {
  1528. if (QuatMode){
  1529. // Quaternion data
  1530. imu::Quaternion quatwaist = waist.getQuat();
  1531. FinalTxtLine += printQuat(quatwaist,NAMEWAIST);
  1532. }else
  1533. {
  1534. imu::Vector<3> euler = waist.getVector(Adafruit_BNO055::VECTOR_EULER);
  1535. // Display the floating point data
  1536. FinalTxtLine += printEuler(euler,NAMEWAIST);
  1537. }
  1538. FinalTxtLine += " ";
  1539. }
  1540. if(thighLPresent)
  1541. {
  1542. if (QuatMode){
  1543. // Quaternion data
  1544. imu::Quaternion quatthighL = thighL.getQuat();
  1545. FinalTxtLine += printQuat(quatthighL,NAMETHIGH_L);
  1546. }else
  1547. {
  1548. imu::Vector<3> euler = thighL.getVector(Adafruit_BNO055::VECTOR_EULER);
  1549. // Display the floating point data
  1550. FinalTxtLine += printEuler(euler,NAMETHIGH_L);
  1551. }
  1552. FinalTxtLine += " ";
  1553. }
  1554. if(kneeLPresent)
  1555. {
  1556. if (QuatMode){
  1557. // Quaternion data
  1558. imu::Quaternion quatkneeL = kneeL.getQuat();
  1559. FinalTxtLine += printQuat(quatkneeL,NAMEKNEE_L);
  1560. }else
  1561. {
  1562. imu::Vector<3> euler = kneeL.getVector(Adafruit_BNO055::VECTOR_EULER);
  1563. // Display the floating point data
  1564. FinalTxtLine += printEuler(euler,NAMEKNEE_L);
  1565. }
  1566. FinalTxtLine += " ";
  1567. }
  1568. if(footLPresent)
  1569. {
  1570. if (QuatMode){
  1571. // Quaternion data
  1572. imu::Quaternion quattfootL = footL.getQuat();
  1573. FinalTxtLine += printQuat(quattfootL,NAMEFOOT_L);
  1574. }else
  1575. {
  1576. imu::Vector<3> euler = footL.getVector(Adafruit_BNO055::VECTOR_EULER);
  1577. // Display the floating point data
  1578. FinalTxtLine += printEuler(euler,NAMEFOOT_L);
  1579. }
  1580. FinalTxtLine += " ";
  1581. }
  1582. if(thighRPresent)
  1583. {
  1584. if (QuatMode){
  1585. // Quaternion data
  1586. imu::Quaternion quatthighR = thighR.getQuat();
  1587. FinalTxtLine += printQuat(quatthighR,NAMETHIGH_R);
  1588. }else
  1589. {
  1590. imu::Vector<3> euler = thighR.getVector(Adafruit_BNO055::VECTOR_EULER);
  1591. // Display the floating point data
  1592. FinalTxtLine += printEuler(euler,NAMETHIGH_R);
  1593. }
  1594. FinalTxtLine += " ";
  1595. }
  1596. if(kneeRPresent)
  1597. {
  1598. if (QuatMode){
  1599. // Quaternion data
  1600. imu::Quaternion quatkneeR = kneeR.getQuat();
  1601. FinalTxtLine += printQuat(quatkneeR,NAMEKNEE_R);
  1602. }else
  1603. {
  1604. imu::Vector<3> euler = kneeR.getVector(Adafruit_BNO055::VECTOR_EULER);
  1605. // Display the floating point data
  1606. FinalTxtLine += printEuler(euler,NAMEKNEE_R);
  1607. }
  1608. FinalTxtLine += " ";
  1609. }
  1610. if(footRPresent)
  1611. {
  1612. if (QuatMode){
  1613. // Quaternion data
  1614. imu::Quaternion quattfootR = footR.getQuat();
  1615. FinalTxtLine += printQuat(quattfootR,NAMEFOOT_R);
  1616. }else
  1617. {
  1618. imu::Vector<3> euler = footR.getVector(Adafruit_BNO055::VECTOR_EULER);
  1619. // Display the floating point data
  1620. FinalTxtLine += printEuler(euler,NAMEFOOT_R);
  1621. }
  1622. FinalTxtLine += " ";
  1623. }
  1624. /* New line for the next sample */
  1625. FinalTxtLine += "PNTEND";
  1626. Serial.println(FinalTxtLine);
  1627. if(dataFile){
  1628. PrintLine2File(FinalTxtLine, dataFile);
  1629. }
  1630.  
  1631. }
  1632. /**************************************************************************/
  1633. /*
  1634. Set Mux to Port with command Line
  1635. */
  1636. /**************************************************************************/
  1637. void setMuxToPort(void)
  1638. {
  1639. int incomingByte = 0;
  1640.  
  1641. Serial.println("Enter Port To Set To:");
  1642.  
  1643. Serial.println("");
  1644.  
  1645. while( Serial.available() == 0) {}
  1646. incomingByte = Serial.read();
  1647.  
  1648. Wire.beginTransmission(112); // transmit to device #112 (0x70)
  1649. // shifting mux to port six output.
  1650. Wire.write(0x1 << incomingByte); // sends value byte
  1651. Wire.endTransmission(); // stop transmitting
  1652.  
  1653. }
  1654. /**************************************************************************/
  1655. /*
  1656. Run TPose Command with countdown.
  1657. */
  1658. /**************************************************************************/
  1659. void RunTPose(File FileIn)
  1660. {
  1661.  
  1662. Busy();
  1663. Serial.println("Get into T Pose Position, taking TPose In:");
  1664. PrintLCDLine1("TPose In..");
  1665. PrintLCDLine2("Countdown...");
  1666. delay(1000);
  1667. Busy();
  1668. Serial.println("5 Seconds");
  1669. PrintLCDLine1("55555");
  1670. PrintLCDLine2("Get In TPose Now");
  1671. delay(1000);
  1672. Busy();
  1673. Serial.println("4 Seconds");
  1674. PrintLCDLine1("4444");
  1675. //PrintLCDLine2("Get In TPose Now");
  1676. delay(1000);
  1677. Busy();
  1678. Serial.println("3 Seconds");
  1679. PrintLCDLine1("333");
  1680. //PrintLCDLine2("Get In TPose Now");
  1681. delay(1000);
  1682. Busy();
  1683. Serial.println("2 Seconds");
  1684. PrintLCDLine1("22");
  1685. //PrintLCDLine2("Get In TPose Now");
  1686. delay(1000);
  1687. Busy();
  1688. Serial.println("1 Seconds");
  1689. PrintLCDLine1("1");
  1690. //PrintLCDLine2("Get In TPose Now");
  1691. delay(1000);
  1692. Busy();
  1693. File uselessFile;
  1694. ReadLineOfData("TPOS", FileIn, 0);
  1695.  
  1696. }
  1697. /**************************************************************************/
  1698. /*
  1699. Arduino setup function (automatically called at startup)
  1700. */
  1701. /**************************************************************************/
  1702. void setup(void)
  1703. {
  1704. Wire.begin();
  1705. /*Some processors also support 10000 (low speed mode), 1000000 (fast mode plus) and 3400000 (high speed mode). */
  1706. Wire.setClock(400000); //fastmode.... 100000 is normal
  1707. QuatMode = true;
  1708. Serial.begin(115200);
  1709. Serial.println("");
  1710. Serial.println("***************************************");
  1711. Serial.println("KeeCap Motion Capture Intializing");
  1712. Serial.println(" Version " + VersionString);
  1713. Serial.println("***************************************");
  1714. PrintLCDLine1("KeeCap Init");
  1715. PrintLCDLine2("SDCard Init");
  1716.  
  1717.  
  1718. Serial.print("Initializing SD card...");
  1719.  
  1720. initializeSDCardForDataLogging();
  1721. //Serial.println("Calibration status values: 0=uncalibrated, 3=fully calibrated");
  1722.  
  1723. SetupLEDs();
  1724. // initialize the pushbutton pin as an input:
  1725. pinMode(buttonPin, INPUT_PULLUP);
  1726. CheckForLCDConnect();
  1727. PrintLCDLine1("KeeCap v" + VersionString);
  1728. PrintLCDLine2("Press to Init.");
  1729. }
  1730. /**************************************************************************/
  1731. /*
  1732. Arduino loop function,
  1733. */
  1734. /**************************************************************************/
  1735. void loop(void)
  1736. {
  1737. unsigned long StartTime;
  1738. unsigned long CurrentTime;
  1739. int incomingByte = 0;
  1740. bool ButtonPushed = false;
  1741. String TimeLine = "";
  1742. Serial.println("Spark MoCap Version: " + VersionString);
  1743. Serial.println("Use the Keyboard to select the following options");
  1744. Serial.println("e : enumerate I2C devices");
  1745. Serial.println("i : Initialize Available Devices");
  1746. Serial.println("r : read from device (press any key to stop)");
  1747. Serial.println("s : Set Mux to Port Number you enter");
  1748. Serial.println("q : Set output read to quaternion mode.");
  1749. Serial.println("w : Set output read to euler mode.");
  1750. Serial.println("t : Begin Take by starting T Pose Process");
  1751. Serial.println("c : calibrate Sensor Process");
  1752. Serial.println("d : SD Card Detect and Display Results.");
  1753. Serial.println("j : Print Current Time");
  1754. Serial.println("v : Set Date and Time");
  1755. Serial.println("P : Set Speed of Capture");
  1756. Serial.println("DoneMenus");
  1757. StatusAllGood();
  1758. while( Serial.available() == 0 && !ButtonPushed) {
  1759. ButtonPushed = !digitalRead(buttonPin);
  1760. }
  1761. if(ButtonPushed){
  1762. if(!IMUsInitialized && !IMUsCalibrated){
  1763. incomingByte = 105; //initialize i
  1764. }else
  1765. if(IMUsInitialized && !IMUsCalibrated){
  1766. incomingByte = 99; //calibrate c
  1767. } else {
  1768. incomingByte = 114; //read data....
  1769. }
  1770. }else{
  1771. incomingByte = Serial.read();
  1772. }
  1773. Busy();
  1774.  
  1775. // keyboard 't' is 116 decimal
  1776. if (incomingByte == 116)
  1777. {
  1778.  
  1779. TakeNumber = TakeNumber + 1;
  1780. File uselessFile;
  1781. RunTPose(uselessFile);
  1782. } else
  1783. // keyboard 'v' is 118 decimal
  1784. if (incomingByte == 118)
  1785. {
  1786. GetTimeFromUserAndSet();
  1787. } else
  1788. // keyboard 'p' is 112 decimal
  1789. if (incomingByte == 112)
  1790. {
  1791. CaptureDelay = SetSpeed(CaptureDelay);
  1792. } else
  1793. // keyboard 'c' is 99 decimal
  1794. if (incomingByte == 99)
  1795. {
  1796. if(IMUsInitialized){
  1797. while(!ReadLineCalData("Cal Status:") && (Serial.available() == 0)){
  1798. Busy();
  1799. delay(BNO055_SAMPLERATE_DELAY_MS);
  1800. }
  1801. IMUsCalibrated = true;
  1802. File emptyFile;
  1803. //have user walk around for several seconds
  1804. PrintLCDLine1("Pace Back&Forth");
  1805. //Current Start Time in tick counts
  1806. StartTime = millis();
  1807. CurrentTime = 0;
  1808. String TimeString = "";
  1809. int WalkTimeSeconds = 30;
  1810. while(CurrentTime < WalkTimeSeconds*1000){
  1811. TimeString = String((float(WalkTimeSeconds) - (float(CurrentTime)/1000)),1);
  1812. PrintLCDLine2("Done In:" + TimeString);
  1813. Serial.println("Walk Time" + TimeString);
  1814. CurrentTime = millis() - StartTime;
  1815. ReadLineOfData("CALPNT", emptyFile, CurrentTime);
  1816. delay(100);
  1817. }
  1818. Serial.println("Done Calibration Successful");
  1819. PrintLCDLine1("Done Calibration");
  1820. PrintLCDLine2("Button 2 Record");
  1821.  
  1822. } else {
  1823. StatusAllBad();
  1824. Serial.println("You Must Initialize the Sensors Prior to Calibration");
  1825. PrintLCDLine1("Cannot Cal.");
  1826. PrintLCDLine2("Must Init 1st.");
  1827. delay(2000);
  1828. }
  1829. incomingByte = Serial.read();
  1830. } else
  1831. // keyboard 'r' is 114 decimal
  1832. if (incomingByte == 114)
  1833. {
  1834. if(IMUsInitialized && IMUsCalibrated){
  1835. String filename = DataLoggingName();
  1836. Serial.println("Creating File:" + filename);
  1837. File dataFile = SD.open(filename, FILE_WRITE);
  1838.  
  1839. //Run TPose
  1840. RunTPose(dataFile);
  1841.  
  1842. PrintLCDLine1("Recording 2 File");
  1843. PrintLCDLine2(filename);
  1844. //Current Start Time in tick counts
  1845. StartTime = millis();
  1846.  
  1847. //Printing Start Time..
  1848. TimeLine = "STARTIME " + GetTime();
  1849. Serial.println(TimeLine);
  1850. if(dataFile){
  1851. PrintLine2File(TimeLine,dataFile);
  1852. }
  1853.  
  1854. //main read loop
  1855. ButtonPushed = false;
  1856. while(Serial.available() == 0 && !ButtonPushed ) {
  1857. //setting LED to blining red..
  1858. Recording();
  1859. //checking button to see if we stop.
  1860. ButtonPushed = !digitalRead(buttonPin);
  1861.  
  1862. CurrentTime = millis() - StartTime;
  1863. ReadLineOfData("PNTSTART", dataFile, CurrentTime);
  1864. delay(CaptureDelay);
  1865. }
  1866. PrintLCDLine1("Done:" + filename);
  1867. PrintLCDLine2("Button 2 Record");
  1868. if(dataFile){
  1869. dataFile.close();
  1870. }
  1871. StatusAllBad();
  1872. delay(1000);
  1873. }else {
  1874. StatusAllBad();
  1875. Serial.println("You Must Initialize and Calibrate ALL of the Sensors Prior to Calibration");
  1876. delay(2000);
  1877. }
  1878. incomingByte = Serial.read();
  1879. } else
  1880. if (incomingByte == 100)
  1881. {
  1882. sdCardPresentandIntialized = DetectAndDisplaySDCardInfo();
  1883. } else
  1884. if (incomingByte == 101)
  1885. {
  1886. enumerateI2CBus();
  1887. } else
  1888. if (incomingByte == 106)
  1889. {
  1890. PrintLCDLine1("Current Time:");
  1891. PrintLCDLine2(GetTime());
  1892. Serial.println(GetTime());
  1893. } else
  1894. if (incomingByte == 115)
  1895. {
  1896. setMuxToPort();
  1897. }else
  1898. if (incomingByte == 105)
  1899. {
  1900. InitAllIMUs();
  1901. IMUsInitialized = true;
  1902. Serial.println("IMUs Initialized.");
  1903. } else
  1904. if (incomingByte == 113){
  1905. Serial.println("QuatMode Mode Enabled.");
  1906. QuatMode = true;
  1907. }else
  1908. if (incomingByte == 119){
  1909. Serial.println("Euler Mode Enabled.");
  1910. QuatMode = false;
  1911. }else
  1912. {
  1913. // say what you got:
  1914. Serial.print("I didn't understand the char I received with value: ");
  1915. Serial.println(incomingByte, DEC);
  1916. }
  1917. }
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!