#include <SCServo.h>

SMS_STS st;

#define S_RXD 0//Serial receive pins
#define S_TXD 1//Serial transmit piins

void setup() {
  Serial.begin(1000000,SERIAL_8N1);

  pinMode (7,INPUT);//set pin 7 as input

 //Initializes serial port 1 communication for controlling the serial bus servo's; SERIAL_8N1 is the configuration data, parity, and stop bits; specifies the serial port pins
 //Serial1.begin(1000000, SERIAL_8N1);
  //Set the serial port parameter of the SCServo object to Serial1, indicating that the 1000000 serial port is used to control the servo
  st.pSerial = &Serial;
  delay(1000);
}

void loop()
{
 //st.WritePosEx(1,4000,1000,50);
  //delay(5000);


 //st.WritePosEx(1,300,1000,50);
  //delay(5000);
  
   //st.WritePosEx(1,2000,1000,50);
 // delay(10000);




 // st.WritePosEx(4, 2050, 3400, 50);    //Set the position, speed and acceleration of servo 1 with a delay of 2s
  //delay(100);
  
  //st.WritePosEx(3, 1500, 1000, 50);    //Set the position, speed and acceleration of servo 1 with a delay of 2s
  //delay(100);



 //st.WritePosEx(5,3400,1000,50);
  //delay(5000);

 //st.WritePosEx(5,4000,1000,50);
  //delay(100);

   //st.WritePosEx(3,2000,1000,50);
 // delay(100);

 //start finger close loop

 int y=digitalRead(7);
 if (y==HIGH) {

 for(int i=0;i<=1;i++){

   st.WritePosEx(4,1500,2000,50);
  delay(500);

  //fingers one at a time

 // delay(2000);

 //fingers close

st.WritePosEx(2,2600,2000,50);//pinky away from thumb
  delay(500);

st.WritePosEx(7,2600,2000,50);//close three
  delay(500);

 st.WritePosEx(1,2600,2000,50);//close two
  delay(500);


 st.WritePosEx(5,2600,2000,50);//close one
  delay(2000);


  //return fingers

 st.WritePosEx(5,4000,2000,50);
  //delay(2000); 
  
 

 st.WritePosEx(1,3900,4000,50);
  //delay(2000);


 
 st.WritePosEx(7,3900,2000,50);
  //delay(2000);

 
 st.WritePosEx(2,3900,2000,50);
 delay(500);
 
  st.WritePosEx(4,1800,2000,50);
  delay(500);



  delay(2000);
 }
 }


//stay here until person is not detected
 y=digitalRead(7);
 while(y==HIGH) {
y=digitalRead(7);

 }





}