AT COMMMAND CODE
#include <SoftwareSerial.h>
// SoftwareSerial(RX, TX)
SoftwareSerial BT(10, 11); // RX = 11, TX = 10
void setup() {
Serial.begin(9600); // Serial Monitor
BT.begin(38400); // HC-05 AT Mode default baud rate
Serial.println("HC-05 AT Command Mode Ready");
Serial.println("Type AT commands below:");
}
void loop() {
// Serial Monitor → HC-05
if (Serial.available()) {
char c = Serial.read();
BT.write(c);
}
// HC-05 → Serial Monitor (Response)
if (BT.available()) {
char c = BT.read();
Serial.write(c);
}
}
SLAVE CODE
/* == SLAVE CODE ==
* Receives "X,Y,Z" from MASTER and controls 3 servo motors
*/
#include <Servo.h>
#include <SoftwareSerial.h>
SoftwareSerial BT(8, 9); // RX, TX
Servo servoX;
Servo servoY;
Servo servoZ;
String incomingData = "";
int angleX, angleY, angleZ;
void setup() {
Serial.begin(9600);
BT.begin(38400);
servoX.attach(3);
servoY.attach(5);
servoZ.attach(6);
Serial.println("Slave Ready...");
}
void loop() {
while (BT.available()) {
char c = BT.read();
if (c == '\n') {
// Parse the received data "X,Y,Z"
int firstComma = incomingData.indexOf(',');
int secondComma = incomingData.lastIndexOf(',');
angleX = incomingData.substring(0, firstComma).toInt();
angleY = incomingData.substring(firstComma + 1, secondComma).toInt();
angleZ = incomingData.substring(secondComma + 1).toInt();
// Move servos
servoX.write(angleX);
servoY.write(angleY);
servoZ.write(angleZ);
Serial.print("X: "); Serial.print(angleX);
Serial.print(" Y: "); Serial.print(angleY);
Serial.print(" Z: "); Serial.println(angleZ);
incomingData = "";
}
else {
incomingData += c;
}
}
}
MASTER CODE
/* ===== MASTER CODE (Correct MPU6050 Angles with X/Y/Z) ===== */
#include <Wire.h>
#include <MPU6050.h>
#include <SoftwareSerial.h>
SoftwareSerial BT(8, 9); // RX, TX
MPU6050 mpu;
int16_t ax, ay, az, gx, gy, gz;
float angleX = 0, angleY = 0, angleZ = 0;
unsigned long lastTime = 0;
void setup() {
Serial.begin(9600);
BT.begin(38400);
Wire.begin();
mpu.initialize();
if (mpu.testConnection()) {
Serial.println("MPU6050 Connected!");
} else {
Serial.println("MPU6050 Failed!");
}
lastTime = millis();
}
void loop() {
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// Time difference
float dt = (millis() - lastTime) / 1000.0;
lastTime = millis();
// Accelerometer-based X/Y angle
float accX = atan2(ay, az) * 180 / PI;
float accY = atan2(ax, az) * 180 / PI;
// Gyro rotation speed (deg/sec)
float gyroX = gx / 131.0;
float gyroY = gy / 131.0;
float gyroZ = gz / 131.0;
// Complementary filter X & Y
angleX = 0.98 * (angleX + gyroX * dt) + 0.02 * accX;
angleY = 0.98 * (angleY + gyroY * dt) + 0.02 * accY;
// ==== Z-axis corrected like X and Y ====
angleZ += gyroZ * dt; // gyro integration
// Limit to -90 to 90
angleZ = constrain(angleZ, -90, 90);
// Convert -90 → 0, 0 → 90, 90 → 180
int sendZ = map(angleZ, -90, 90, 0, 180);
// Output X and Y also mapped to 0–180
int sendX = constrain(map(angleX, -90, 90, 0, 180), 0, 180);
int sendY = constrain(map(angleY, -90, 90, 0, 180), 0, 180);
// === Send to Bluetooth ===
BT.print(sendX);
BT.print(",");
BT.print(sendY);
BT.print(",");
BT.println(sendZ);
// === Print on Serial Monitor ===
Serial.print("X=");
Serial.print(sendX);
Serial.print(" Y=");
Serial.print(sendY);
Serial.print(" Z=");
Serial.println(sendZ);
delay(30);
}