#include <Servo.h>
#include <SoftwareSerial.h>
Servo myservo1, myservo2, myservo3, myservo4;
int pos = 0;
int bluetoothTx = 2;
int bluetoothRx = 3;
SoftwareSerial bluetooth (bluetoothTx, bluetoothRx);
void setup() {
// Maps the servos to there respective PWM pins
myservo1.attach(5);
myservo2.attach(6);
myservo3.attach(9);
myservo4.attach(12);
// Moves all the servos to starting positions
myservo1.write(90);
myservo2.write(160);
myservo3.write(72);
myservo4.write(0);
//Setup Bluetooth serial connection to computer
Serial.begin(9600);
//Setup Bluetooth serial connection to andriod device
bluetooth.begin(9600);
}
void loop() {
//Reads from bluetooth and writes to the usb serial
if(bluetooth.available() >= 4){
unsigned char digit1 = (uint8_t)bluetooth.read();
unsigned char digit2 = (uint8_t)bluetooth.read();
unsigned char digit3 = (uint8_t)bluetooth.read();
unsigned char digit4 = (uint8_t)bluetooth.read();
Serial.println((char)digit1);
Serial.println((char)digit2);
Serial.println((char)digit3);
Serial.println((char)digit4);
String realNum = “”;
realNum.concat((char)digit1);
realNum.concat((char)digit2);
realNum.concat((char)digit3);
realNum.concat((char)digit4);
unsigned int realservo = realNum.toInt();
Serial.println(realservo);
if (realservo >= 1050 && realservo < 1140){
int servo1 = realservo;
servo1 = map(servo1, 1050, 1140, 50, 140);
myservo1.write(servo1);
Serial.println(“Servo 1 is ON :”);
delay(10);
}
if (realservo >= 2000 && realservo < 2160){
int servo2 = realservo;
servo2 = map(servo2, 2000, 2160, 0, 160);
myservo2.write(servo2);
Serial.println(“Servo 2 is ON”);
delay(10);
}
if (realservo >= 3072 && realservo < 3180){
int servo3 = realservo;
servo3 = map(servo3, 3072, 3180, 72, 180);
myservo3.write(servo3);
Serial.println(“Servo 3 is ON”);
delay(10);
}
if (realservo >= 4000 && realservo < 4180){
int servo4 = realservo;
servo4 = map(servo4, 4000, 4180, 0, 180);
myservo4.write(servo4);
Serial.println(“Servo 4 is ON:”);
delay(10);
}
}
}