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

int tx = 2; //input
int rx = 4; //output
int servopin1 = 5; //motor pin no.1
int servopin2 = 6; //motor pin no.2
int servopin3 = 3; //motor pin no.3
int servopin4 = 11; //motor pin no.4
String myString = "";
int ini1 = 120; //motor1 starting point
int ini2 = 45; //motor2 starting point
int ini3 = 20; //motor3 starting point
int ini4 = 60; //motor4 starting point
int angle1, angle2, angle3, angle4;
int pos1, pos2, pos3, pos4;
int goal1, goal2, goal3, goal4;
int dly = 120; //motor speed


Servo servo1; //definition with header
Servo servo2;
Servo servo3;
Servo servo4;


SoftwareSerial bluetooth(tx, rx);

void setup()
{
  Serial.begin(9600);
  delay(100);
  bluetooth.begin(9600);
  servo1.attach(servopin1);
  servo2.attach(servopin2);
  servo3.attach(servopin3);
  servo4.attach(servopin4);
  servo1.write(ini1 - 35); // 0 deg to 90 deg on Motor 1 is 120 deg to 30 deg
  servo2.write(ini2 + 35); // 0 deg to 90 deg on Motor 2 is 45 deg to 135 deg
  servo3.write(ini3 + 35); // 0 deg to 90 deg on Motor 3 is 20 deg to 110 deg
  servo4.write(ini4 + 35); // 0 deg to 90 deg on Motor 4 is 60 deg to 150 deg
  //start degree is 35 degree
}


void loop() {
  while (bluetooth.available()) 
  {
    char myChar = (char)bluetooth.read(); 
    myString += myChar; 
    delay(5);      
  }

  if (!myString.equals("")) 
  {
    Serial.println("input value: " + myString);

    if (myString == "down") //if you click button "down"
    {
      int pos1 = servo1.read();
      int pos2 = servo2.read();
      int pos3 = servo3.read();
      int pos4 = servo4.read();
      int goal1 = 5; //all motor moves 5 degrees, if you make this number bigger, it moves bigger
      delay(dly);
      for (angle1 = 0; angle1 <= goal1; angle1 += 1)
      {
        servo1.write(pos1 - angle1);
        servo2.write(pos2 + angle1);
        servo3.write(pos3 + angle1);
        servo4.write(pos4 + angle1); //all motor moves 5 degrees downward
        delay(dly);
      }
    } else if (myString == "up") {

      int pos1 = servo1.read();
      int pos2 = servo2.read();
      int pos3 = servo3.read();
      int pos4 = servo4.read();
      int goal1 = 5;
      delay(dly);
     for (angle1 = 0; angle1 <= goal1; angle1 += 1)
      {
        servo1.write(pos1 + angle1);
        servo2.write(pos2 - angle1);
        servo3.write(pos3 - angle1);
        servo4.write(pos4 - angle1); //all motor moves 5 degree upward
        delay(dly);
      }

    } else if (myString == "go") {

      int pos1 = servo1.read();
      int pos2 = servo2.read();
      int pos3 = servo3.read();
      int pos4 = servo4.read();
      int goal1 = 5;
      delay(dly);
     for (angle1 = 0; angle1 <= goal1; angle1 += 1)
      {
        servo1.write(pos1 + angle1);
        servo2.write(pos2 + angle1);
        servo3.write(pos3 + angle1);
        servo4.write(pos4 - angle1); //all motor moves 5 degree forward
        delay(dly);
      }

    } else if (myString == "back") {

      int pos1 = servo1.read();
      int pos2 = servo2.read();
      int pos3 = servo3.read();
      int pos4 = servo4.read();
      int goal1 = 5;
      delay(dly);
     for (angle1 = 0; angle1 <= goal1; angle1 += 1)
      {
        servo1.write(pos1 - angle1);
        servo2.write(pos2 - angle1);
        servo3.write(pos3 - angle1);
        servo4.write(pos4 + angle1); //all motor moves 5 degree backward
        delay(dly);
      }

    } else if (myString == "left") {

      int pos1 = servo1.read();
      int pos2 = servo2.read();
      int pos3 = servo3.read();
      int pos4 = servo4.read();
      int goal1 = 5;
      delay(dly);
     for (angle1 = 0; angle1 <= goal1; angle1 += 1)
      {
        servo1.write(pos1 + angle1);
        servo2.write(pos2 - angle1);
        servo3.write(pos3 + angle1);
        servo4.write(pos4 + angle1); //all motor moves 5 degree left
        delay(dly);
      }

    } else if (myString == "right") {

      int pos1 = servo1.read();
      int pos2 = servo2.read();
      int pos3 = servo3.read();
      int pos4 = servo4.read();
      int goal1 = 5;
      delay(dly);
     for (angle1 = 0; angle1 <= goal1; angle1 += 1)
      {
        servo1.write(pos1 - angle1);
        servo2.write(pos2 + angle1);
        servo3.write(pos3 - angle1);
        servo4.write(pos4 - angle1); //all motor moves 5 degree right
        delay(dly);
      }

    } else if (myString == "stop") {
      servo1.detach();
      servo2.detach();
      servo3.detach();
      servo4.detach(); //all motor stop moving
    }
    myString = ""; //initialize variable myString
  }
}