#include <ArduinoBLE.h>
#include <Arduino_LSM6DSOX.h>
BLEService RCService("27810000-4786-42e7-bd27-45257b2e988f"); // BLE LED Service
// BLE AppInventorRC Characteristic - custom 128-bit UUID, read and writable by central
BLEIntCharacteristic RC_AngleCharacteristic("27810001-4786-42e7-bd27-45257b2e988f", BLERead | BLEWrite | BLEWriteWithoutResponse);
BLEIntCharacteristic RC_SpeedCharacteristic("27810002-4786-42e7-bd27-45257b2e988f", BLERead | BLEWrite | BLEWriteWithoutResponse);
BLEIntCharacteristic RC_HeightCharacteristic("27810003-4786-42e7-bd27-45257b2e988f", BLERead | BLEWrite | BLEWriteWithoutResponse);
BLEIntCharacteristic RC_RotationCharacteristic("27810004-4786-42e7-bd27-45257b2e988f", BLERead | BLEWrite | BLEWriteWithoutResponse);
int LastSpeed = 0;
int LastAngle = 0;
int LastHeight = 0;
int LastRot = 0;
int tick = 0;
bool CentralConnected = 0;
//IMU
float x, y, z;
void setup() {
Serial.begin(9600);
// begin initialization
if (!BLE.begin()) {
Serial.println("starting Bluetooth® Low Energy failed!");
}
// set advertised local name and service UUID:
BLE.setLocalName("Pinky_RX");
BLE.setAdvertisedService(RCService);
// add the characteristics to the service
RCService.addCharacteristic(RC_AngleCharacteristic);
RCService.addCharacteristic(RC_SpeedCharacteristic);
RCService.addCharacteristic(RC_HeightCharacteristic);
RCService.addCharacteristic(RC_RotationCharacteristic);
// add service
BLE.addService(RCService);
// start advertising
BLE.advertise();
Serial.println("Pinky Peripheral, waiting for connections....");
//IMU stuff
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!");
while (1);
}
Serial.print("Gyroscope sample rate = ");
Serial.print(IMU.gyroscopeSampleRate());
Serial.println(" Hz");
Serial.println();
Serial.println("Gyroscope in degrees/second");
Serial.println("X\tY\tZ");
}
void loop() {
// listen for BLE peripherals to connect:
BLEDevice central = BLE.central();
// if a central is connected to peripheral:
if (central) {
if (!CentralConnected){
Serial.print("Connected to central: ");
// print the central's MAC address:
Serial.println(central.address());
// while the central is still connected to peripheral:
CentralConnected = 1;
}
while (central.connected()) {
//Serial.print("Waiting for the write thing: ");
//delay(100);//This causes hangs / crashes
if (RC_AngleCharacteristic.written() || RC_SpeedCharacteristic.written() || RC_HeightCharacteristic.written() || RC_RotationCharacteristic.written()) {
//Debounce the BLE
if (LastAngle != RC_AngleCharacteristic.value() || LastSpeed != RC_SpeedCharacteristic.value() || LastHeight != RC_HeightCharacteristic.value() || LastRot != RC_RotationCharacteristic.value()){
tick++;
Serial.println(tick);
Serial.print("Angle: ");
Serial.print(RC_AngleCharacteristic.value());
Serial.print(" Speed: ");
Serial.print(RC_SpeedCharacteristic.value());
Serial.print(" Height: ");
Serial.print(RC_HeightCharacteristic.value());
Serial.print(" Rotation: ");
Serial.println(RC_RotationCharacteristic.value());
LastAngle = RC_AngleCharacteristic.value();
LastSpeed = RC_SpeedCharacteristic.value();
LastHeight = RC_HeightCharacteristic.value();
LastRot = RC_RotationCharacteristic.value();
}
}
// if (IMU.gyroscopeAvailable()) {
// IMU.readGyroscope(x, y, z);
// Serial.print(x);
// Serial.print('\t');
// Serial.print(y);
// Serial.print('\t');
// Serial.println(z);
//}
}
if(CentralConnected){
// when the central disconnects, print it out:
Serial.print(F("Disconnected from central: "));
Serial.println(central.address());
CentralConnected = 0;
}
}
}