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);
}
}
}