版权说明：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-12 https://www.robotway.com/

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

#include <Servo.h>

Servo servo_pin_4;

Servo servo_pin_3;

int i = 0;

int j = 0;

void setup()

{

  Serial.begin(9600);

  pinMode( 10, OUTPUT);

  pinMode( 6, OUTPUT);

  pinMode( 5, OUTPUT);

  pinMode( 9, OUTPUT);

  servo_pin_4.attach(4);

  servo_pin_3.attach(3);

  servo_pin_4.write(0);

  servo_pin_3.write(180);

}

void loop()

{

forward();

for(i = 0; i <= 180; i+=2)

{

  servo_pin_4.write(i);

  servo_pin_3.write(180-i);

  delay(5);

}

for(i = 180; i >= 0; i-=2)

{

  servo_pin_4.write(i);

  servo_pin_3.write(180-i);

  delay(5);

}

}

void forward()

{

  digitalWrite(5,HIGH);

  digitalWrite(6,LOW);

  digitalWrite(9,HIGH);

  digitalWrite(10,LOW);

}