Skip to content
Snippets Groups Projects
Commit d1b0a443 authored by Denis Bobrovskiy's avatar Denis Bobrovskiy
Browse files

Added wristwatch testing code

parent ca6152f2
No related branches found
No related tags found
No related merge requests found
#include <Arduino.h>
#include<Wire.h>
#include <math.h>
const int MPU_addr=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
double AccelerationX,AccelerationY,AccelerationZ;
double R,AngX,AngY,AngZ, NoGAccelX, NoGAccelY, NoGAccelZ; // Not sure if to use doubles or int_t or doubles_t or floats
float velocityX =0;
float velocityY =0;
float velocityZ =0;
bool first =true;
unsigned long last_time_sampled =0; // Used for finding velovity
unsigned long last_time_printed =0; // Used to to print
# define convertAdcMss ( 9.80665 / (pow(2,14))) // Gravity / 2^14 .
// This is because the range of the MPU is +-2G hence 2^16 represents 4 G.
#define DEBOUNCE_TIME 1000 // This is to debounce the buttons to problems with button.
volatile uint32_t DebounceTimerUp = 0; // Used to calcuale the current time since the button was allowed.
volatile uint32_t DebounceTimerDown = 0; // Used to calcuale the current time since the button was allowed.
#define PIN_BUTTON 4 // Pin to which the button, PIR motion detector or radar is connected
bool buttonpressdown = false; // Used to flag if the button is pressed.
bool buttonpressup = false; // Used to flag is button is up aka foot is in the air.
// #define PIN_LED 2 // On board LED used for debugging
// #define DELAY_LED 2000
// These functions are not used yet.
// The function is placed in the RAM of the ESP32.
void IRAM_ATTR buttonpresseddown() {
if ( millis() - DEBOUNCE_TIME >= DebounceTimerDown ) {
DebounceTimerDown = millis();
buttonpressdown=true;
}
}
void IRAM_ATTR buttonpressedup() {
if ( millis() - DEBOUNCE_TIME >= DebounceTimerUp ) {
DebounceTimerUp = millis();
buttonpressup=true;
}
}
void setup(){
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Serial.begin(9600);
pinMode(PIN_BUTTON, INPUT_PULLDOWN);
attachInterrupt(PIN_BUTTON, buttonpresseddown, RISING); // This is creating an interupt when the button is pressed.
}
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); //
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); //
AcX=Wire.read()<<8|Wire.read(); // Reads the accelormeter
AcY=Wire.read()<<8|Wire.read(); //
AcZ=Wire.read()<<8|Wire.read(); //
// Tmp=Wire.read()<<8|Wire.read(); // Reads temp
// GyX=Wire.read()<<8|Wire.read(); // Reads the gyroscope
// GyY=Wire.read()<<8|Wire.read(); //
// GyZ=Wire.read()<<8|Wire.read(); //
AccelerationX=AcX*convertAdcMss; // Converts the input voltage into m/ss
AccelerationY=AcY*convertAdcMss;
AccelerationZ=AcZ*convertAdcMss;
R = sqrt(pow(AccelerationX,2)+pow(AccelerationY,2)+pow(AccelerationZ,2)); // Resultant vector of acceleration.
NoGAccelX=AccelerationX - 9.80665*(AccelerationX/R); // Removes the acceleration due to graveity component of the acceleration.
NoGAccelY=AccelerationY - 9.80665*(AccelerationY/R);
NoGAccelZ=AccelerationZ - 9.80665*(AccelerationZ/R);
// AngX= acos(AccelerationX/R) *180/PI;
// AngY= acos(AccelerationY/R)*180/PI; // Calculates the angle of each axis
//AngZ= acos(AccelerationZ/R)*180/PI;
unsigned long now = micros(); // This is the calcualtion of velocity, acceleration * time at each stage.
if(abs(NoGAccelX)>0.5) // Time at each stage changed in calculated.
velocityX += NoGAccelX * (now - last_time_sampled)/1000000;
if(abs(NoGAccelY)>0.5)
velocityY += NoGAccelY * (now - last_time_sampled)/1000000;
if(abs(NoGAccelZ)>0.5)
velocityZ += NoGAccelZ * (now - last_time_sampled)/1000000;
last_time_sampled = now;
if (now - last_time_printed >= 200000) {
Serial.print("x = ");
Serial.print(NoGAccelX); // Just changed to print every 0.2 seconds.
Serial.print(", x = ");
Serial.print(velocityX );
Serial.print(" y = ");
Serial.print(NoGAccelY);
Serial.print(", y = ");
Serial.print(velocityY );
Serial.print(" z = ");
Serial.print(NoGAccelZ);
Serial.print(", z = ");
Serial.println(velocityZ );
last_time_printed = now;
}
delay(30);
}
\ No newline at end of file
#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
// See the following for generating UUIDs:
// https://www.uuidgenerator.net/
#define SERVICE_UUID "61a7f44e-5a29-4b16-a219-3dfd49d28ebc"
#define CHARACTERISTIC_UUID1 "beb5483e-36e1-4688-b7f5-ea07361b26a8"
#define CHARACTERISTIC_UUID2 "717740b7-02f8-4e5e-a236-2ec94d303934"
#define CHARACTERISTIC_UUID3 "77e846fd-d2bd-4ea1-b2ef-639bac74bac8"
void setup() {
Serial.begin(115200);
Serial.println("Program started");
BLEDevice::init("ExampleBluetoothServer");
BLEServer *server = BLEDevice::createServer();
BLEService *testService = server->createService(SERVICE_UUID);
BLECharacteristic *accelerometerCharX = testService->createCharacteristic(
CHARACTERISTIC_UUID1,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE
);
BLECharacteristic *accelerometerCharY = testService->createCharacteristic(
CHARACTERISTIC_UUID2,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE
);
BLECharacteristic *accelerometerCharZ = testService->createCharacteristic(
CHARACTERISTIC_UUID3,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE
);
accelerometerCharX->setValue("Accelerometer value X");
accelerometerCharY->setValue("Accelerometer value Y");
accelerometerCharZ->setValue("Accelerometer value Z");
testService->start();
//Set up broadcasting
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->setScanResponse(true);
pAdvertising->setMinPreferred(0x08);
pAdvertising->setMinPreferred(0x16);
BLEDevice::startAdvertising();
Serial.println("GATT server set up");
}
void loop() {
// put your main code here, to run repeatedly:
delay(2000);
}
#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLE2902.h>
// See the following for generating UUIDs:
// https://www.uuidgenerator.net/
//UUIDs
#define SERVICE_UUID "9c234f41-3a33-4179-a7a8-ef365241e45d"
#define GPS_CHAR_UUID "3d4512a1-e027-473d-b880-f58e75ad8a9e"
#define STEP_COUNTER_CHAR_UUID "5609995a-8003-4f11-bd73-8ae0eee8d004"
#define HEART_RATE_CHAR_UUID "50787168-dec5-437c-a6ab-e52fa492c85c"
#define GPS_DATA_UPLOAD_INTERVAL 3 //in seconds
BLECharacteristic *gpsChar;
BLECharacteristic *stepCounterChar;
BLECharacteristic *heartRateChar;
//For testing
uint8_t tempCounter1 = 0;
void gpsThread(void *parameter){
for(;;){
Serial.println("Looping gps thread");
//35 bytes of test data
unsigned char *gpsDataPtr = (unsigned char *)malloc(35);
gpsDataPtr[0] = tempCounter1;
gpsDataPtr[1] = tempCounter1+1;
gpsDataPtr[2] = tempCounter1+2;
gpsDataPtr[3] = tempCounter1+3;
//Send this data and notify(important)
gpsChar->setValue(gpsDataPtr,35);
gpsChar->notify();
tempCounter1++;
free(gpsDataPtr);
delay(GPS_DATA_UPLOAD_INTERVAL * 1000);
}
}
void stepCounterThread(void *parameter){
for(;;){
Serial.println("Looping step counter thread");
unsigned char tempToggle = 0;
//Toggle this value between 0 and 1 for every new step detected
tempToggle = tempToggle?0:1;
stepCounterChar->setValue(&tempToggle,1);
stepCounterChar->notify();
delay(500);
}
}
void heartRateThread(void *parameter){
for(;;){
Serial.println("Looping heart rate thread");
unsigned char tempToggle = 0;
//Toggle this value between 0 and 1 for every new step detected
tempToggle = tempToggle?0:1;
heartRateChar->setValue(&tempToggle,1);
heartRateChar->notify();
delay(500);
}
}
void setup() {
//SETTING UP BLUETOOTH STUFF
Serial.begin(9600);
Serial.println("Program started");
//Initialize BLE server
BLEDevice::init("Wristwatch");
BLEServer *server = BLEDevice::createServer();
BLEService *mainService = server->createService(SERVICE_UUID);
//Initialize characteristics (with read/write access and notification permissions)
gpsChar = mainService->createCharacteristic(
GPS_CHAR_UUID,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_NOTIFY |
BLECharacteristic::PROPERTY_INDICATE
);
stepCounterChar = mainService->createCharacteristic(
STEP_COUNTER_CHAR_UUID,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_NOTIFY |
BLECharacteristic::PROPERTY_INDICATE
);
heartRateChar = mainService->createCharacteristic(
HEART_RATE_CHAR_UUID,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_NOTIFY |
BLECharacteristic::PROPERTY_INDICATE
);
//Add descriptor to set up notifications
gpsChar->addDescriptor(new BLE2902());
stepCounterChar->addDescriptor(new BLE2902());
heartRateChar->addDescriptor(new BLE2902());
//Set initial values (this doesnt really matter)
gpsChar->setValue("GPS initial data");
stepCounterChar->setValue("Step counter initial data");
heartRateChar->setValue("heart rate initial data");
mainService->start();
//Set up broadcasting
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->setScanResponse(true);
pAdvertising->setMinPreferred(0x08);
pAdvertising->setMinPreferred(0x16);
BLEDevice::startAdvertising();
Serial.println("GATT server set up");
//SETUP THREADS
TaskHandle_t gpsThreadHandle;
TaskHandle_t heartRateThreadHandle;
TaskHandle_t stepCounterThreadHandle;
xTaskCreatePinnedToCore(
gpsThread, /* Function to implement the task */
"GpsThread", /* Name of the task */
10000, /* Stack size in words */
NULL, /* Task input parameter */
0, /* Priority of the task */
&gpsThreadHandle, /* Task handle. */
0); /* Core where the task should run */
xTaskCreatePinnedToCore(
stepCounterThread, /* Function to implement the task */
"StepCounterThread", /* Name of the task */
10000, /* Stack size in words */
NULL, /* Task input parameter */
0, /* Priority of the task */
&stepCounterThreadHandle, /* Task handle. */
0); /* Core where the task should run */
xTaskCreatePinnedToCore(
heartRateThread, /* Function to implement the task */
"HeartRateThread", /* Name of the task */
10000, /* Stack size in words */
NULL, /* Task input parameter */
0, /* Priority of the task */
&heartRateThreadHandle, /* Task handle. */
0); /* Core where the task should run */
}
void loop() {
Serial.println("Looping");
// //Upload test data to GPS characteristic
// gpsChar->setValue(&tempCounter,1);
// gpsChar->notify();
// Serial.println("Updated characterstic");
// tempCounter++;
//Upload test data to step counter module
delay(2000);
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment