Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Wire.h>
- #include <SPI.h>
- #include <Adafruit_LSM9DS1.h>
- #include <Adafruit_Sensor.h> // not used in this demo but required!
- // i2c
- Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();
- #include <Arduino.h>
- #include <SPI.h>
- #include "Adafruit_BLE.h"
- #include "Adafruit_BluefruitLE_SPI.h"
- #include "Adafruit_BluefruitLE_UART.h"
- #include "Adafruit_BLEGatt.h"
- #include "BluefruitConfig.h"
- // Create the bluefruit object
- /* ...hardware SPI, using SCK/MOSI/MISO hardware SPI pins and then user selected CS/IRQ/RST */
- Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);
- Adafruit_BLEGatt gatt(ble);
- // A small helper
- void error(const __FlashStringHelper*err) {
- Serial.println(err);
- while (1);
- }
- /* The service information */
- int32_t htsServiceId;
- int32_t htsMeasureCharId;
- void setupSensor() //IMU sensor
- {
- // 1.) Set the accelerometer range
- lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G);
- //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G);
- //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G);
- //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G);
- // 2.) Set the magnetometer sensitivity
- lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);
- //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS);
- //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS);
- //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS);
- // 3.) Setup the gyroscope
- lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);
- //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS);
- //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS);
- }
- uint8_t measurement [12] = { 0 }; // Laver et array med 12 pladser - Kaldt measurement til bluetooth del
- int B[10]; //x
- int A[10]; //y(x)
- int runde = 1;
- float gennemsnit = 0;
- float degrees = 0;
- void setup()
- {
- Serial.begin(115200);
- while (!Serial) {
- delay(1); // will pause Zero, Leonardo, etc until serial console opens
- }
- // Try to initialise and warn if we couldn't detect the chip
- if (!lsm.begin())
- {
- Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
- while (1);
- }
- Serial.println("Found LSM9DS1 9DOF");
- // helper to just set the default scaling we want, see above!
- setupSensor();
- /* Initialise the Bluetooth module */
- Serial.print(F("Initialising the Bluefruit LE module: "));
- if ( !ble.begin(VERBOSE_MODE) )
- {
- error(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
- }
- Serial.println( F("OK!") );
- /* Perform a factory reset to make sure everything is in a known state */
- Serial.println(F("Performing a factory reset: "));
- if (! ble.factoryReset() ) {
- error(F("Couldn't factory reset"));
- }
- /* Disable command echo from Bluefruit */
- ble.echo(false);
- Serial.println("Requesting Bluefruit info:");
- /* Print Bluefruit information */
- ble.info();
- // this line is particularly required for Flora, but is a good idea
- // anyways for the super long lines ahead!
- // ble.setInterCharWriteDelay(5); // 5 ms
- Serial.println(F("Adding the Health Thermometer Service definition (UUID = 0x1809): "));
- htsServiceId = gatt.addService(0x1809);
- if (htsServiceId == 0) {
- error(F("Could not add Thermometer service"));
- }
- Serial.println(F("Adding the Temperature Measurement characteristic (UUID = 0x2A1C): "));
- htsMeasureCharId = gatt.addCharacteristic(0x2A1C, GATT_CHARS_PROPERTIES_INDICATE, 12, 12, BLE_DATATYPE_BYTEARRAY);
- if (htsMeasureCharId == 0) {
- error(F("Could not add Temperature characteristic"));
- }
- /* Add the Health Thermometer Service to the advertising data (needed for Nordic apps to detect the service) */
- Serial.print(F("Adding Health Thermometer Service UUID to the advertising payload: "));
- uint8_t advdata[] { 0x02, 0x01, 0x06, 0x05, 0x02, 0x09, 0x18, 0x0a, 0x18 };
- ble.setAdvData( advdata, sizeof(advdata) );
- /* Reset the device for the new service setting changes to take effect */
- Serial.print(F("Performing a SW reset (service changes require a reset): "));
- ble.reset();
- Serial.println();
- KalibreNul(); //første runde og kalibreringsdata af 0-grader er slut
- }
- /** Send randomized heart rate data continuously **/
- void loop() {
- lsm.read(); /* ask it to read in the data */
- char data = Serial.read();
- Serial.print("runde \t");
- Serial.println(runde);
- if (data = 'k') { // hvis 'k' => Kalibre360
- Kalibre360();
- }
- /* Get a new sensor event */
- sensors_event_t a, m, g, temp;
- lsm.getEvent(&a, &m, &g, &temp);
- int gyroData_y = lsm.gyroData.y; //skriv y
- degrees = gyroData_y - gennemsnit;
- Serial.print (gyroData_y);
- Serial.print(" - ");
- Serial.print (gennemsnit);
- Serial.print(" = ");
- Serial.println(degrees);
- //Serial.println(lsm.gyroData.z);
- degrees
- DataToBluetooth();
- delay(100);
- runde++;
- }
- void KalibreNul() {
- Serial.println("n; \t x[n-1]; \t y= x[n-1]+n \t y[n-1] \t integreret");
- for ( int i = 0 ; i < 10; i++ ) {
- lsm.read(); //lsm (sensoer) læser data
- B[i] = lsm.gyroData.y; //læser w_acc om y-akse
- Serial.println(B[i]); //n
- /*Serial.print("\t ");
- int x = B[i - 1]; //x[n-1]
- 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
- Serial.print("\t\t ");
- int y = B[i] + B[i - 1];
- A[i] = y; // gem y på plads i (i Array A)
- Serial.print(y);//y=x[n-1]+n
- Serial.print("\t\t ");
- Serial.print(A[i - 1]); //y[n-1]
- int z = A[i - 1];
- Serial.print("\t\t ");
- Serial.println(y + z); //Y(n)= x[n]+y[n-1] = integreret
- int sum = B[9] + B[9 - 1]+ A[9 - 1];
- Gns = sum/10;*/
- }
- Serial.print("\t Gns; ");
- float sum = 0; //for at kunne læse sum efter for-loop
- for ( int i = 0 ; i < 10; i++ ) {
- sum += B[i]; //+ B[2] + B[3] + B[4] + B[5] + B[6] + B[7] + B[8] + B[9] + B[10]) / 10);
- }
- gennemsnit = sum / 10;
- Serial.println (gennemsnit);
- //int GnsNU = (Gns - B[10]);
- //Serial.print(GnsNU);
- // for ( int runde = 0 ; runde < 10; runde++ ) { //hvor mange tal der udskrives efter lige med (11-1= 10)
- lsm.read(); //lsm (sensoer) læser data
- A[runde] = lsm.gyroData.y; //skriv y
- int a = A[runde];
- Serial.print (a);
- Serial.print(" - ");
- Serial.print (gennemsnit);
- Serial.print(" = ");
- int gyro_y = a - gennemsnit;
- Serial.println(gyro_y);
- //int x = A[runde - 1];
- //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
- //Serial.print("\t\t ");
- //int y = A[runde] + A[runde - 1];
- //Serial.print(y);
- //Serial.print("\t\t ");
- //Serial.println (y + x);
- //}
- runde++;
- }
- void Kalibre360() {
- }
- void DataToBluetooth() {
- measurement[1] = ((unsigned int)lsm.accelData.x) / 256;
- measurement[0] = ((unsigned int)lsm.accelData.x) & 255;
- measurement[3] = ((unsigned int)lsm.accelData.y) / 256;
- measurement[2] = ((unsigned int)lsm.accelData.y) & 255;
- measurement[5] = ((unsigned int)lsm.accelData.z) / 256;
- measurement[4] = ((unsigned int)lsm.accelData.z) & 255;
- measurement[7] = ((unsigned int)lsm.gyroData.x) / 256;
- measurement[6] = ((unsigned int)lsm.gyroData.x) & 255;
- measurement[9] = ((unsigned int)lsm.gyroData.y) / 256;
- measurement[8] = ((unsigned int)lsm.gyroData.y) & 255;
- measurement[11] = ((unsigned int)lsm.gyroData.z) / 256;
- measurement[10] = ((unsigned int)lsm.gyroData.z) & 255; //et array på 12 byte, som overføres til Inventor
- // TODO temperature is not correct due to Bluetooth use IEEE-11073 format
- 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
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement