Project_10480_ARM_6DOF_BT_RX

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


unsigned long previousMillis = 0;
const long interval = 500;

#define RX 4
#define TX 5
#define S1 6
#define S2 7
#define S3 8
#define S4 9
#define S5 10
#define S6 11

Servo Servo1;
Servo Servo2;
Servo Servo3;
Servo Servo4;
Servo Servo5;
Servo Servo6;
SoftwareSerial ss(RX, TX);

char c;
String dataIn;
int8_t indexOfA, indexOfB, indexOfC, indexOfD, indexOfE, indexOfF;
String data1, data2, data3, data4, data5, data6;

void setup() {

  Serial.begin(9600);

  ss.begin(9600);
  attachServos();
  Servo1.write(0);
  Servo2.write(40);//90-20
  Servo3.write(90);
  Servo4.write(90);
  Servo5.write(90);
  Servo6.write(125);//175-125
}

void loop() {

  while (ss.available() > 0) {
    c = ss.read();
    if (c == '\n') {
      break;
    }
    else       {
      dataIn += c;
    }
  }
  if (c == '\n') {
    parseData();
    ShowRxData();
    if (millis() - previousMillis >= interval) {
      previousMillis = millis();
      moveARM();
    }
    resetData();
  }

}// end of LOOP.

//******************************************************
void parseData() {
  indexOfA = dataIn.indexOf("A");
  indexOfB = dataIn.indexOf("B");
  indexOfC = dataIn.indexOf("C");
  indexOfD = dataIn.indexOf("D");
  indexOfE = dataIn.indexOf("E");
  indexOfF = dataIn.indexOf("F");
  data1 = dataIn.substring(0, indexOfA);
  data2 = dataIn.substring(indexOfA + 1, indexOfB);
  data3 = dataIn.substring(indexOfB + 1, indexOfC);
  data4 = dataIn.substring(indexOfC + 1, indexOfD);
  data5 = dataIn.substring(indexOfD + 1, indexOfE);
  data6 = dataIn.substring(indexOfE + 1, indexOfF);
}
//******************************************************
void ShowRxData() {
  Serial.println("S1: " + data1);
  Serial.println("S2: " + data2);
  Serial.println("S3: " + data3);
  Serial.println("S4: " + data4);
  Serial.println("S5: " + data5);
  Serial.println("S6: " + data6);
  Serial.println("-----------------");
}
//******************************************************
void resetData() {
  c = 0;
  dataIn = "";
}
//******************************************************
void attachServos() {
  delay(15);
  Servo1.attach(S1);
  delay(15);
  Servo2.attach(S2);
  delay(15);
  Servo3.attach(S3);
  delay(15);
  Servo4.attach(S4);
  delay(15);
  Servo5.attach(S5);
  delay(15);
  Servo6.attach(S6);
  delay(15);
}
//******************************************************
void moveServos() {
  //delay(15);
  Servo1.write(data1.toInt());
  //delay(15);
  Servo2.write(data2.toInt());
  //delay(15);
  Servo3.write(data3.toInt());
  //delay(15);
  Servo4.write(data4.toInt());
  //delay(15);
  Servo5.write(data5.toInt());
  //delay(15);
  Servo6.write(data6.toInt());
  //delay(15);
}
//******************************************************
void detachServos() {
  delay(15);
  Servo1.detach();
  delay(15);
  Servo2.detach();
  delay(15);
  Servo3.detach();
  delay(15);
  Servo4.detach();
  delay(15);
  Servo5.detach();
  delay(15);
  Servo6.detach();
  delay(15);
}
//******************************************************
void moveARM() {
  //attachServos();
  //delay(500);
  moveServos();
  //delay(100);
  //detachServos();
  //delay(500);
}
//******************************************************