I’ve got a working Bluetooth connection to act as a remote switch. Which works on its own but once I add in the addition IMU and PID control code the Bluetooth connections disconnects right as I send my on signal. There are three PID loops running in my code and if I removed one of them the code runs just fine but its no specific loop. I’ve tried commenting out each loop while having all other remaining and it seems to be the number of loops and not which ones are running.
#include <PID_v1.h>
#include <ArduinoBLE.h>
#include "LSM6DS3.h"
#include "Wire.h"
BLEService onSwitch("19B10000-E8F2-537E-4F6C-D104768A1214"); //Bluetooth Low Energy Switch Service
//Bluetooth Low Energy Switch Characteristic - custom 128-bit UUID, read and writable by central
BLEByteCharacteristic switchCharacteristic("19B10001-E8F2-537E-4F6C-D104768A1214", BLERead | BLEWrite);
//Create a instance of class LSM6DS3
LSM6DS3 myIMU(I2C_MODE, 0x6A); //I2C device address 0x6A
double InputX, OutputX, InputY, OutputY, Setpoint, InputZ, OutputZ, PWMZ;
int CLKWiseX = 0; //Motors for tilt control about the X axis
int AntiCLKWiseX = 2;
int CLKWiseY = 4; //Motors for tilt control about the Y axis
int AntiCLKWiseY = 6;
int ZAxis = 9; //Motor for rotation control about the Z axis
//PID parameters, for X and Y axis control these parameters are assumed to be equivalent
double kp = 0.5;
double ki = 1;
double kd = 0.01;
double kpZ = 0.5;
double kiZ = 1;
double kdZ = 0.01;
bool On = false;
PID myPIDX(&InputX, &OutputX, &Setpoint, kp, ki, kd, DIRECT); //Initialize PID, X axis
PID myPIDY(&InputY, &OutputY, &Setpoint, kp, ki, kd, DIRECT); //Initialize PID, Y axis
PID myPIDZ(&InputZ, &OutputZ, &Setpoint, kpZ, kiZ, kdZ, DIRECT); //Initialize PID, Z axis
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
//Initialize bluetooth and IMU communication.
myIMU.begin();
BLE.begin();
BLE.setLocalName("Micro");
BLE.setAdvertisedService(onSwitch);
onSwitch.addCharacteristic(switchCharacteristic);
BLE.addService(onSwitch);
switchCharacteristic.writeValue(0);
BLE.advertise();
//Set PID to automatic and define output limits to allow for negative rotation
myPIDX.SetMode(AUTOMATIC);
myPIDX.SetOutputLimits(-255, 255);
myPIDY.SetMode(AUTOMATIC);
myPIDY.SetOutputLimits(-255, 255);
myPIDZ.SetMode(AUTOMATIC);
myPIDZ.SetOutputLimits(-127, 127);
Setpoint = 0;
pinMode(CLKWiseX, OUTPUT);
pinMode(AntiCLKWiseX, OUTPUT);
pinMode(CLKWiseY, OUTPUT);
pinMode(AntiCLKWiseY, OUTPUT);
pinMode(ZAxis, OUTPUT);
//analogWrite(ZAxis, 127); //The motor for Z axis control is set to be a momentum wheel all others are reaction wheels
}
void loop() {
// put your main code here, to run repeatedly:
//Listen for Bluetooth Low Energy peripherals to connect:
BLEDevice central = BLE.central();
//While the central is still connected to peripheral:
if (central.connected()) {
if (switchCharacteristic.written()) {
if (switchCharacteristic.value()) {
On = false;
} else {
On = true;
}
}
}
if (On == true) {
xAxis();
yAxis();
zAxis();
Serial.println("Data Sent.");
delay(1000);
}
if (On == false) {
shutDown();
}
}
//Control of rotation about the Z axis
void zAxis() {
InputZ = myIMU.readFloatGyroZ();
myPIDZ.Compute();
PWMZ = 127 + OutputZ;
analogWrite(ZAxis, PWMZ);
}
//Control of rotation about the X axis
void xAxis() {
InputX = myIMU.readFloatGyroX();
myPIDX.Compute();
if (OutputX < 0) {
analogWrite(CLKWiseX, 0);
analogWrite(AntiCLKWiseX, abs(OutputX));
}
if (OutputX > 0) {
analogWrite(AntiCLKWiseX, 0);
analogWrite(CLKWiseX, OutputX);
} else {
analogWrite(CLKWiseX, 0);
analogWrite(AntiCLKWiseX, 0);
}
}
//Control of rotation about the Y axis
void yAxis() {
InputY = myIMU.readFloatGyroY();
myPIDY.Compute();
if (OutputY < 0) {
analogWrite(CLKWiseY, 0);
analogWrite(AntiCLKWiseY, abs(OutputY));
}
if (OutputY > 0) {
analogWrite(AntiCLKWiseY, 0);
analogWrite(CLKWiseY, OutputY);
} else {
analogWrite(CLKWiseY, 0);
analogWrite(AntiCLKWiseY, 0);
}
}
//Turn off all connected devices.
void shutDown() {
analogWrite(CLKWiseX, 0);
analogWrite(AntiCLKWiseX, 0);
analogWrite(CLKWiseY, 0);
analogWrite(AntiCLKWiseY, 0);
analogWrite(ZAxis, 0);
}