Controlling Multiple Servos with 2 HC-05 Bluetooth Chips

Hey guys,
I am working on a research project in which I am trying to use two corresponding HC-05 chips set up as master and slave to control two servos on the slave circuit.
The controller uses two flex sensors and a MPU6050 gyroscope to output values. The robot circuit has one continuous rotation servo and one 180 degree servo.
I finally have the bluetooth chips connected, but I haven't been able to get any of the values to transmit. I am using both the SoftwareSerial library and the Servo library, and I have included the code below.

Master Controller Code:

#include <Wire.h>
#include <SoftwareSerial.h>

// create object named bt of the class SoftwareSerial
SoftwareSerial BTSerial(3,4); //(Rx, Tx)

//Create  Sensors
int middle; //middle finger
int pointer; //pointer finger

int middle_Data = A6;
int pointer_Data = A3;

//const int MPU_addr = 0x68;
const int MPU1 = 0x68;

//First MPU6050
int16_t AcX1, AcY1, AcZ1, Tmp1, GyX1, GyY1, GyZ1;
int minVal = 265;
int maxVal = 402;
double x;
double y;
double z;

char receiveData = 0; // Variable to store data recieved over bluetooth

//How often to send values to the Robot
int response_time = 100;

void setup() {
  pinMode(3, INPUT);
  pinMode(4, OUTPUT);
  Wire.begin();
  Wire.beginTransmission(MPU1);
  Wire.write(0x6B);// PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true); Wire.begin();
  Serial.begin(38400);
  BTSerial.begin(38400);
  delay(1000);

}
void loop() {
  /*
    Note: Serial.print() would send all values to the robotic using via bluetooth.
  */
  // debug_flex(); //Debug Mode on/off
  // Serial.println(" ");
  //get values for first mpu having address of 0x68
  GetMpuValue1(MPU1);
  //Serial.prinlnt("  ");
  delay(100);

  //Print out a value, based on the change of the XYZ co-ordinates of 1st or 2nd MPU

  //Move Left
  if ( x > 0 && x < 150) {
    BTSerial.print("L");
    delay(response_time);
  }

  //Move Right
  if ( x < 310 && x > 200) {
    BTSerial.print("R");
    delay(response_time);
  }

  // read the values from Flex Sensors to Arduino
  middle = analogRead(middle_Data);
  pointer = analogRead(pointer_Data);
  delay(response_time);

  // middle
  if (middle <= 150) {
    BTSerial.print("m");
    delay(response_time);

  } else {
    BTSerial.print("M");
    delay(response_time);
  }

  // index 1 - Claw Bend/Open
  if (pointer <= 150) {
    BTSerial.print("p");
    delay(response_time);
  } else {
    BTSerial.print("P");
    delay(response_time);
  }
}

void GetMpuValue1(const int MPU) {

  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14, true); // request a total of 14 registers

  AcX1 = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY1 = Wire.read() << 8 |  Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ1 = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

  Tmp1 = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)

  int xAng = map(AcX1, minVal, maxVal, -90, 90);
  int yAng = map(AcY1, minVal, maxVal, -90, 90);
  int zAng = map(AcZ1, minVal, maxVal, -90, 90);

  GyX1 = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY1 = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ1 = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  x = RAD_TO_DEG * (atan2(-yAng, -zAng) + PI) + 4; //offset by 4 degrees to get back to zero
  y = RAD_TO_DEG * (atan2(-xAng, -zAng) + PI);
  z = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);

  //-- Comment to Debug
  //  Serial.print("AngleX= ");
  //  Serial.print(x);
  //  Serial.print("\t");
  //
  //  Serial.print("AngleY= ");
  //  Serial.print(y);
  //  Serial.print("\t");
  //
  //  Serial.print("AngleZ= ");
  //  Serial.print(z);
  //  Serial.print("\t");
  //  Serial.println("-----------------------------------------");


  //  Serial.print("AcX = ");
  //  Serial.print(AcX1);
  //  Serial.print(" | AcY = ");
  //  Serial.print(AcY1);
  //  Serial.print(" | AcZ = ");
  //  Serial.print(AcZ1);
  //  Serial.print(" | GyX = ");
  //  Serial.print(GyX1);
  //  Serial.print(" | GyY = ");
  //  Serial.print(GyY1);
  //  Serial.print(" | GyZ = ");
  //  Serial.println(GyZ1);
}

 void debug_flex() {
  //Sends value as a serial monitor to port

  //index (Claw Further)
  Serial.print("pointer: ");
  Serial.print(pointer);
  Serial.print("\t");

  //middle (Claw Further)
  Serial.print("middle: ");
  Serial.print(middle);
  Serial.print("\t");
}

Slave Robot Code:

#include <Servo.h>
#include <SoftwareSerial.h>

// create servo object to control a servo
Servo servoR;  // Rudder servo
Servo servoM;  // Mainsheet servo

SoftwareSerial BTSerial(3,4); // (Rx, Tx)

// variable to store the servo position
int posR = 0;    // Rudder position
int posM = 0;    // Main position
char receiveData = 0; // Variable to store data recieved over bluetooth
int response_time = 100;

void setup() {
  servoR.attach(6);  // attaches Rudder servo on pin 6 to the Rudder servo object
  servoM.attach(5);  // ^^^ but for Main to pin 5
  pinMode(3, INPUT);
  pinMode(4, OUTPUT);
  Serial.begin(38400); // define baud rate for serial communication
  BTSerial.begin(38400);
  delay(1000);
}

void loop() {
  if(Serial.available()) { // if data is available on serial port
    receiveData = BTSerial.read();
    Serial.print(receiveData + "");

    if(receiveData == 'L'){
      posR = posR + 1;
      servoR.write(posR);
      delay(response_time);
    } else if(receiveData == 'R'){
      posR = posR - 1;
      servoR.write(posR);
      delay(response_time);
    }

    if(receiveData == 'P'){
      servoM.write(180);
      delay (1000);
      servoM.write(90);
      delay(response_time);
    }

    if(receiveData == 'M'){
      servoM.write(0);
      delay (1000);
      servoM.write(90);
      delay(response_time);
    }
  }
}

Hi @ghostshipb

welcome to the arduino-forum.
well done posting your code as code-sections and writing a description of what your code shall do

Well writing the code for such a project is best done in steps.

step 0:
googling for a tutorial send / receive characters with HC05-bluetooth modules.
You have to pair the bluetooth-modules like any blue-tooth devices.

step1
modifying an existing HC05-demo-code that does only one thing:

master: send a constant character once every 3 seconds
slave: receive the character over bluetooth and print it to the serial monitor

best regards Stefan

1 Like

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.