// MCU BASIC: https://www.basic4mcu.com
// DateTime : 2019-11-01 오후 7:28:19
// by Ok-Hyun Park
//
#include <SoftwareSerial.h>
SoftwareSerial mySerial(12,13); // RX,TX
#include <Servo.h>
//
const int SERVOS=4;
Servo myservo[SERVOS];
//
int  DELAYTIME=200;
int  PIN[SERVOS],value[SERVOS],currentAngle[SERVOS],MIN[SERVOS],MAX[SERVOS],INITANGLE[SERVOS];
int  afrom[]={90,55,165},ato[]={20,140,110},amiddle[]={20,55,165},afinal[]={179,125,95},aafterfinal[]={179,55,165};  
int  servo_moving[SERVOS]={0,0,0,0};
int  bt_servo=0,bt_move=0,idle=0;
bool stringComplete=false;
//
void setup(){
  pinMode(3,OUTPUT); digitalWrite(3,HIGH);
  mySerial.begin(9600);
  init_Pins();
  //auto_mode();
  Serial.begin(9600);
}
//
void loop(){
  move_bt();
  move_joy();
}
//
void init_Pins(){
  PIN[0]=11; MIN[0]= 0; MAX[0]=179; INITANGLE[0]= 90;
  PIN[1]=10; MIN[1]=35; MAX[1]=179; INITANGLE[1]= 90;
  PIN[2]= 9; MIN[2]=90; MAX[2]=179; INITANGLE[2]=155;
  PIN[3]= 5; MIN[3]= 0; MAX[3]=179; INITANGLE[3]= 25;
  //
  for(int i=0; i<SERVOS; i++){
    myservo[i].attach(PIN[i]); myservo[i].write(INITANGLE[i]); value[i]=0;
    idle=0;
  }
}
//
void move_bt(){
  checkSoftSerial();
  for(int i=0; i<SERVOS; i++){
    currentAngle[i]=myservo[i].read();
    if(servo_moving[i]!=0){
      currentAngle[i]+=servo_moving[i];
      currentAngle[i]=currentAngle[i]>MAX[i]?--currentAngle[i]: currentAngle[i];
      currentAngle[i]=currentAngle[i]<MIN[i]?++currentAngle[i]: currentAngle[i];
      myservo[i].write(currentAngle[i]);
      delay(20);
    }
  }
}
//
void checkSoftSerial(){
  String str="";
  if(mySerial.available()){
    for(int i=0; i<2; i++)str+=(char)mySerial.read();
    //
    int value=str.toInt(); // the servo to move
    bt_servo=value/10;
    //
    int angle=value%10; // the direction to move
    if     (angle==2)bt_move= 1;
    else if(angle==1)bt_move=-1;
    else             bt_move= 0;
    //
    servo_moving[bt_servo]=bt_move;
  }
}
//
void move_joy(){
  for(int i=0; i<SERVOS; i++){
    value[i]=analogRead(i);
    currentAngle[i]=myservo[i].read();
    if(value[i]>612){
      idle=0;
      if(currentAngle[i]>MIN[i])--currentAngle[i];
    }
    else if(value[i]<412){
      idle=0;
      if(currentAngle[i]<MAX[i])++currentAngle[i];
    }
    else{ idle++; }
    //
    if(idle==100){ myservo[i].detach(); }
  }
  for(int i=0; i<SERVOS; i++){
    if(!myservo[i].attached())myservo[i].attach(PIN[i]);
    myservo[i].write(currentAngle[i]);
  }
  delay(20);
}
//
void auto_mode(){
  for(int i=0; i<2; i++){
    closeclaw(false); armfromto(afrom,ato); closeclaw(true); delay(DELAYTIME);
    armfromto(ato,amiddle);        delay(DELAYTIME);
    armfromto(amiddle,afinal);     closeclaw(false); delay(DELAYTIME);
    armfromto(afinal,aafterfinal); delay(DELAYTIME);
    armfromto(aafterfinal,afrom);  delay(DELAYTIME);
  }
}
//
void armfromto(int*arrf,int*arrt){
  int lp[3],seg=3,sign;
  for(int i=0; i<3; i++)lp[i]=abs((arrt[i]-arrf[i])/seg);
  //delay(DALAYTIME);
  for(int i=0; i<3; i++){
    sign=arrt[i]-arrf[i]>0?1:-1;
    for(int j=0; j<lp[i]; j++){ myservo[i].write(arrf[i]+j*seg*sign); delay(20); }
    delay(DELAYTIME);
  }
}
//
void closeclaw(boolean op){
  if(op){ myservo[3].write( 5); }
  else  { myservo[3].write(30); }
}