/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-05-10 https://www.robotway.com/

  ------------------------------*/

#include <Servo.h>

Servo servo_c0;

Servo servo_c1;

Servo servo_w;

Servo servo_z;

Servo servo_b0;

Servo servo_b1;

int init_c = 90;

int init_w = 90;

int init_z = 90;

int init_b0 = 90;

int init_b1 = 90;

int i = 0;

int j = 0;

void setup() {

  // put your setup code here, to run once:

  Serial.begin(9600);

  servo_c0.attach(7);

  servo_c1.attach(11);

  servo_w.attach(12);

  servo_z.attach(4);

  servo_b0.attach(3);

  servo_b1.attach(8);

  servo_b0.write(50);

  servo_b1.write(50);

  servo_c0.write(90);

  servo_c0.write(90);

  delay(15);

  servo_c0.write(0);

}

void loop() {

  for(i = 90;i>=30;i-=2)

  {

    j = i+30;

    servo_c1.write(j);

    servo_c0.write(i);

    servo_z.write(i);

    j-=4;

    delay(50);

  }

    for(i = 70; i <= 110; i+=3)

  {

    servo_w.write(i);

    delay(15);

  }

      for(i = 30;i<=90;i+=2)

  {

    j = 0;

    servo_c1.write(j);

    servo_c0.write(i);

    servo_z.write(i);

    j+=4;

    delay(50);

  }

  for(i = 110; i >= 70; i-=3)

  {

    servo_w.write(i);

    delay(15);

  }

}