#include <Servo.h>

Servo base_servo;
Servo shoulder_servo;
Servo wrist_servo;
Servo gripper_servo1;
Servo gripper_servo2;

int base_pin = 9;
int shoulder_pin = 10;
int wrist_pin = 11;
int gripper_pin1 = 12;
int gripper_pin2 = 13;

int positions[][5] = {
  {0, 90, 0, 0, 0},
  {90, 0, 0, 0, 0},
  {90, 90, 0, 0, 0},
  {0, 0, 90, 0, 0},
  {0, 0, 0, 90, 0},
  {0, 0, 0, 0, 90}
};

int num_positions = 5;
int current_position = 0;

void setup() {
  base_servo.attach(base_pin);
  shoulder_servo.attach(shoulder_pin);
  wrist_servo.attach(wrist_pin);
  gripper_servo1.attach(gripper_pin1);
  gripper_servo2.attach(gripper_pin2);
}

void loop() {
  current_position = (current_position + 1) % num_positions;
  int* position = positions[current_position];
  base_servo.write(position[0]);
  shoulder_servo.write(position[1]);
  wrist_servo.write(position[2]);
  gripper_servo1.write(position[3]);
  gripper_servo2.write(position[4]);
  delay(1000);
}