SHARE
TWEET

Untitled

a guest Dec 14th, 2019 99 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Wire.h>
  2. #include <SPI.h>
  3. #include <Adafruit_LSM9DS1.h>
  4. #include <Adafruit_Sensor.h>  // not used in this demo but required!
  5.  
  6. // i2c
  7. Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();
  8.  
  9.  
  10. #include <Arduino.h>
  11. #include <SPI.h>
  12. #include "Adafruit_BLE.h"
  13. #include "Adafruit_BluefruitLE_SPI.h"
  14. #include "Adafruit_BluefruitLE_UART.h"
  15. #include "Adafruit_BLEGatt.h"
  16.  
  17. #include "BluefruitConfig.h"
  18.  
  19.  
  20. // Create the bluefruit object
  21. /* ...hardware SPI, using SCK/MOSI/MISO hardware SPI pins and then user selected CS/IRQ/RST */
  22. Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);
  23.  
  24. Adafruit_BLEGatt gatt(ble);
  25.  
  26. // A small helper
  27. void error(const __FlashStringHelper*err) {
  28.   Serial.println(err);
  29.   while (1);
  30. }
  31.  
  32. /* The service information */
  33.  
  34. int32_t htsServiceId;
  35. int32_t htsMeasureCharId;
  36.  
  37. void setupSensor()  //IMU sensor
  38. {
  39.   // 1.) Set the accelerometer range
  40.   lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G);
  41.   //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G);
  42.   //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G);
  43.   //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G);
  44.  
  45.   // 2.) Set the magnetometer sensitivity
  46.   lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);
  47.   //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS);
  48.   //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS);
  49.   //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS);
  50.  
  51.   // 3.) Setup the gyroscope
  52.   lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);
  53.   //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS);
  54.   //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS);
  55. }
  56.  
  57.  
  58.  
  59.  
  60.  
  61.  
  62. uint8_t measurement [12] = { 0 }; // Laver et array med 12 pladser - Kaldt measurement til bluetooth del
  63.  
  64.  
  65. int B[10]; //x
  66. int A[10]; //y(x)
  67. int runde = 1;
  68. float gennemsnit = 0;
  69. float degrees = 0;
  70.  
  71.  
  72.  
  73. void setup()
  74. {
  75.   Serial.begin(115200);
  76.  
  77.   while (!Serial) {
  78.     delay(1); // will pause Zero, Leonardo, etc until serial console opens
  79.   }
  80.  
  81.  
  82.   // Try to initialise and warn if we couldn't detect the chip
  83.   if (!lsm.begin())
  84.   {
  85.     Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
  86.     while (1);
  87.   }
  88.   Serial.println("Found LSM9DS1 9DOF");
  89.  
  90.   // helper to just set the default scaling we want, see above!
  91.   setupSensor();
  92.  
  93.   /* Initialise the Bluetooth module */
  94.   Serial.print(F("Initialising the Bluefruit LE module: "));
  95.  
  96.   if ( !ble.begin(VERBOSE_MODE) )
  97.   {
  98.     error(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
  99.   }
  100.   Serial.println( F("OK!") );
  101.  
  102.   /* Perform a factory reset to make sure everything is in a known state */
  103.   Serial.println(F("Performing a factory reset: "));
  104.   if (! ble.factoryReset() ) {
  105.     error(F("Couldn't factory reset"));
  106.   }
  107.  
  108.   /* Disable command echo from Bluefruit */
  109.   ble.echo(false);
  110.  
  111.   Serial.println("Requesting Bluefruit info:");
  112.   /* Print Bluefruit information */
  113.   ble.info();
  114.  
  115.   // this line is particularly required for Flora, but is a good idea
  116.   // anyways for the super long lines ahead!
  117.   // ble.setInterCharWriteDelay(5); // 5 ms
  118.  
  119.   Serial.println(F("Adding the Health Thermometer Service definition (UUID = 0x1809): "));
  120.   htsServiceId = gatt.addService(0x1809);
  121.   if (htsServiceId == 0) {
  122.     error(F("Could not add Thermometer service"));
  123.   }
  124.  
  125.   Serial.println(F("Adding the Temperature Measurement characteristic (UUID = 0x2A1C): "));
  126.   htsMeasureCharId = gatt.addCharacteristic(0x2A1C, GATT_CHARS_PROPERTIES_INDICATE, 12, 12, BLE_DATATYPE_BYTEARRAY);
  127.   if (htsMeasureCharId == 0) {
  128.     error(F("Could not add Temperature characteristic"));
  129.   }
  130.  
  131.   /* Add the Health Thermometer Service to the advertising data (needed for Nordic apps to detect the service) */
  132.   Serial.print(F("Adding Health Thermometer Service UUID to the advertising payload: "));
  133.   uint8_t advdata[] { 0x02, 0x01, 0x06, 0x05, 0x02, 0x09, 0x18, 0x0a, 0x18 };
  134.   ble.setAdvData( advdata, sizeof(advdata) );
  135.  
  136.   /* Reset the device for the new service setting changes to take effect */
  137.   Serial.print(F("Performing a SW reset (service changes require a reset): "));
  138.   ble.reset();
  139.  
  140.   Serial.println();
  141.  
  142.  
  143.   KalibreNul();  //første runde og kalibreringsdata af 0-grader er slut
  144. }
  145. /** Send randomized heart rate data continuously **/
  146.  
  147.  
  148. void loop() {
  149.  
  150.   lsm.read();  /* ask it to read in the data */
  151.   char data = Serial.read();
  152.   Serial.print("runde \t");
  153.   Serial.println(runde);
  154.  
  155.  
  156.   if (data = 'k') { // hvis 'k' => Kalibre360
  157.       Kalibre360();
  158.   }
  159.  
  160.  
  161.   /* Get a new sensor event */
  162.   sensors_event_t a, m, g, temp;
  163.   lsm.getEvent(&a, &m, &g, &temp);
  164.  
  165.  
  166.   int gyroData_y = lsm.gyroData.y; //skriv y
  167.   degrees = gyroData_y - gennemsnit;
  168.  
  169.   Serial.print (gyroData_y);
  170.   Serial.print("  -  ");
  171.   Serial.print (gennemsnit);
  172.   Serial.print("  =  ");
  173.   Serial.println(degrees);
  174.   //Serial.println(lsm.gyroData.z);
  175.  
  176.   degrees
  177.  
  178.  
  179.   DataToBluetooth();
  180.  
  181.   delay(100);
  182.   runde++;
  183.  
  184. }
  185.  
  186.  
  187.  
  188. void KalibreNul() {
  189.     Serial.println("n; \t x[n-1]; \t y= x[n-1]+n \t y[n-1] \t integreret");
  190.     for ( int i = 0 ; i < 10; i++ ) {
  191.       lsm.read();  //lsm (sensoer) læser data
  192.       B[i] = lsm.gyroData.y; //læser w_acc om y-akse
  193.       Serial.println(B[i]); //n
  194.       /*Serial.print("\t ");
  195.         int x = B[i - 1]; //x[n-1]
  196.         Serial.print(x); // + B[runde - 1]+ B[runde - 2]+ B[runde - 3]+ B[runde - 4]+ B[runde - 5]+ B[runde - 6]+ B[runde - 7]+ B[runde - 8]); //data nuvæBende væBdi sammenlægges med davæBende væBdi
  197.         Serial.print("\t\t ");
  198.         int y = B[i] + B[i - 1];
  199.         A[i] = y; // gem y på plads i (i Array A)
  200.         Serial.print(y);//y=x[n-1]+n
  201.         Serial.print("\t\t ");
  202.         Serial.print(A[i - 1]); //y[n-1]
  203.         int z = A[i - 1];
  204.         Serial.print("\t\t ");
  205.         Serial.println(y + z); //Y(n)= x[n]+y[n-1] = integreret
  206.         int sum = B[9] + B[9 - 1]+ A[9 - 1];
  207.         Gns = sum/10;*/
  208.     }
  209.     Serial.print("\t Gns; ");
  210.  
  211.     float sum = 0; //for at kunne læse sum efter for-loop
  212.     for ( int i = 0 ; i < 10; i++ ) {
  213.       sum += B[i]; //+ B[2] + B[3] + B[4] + B[5] + B[6] + B[7] + B[8] + B[9] + B[10]) / 10);
  214.     }
  215.     gennemsnit = sum / 10;
  216.     Serial.println (gennemsnit);
  217.     //int GnsNU = (Gns - B[10]);
  218.     //Serial.print(GnsNU);
  219.  
  220.     // for ( int runde = 0 ; runde < 10; runde++ ) { //hvor mange tal der udskrives efter lige med (11-1= 10)
  221.     lsm.read();  //lsm (sensoer) læser data
  222.     A[runde] = lsm.gyroData.y; //skriv y
  223.     int a = A[runde];
  224.     Serial.print (a);
  225.     Serial.print("  -  ");
  226.     Serial.print (gennemsnit);
  227.     Serial.print("  =  ");
  228.     int gyro_y = a - gennemsnit;
  229.     Serial.println(gyro_y);
  230.     //int x = A[runde - 1];
  231.     //Serial.print (x); // + B[runde - 1]+ B[runde - 2]+ B[runde - 3]+ B[runde - 4]+ B[runde - 5]+ B[runde - 6]+ B[runde - 7]+ B[runde - 8]); //data nuvæBende væBdi sammenlægges med davæBende væBdi
  232.     //Serial.print("\t\t ");
  233.     //int y = A[runde] + A[runde - 1];
  234.     //Serial.print(y);
  235.     //Serial.print("\t\t ");
  236.     //Serial.println (y + x);
  237.     //}
  238.  
  239.     runde++;
  240. }
  241.  
  242.  
  243. void Kalibre360() {
  244.  
  245. }
  246.  
  247.  
  248. void DataToBluetooth() {
  249.   measurement[1] = ((unsigned int)lsm.accelData.x) / 256;
  250.   measurement[0] = ((unsigned int)lsm.accelData.x) & 255;
  251.   measurement[3] = ((unsigned int)lsm.accelData.y) / 256;
  252.   measurement[2] = ((unsigned int)lsm.accelData.y) & 255;
  253.   measurement[5] = ((unsigned int)lsm.accelData.z) / 256;
  254.   measurement[4] = ((unsigned int)lsm.accelData.z) & 255;
  255.  
  256.   measurement[7] = ((unsigned int)lsm.gyroData.x) / 256;
  257.   measurement[6] = ((unsigned int)lsm.gyroData.x) & 255;
  258.   measurement[9] = ((unsigned int)lsm.gyroData.y) / 256;
  259.   measurement[8] = ((unsigned int)lsm.gyroData.y) & 255;
  260.   measurement[11] = ((unsigned int)lsm.gyroData.z) / 256;
  261.   measurement[10] = ((unsigned int)lsm.gyroData.z) & 255; //et array på 12 byte, som overføres til Inventor
  262.  
  263.   // TODO temperature is not correct due to Bluetooth use IEEE-11073 format
  264.   gatt.setChar(htsMeasureCharId, measurement, 12); //send 12 bytes bliver sat ind, som arg(measurement).. Send 12 elemneter(bytes) i arrayet afsted via bluetooth med krakteristik af htsMeasureCharId
  265.  
  266. }
RAW Paste Data
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
 
Top