#include <ACB_ARM.h>//Add the Robot arm libraries
ACB_ARM ARM;

bool RunningState = true;

int startx = 7, starty = 13, startz = 2;//Initial coordinates
int midz = 15;//Height of middle point
int endx = -7, endy = 13, endz = 0;//End point coordinates
int openAngle = 130, closeAngle = 90;
int count = 2;//Number of blocks
int i = 0;

void setup() {
  ARM.Chassis_angle_adjust(8);//Chassis error calibration
  ARM.Slight_adjust(0,2);//Small calibration error
  ARM.ARM_init(5,16,17,18);//The parameters are four Servo pins
  Serial.begin(115200);
}

void loop() {
  while(i < count) {
    ARM.ClawsCmd(openAngle);//open claws
    delay(1000);
    ARM.PtpCmd(startx, starty, startz-i*2);//The target point is the upper object
    delay(1000);
    ARM.ClawsCmd(closeAngle);//close claws
    delay(1000);
    ARM.PtpCmd(startx, starty, midz);//Lift the object after picking it up
    delay(1000);
    ARM.PtpCmd(endx, endy, midz);//Rotate above the target point
    delay(1000);
    ARM.PtpCmd(endx, endy, endz+i*2);//Placing Objects
    delay(1000);
    ARM.ClawsCmd(openAngle);
    delay(1000);
    ARM.PtpCmd(endx, endy, midz);//Raise arms
    delay(1000);
    i = i + 1;
  }
  ARM.Zero();//Servo initialization
}