Slave code
#include <Servo.h>
#include <SoftwareSerial.h>
SoftwareSerial BT(8, 9); // RX, TX
Servo servoX, servoY, servoZ;
String incomingData = "";
int angleX = 90, angleY = 90, angleZ = 90;
int lastX = 90, lastY = 90, lastZ = 90;
const int deadBand = 2; // ignore small jitter
void setup() {
Serial.begin(9600);
BT.begin(38400);
servoX.attach(3);
servoY.attach(5);
servoZ.attach(6);
servoX.write(90);
servoY.write(90);
servoZ.write(90);
Serial.println("Slave Ready...");
}
void loop() {
while (BT.available()) {
char c = BT.read();
if (c == '\n') {
int c1 = incomingData.indexOf(',');
int c2 = incomingData.lastIndexOf(',');
if (c1 > 0 && c2 > c1) {
angleX = incomingData.substring(0, c1).toInt();
angleY = incomingData.substring(c1 + 1, c2).toInt();
angleZ = incomingData.substring(c2 + 1).toInt();
// === DEAD BAND FILTER ===
if (abs(angleX - lastX) > deadBand) {
servoX.write(angleX);
lastX = angleX;
}
if (abs(angleY - lastY) > deadBand) {
servoY.write(angleY);
lastY = angleY;
}
if (abs(angleZ - lastZ) > deadBand) {
servoZ.write(angleZ);
lastZ = angleZ;
}
}
incomingData = "";
}
else {
incomingData += c;
}
}
}
Master Code
/* ===== MASTER CODE =====
* MPU6050 → HC-05 → Slave (3 Servo)
* Stable + Smooth Angles
*/
#include <Wire.h>
#include <MPU6050.h>
#include <SoftwareSerial.h>
SoftwareSerial BT(8, 9); // RX, TX
MPU6050 mpu;
// Raw sensor data
int16_t ax, ay, az, gx, gy, gz;
// Filtered angles
float angleX = 0, angleY = 0, angleZ = 0;
// Smoothed output angles
float smoothX = 90, smoothY = 90, smoothZ = 90;
unsigned long lastTime = 0;
// Smoothing factor (0.1 – 0.3 best)
const float alpha = 0.15;
void setup() {
Serial.begin(9600);
BT.begin(38400);
Wire.begin();
mpu.initialize();
if (mpu.testConnection()) {
Serial.println("MPU6050 Connected");
} else {
Serial.println("MPU6050 Connection Failed");
}
lastTime = millis();
}
void loop() {
// === Read MPU6050 ===
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// === Time Difference ===
float dt = (millis() - lastTime) / 1000.0;
lastTime = millis();
// === Accelerometer Angles ===
float accX = atan2(ay, az) * 180 / PI;
float accY = atan2(ax, az) * 180 / PI;
// === Gyro (deg/sec) ===
float gyroX = gx / 131.0;
float gyroY = gy / 131.0;
float gyroZ = gz / 131.0;
// === Complementary Filter ===
angleX = 0.98 * (angleX + gyroX * dt) + 0.02 * accX;
angleY = 0.98 * (angleY + gyroY * dt) + 0.02 * accY;
// === Z Axis (Yaw) ===
angleZ += gyroZ * dt;
angleZ = constrain(angleZ, -90, 90);
// === Map to Servo Range ===
int rawX = constrain(map(angleX, -90, 90, 0, 180), 0, 180);
int rawY = constrain(map(angleY, -90, 90, 0, 180), 0, 180);
int rawZ = constrain(map(angleZ, -90, 90, 0, 180), 0, 180);
// === SMOOTH OUTPUT (LOW PASS FILTER) ===
smoothX += alpha * (rawX - smoothX);
smoothY += alpha * (rawY - smoothY);
smoothZ += alpha * (rawZ - smoothZ);
// === SEND TO SLAVE ===
BT.print((int)smoothX);
BT.print(",");
BT.print((int)smoothY);
BT.print(",");
BT.println((int)smoothZ);
// === DEBUG ===
Serial.print("X=");
Serial.print((int)smoothX);
Serial.print(" Y=");
Serial.print((int)smoothY);
Serial.print(" Z=");
Serial.println((int)smoothZ);
delay(20); // 50 Hz update
}