#include <Servo.h>


#define BASE_SERVO_PIN 2
#define SHOULDER_SERVO_PIN 3
#define ELBOW_SERVO_PIN 4
#define WRIST_SERVO_PIN 5
#define GRIPPER_SERVO_PIN 6
#define ROTATE_SERVO_PIN 7


Servo baseServo;
Servo shoulderServo;
Servo elbowServo;
Servo wristServo;
Servo gripperServo;
Servo rotateServo;


int basePos = 90;
int shoulderPos = 90;
int elbowPos = 90;
int wristPos = 90;
int gripperPos = 90;
int rotatePos = 90;

void setup() {

  baseServo.attach(BASE_SERVO_PIN);
  shoulderServo.attach(SHOULDER_SERVO_PIN);
  elbowServo.attach(ELBOW_SERVO_PIN);
  wristServo.attach(WRIST_SERVO_PIN);
  gripperServo.attach(GRIPPER_SERVO_PIN);
  rotateServo.attach(ROTATE_SERVO_PIN);
}

void loop() {

  moveToInitialPosition();


  liftWeight();

  moveToPlatform();


  releaseWeight();

  moveToInitialPosition();


  delay(2000);
}

void moveToInitialPosition() {

  basePos = 90;
  shoulderPos = 90;
  elbowPos = 90;
  wristPos = 90;
  gripperPos = 90;
  rotatePos = 90;

  baseServo.write(basePos);
  shoulderServo.write(shoulderPos);
  elbowServo.write(elbowPos);
  wristServo.write(wristPos);
  gripperServo.write(gripperPos);
  rotateServo.write(rotatePos);

  delay(1000); 
}

void liftWeight() {

  basePos = 120;
  shoulderPos = 100;
  elbowPos = 70;
  wristPos = 40;


  baseServo.write(basePos);
  shoulderServo.write(shoulderPos);
  elbowServo.write(elbowPos);
  wristServo.write(wristPos);

  delay(2000); 
}

void moveToPlatform() {

  basePos = 90;
  shoulderPos = 120;
  elbowPos = 100;
  wristPos = 80;

 
  baseServo.write(basePos);
  shoulderServo.write(shoulderPos);
  elbowServo.write(elbowPos);
  wristServo.write(wristPos);

  delay(2000); 
}

void releaseWeight() {

  gripperPos = 30;

  gripperServo.write(gripperPos);

  delay(1000); 
}