Скетч управления роботом — манипулятором

Servo s_gripper; // захват
Servo s_brush; // кисть
Servo s_elbow; // локоть
Servo s_turn; // поворот

AF_DCMotor my_motor1(1); // правый мотор к пину 1 шилда
AF_DCMotor my_motor2(2);

void setup() {

s_gripper.attach(2); // включил серву
s_brush.attach(13);
s_elbow.attach(10);
s_turn.attach(9);
my_motor1.setSpeed(250); // скорость вращения двигателя
my_motor2.setSpeed(250);

s_gripper.write(70); // установить сервы в стартовое значение
s_brush.write(150);
s_elbow.write(60);
s_turn.write(70);
}

void loop() {

int position;

// Управление движением вездехода

my_motor1.run(FORWARD);
my_motor2.run(FORWARD);
delay(2000);
my_motor1.run(BACKWARD);
my_motor2.run(FORWARD);
delay(2000);
my_motor1.run(FORWARD);
my_motor2.run(BACKWARD);
delay(2000);
my_motor1.run(BACKWARD);
my_motor2.run(BACKWARD);
delay(2000);
my_motor1.run(RELEASE);
my_motor2.run(RELEASE);
delay(300);

// управление манипулятором

// захват
s_gripper.write(30);
delay(20);
for(position = 30; position <= 170; position += 2)
{
s_gripper.write(position); // Переместиться на следующую позицию
delay(100); // Короткая пауза
}
for(position = 170; position <= 70; position -= 2)
{
s_gripper.write(position); // Переместиться на следующую позицию
delay(100); // Короткая пауза
}

// кисть
s_brush.write(170);
delay(20);
for(position = 170; position >= 80; position -= 2)
{
s_brush.write(position); // Переместиться на следующую позицию
delay(100); // Короткая пауза
}
for(position = 80; position <= 170; position += 2)
{
s_brush.write(position); // Переместиться на следующую позицию
delay(100); // Короткая пауза
}

// локоть
s_elbow.write(60);
delay(20);
for(position = 60; position >= 20; position -= 2)
{
s_elbow.write(position); // Переместиться на следующую позицию
delay(200); // Короткая пауза
}
for(position = 30; position <= 60; position += 2)
{
s_elbow.write(position); // Переместиться на следующую позицию
delay(200); // Короткая пауза
}

// поворот
s_turn.write(70);
delay(20);
for(position = 70; position <= 140; position += 2) { s_turn.write(position); // Переместиться на следующую позицию delay(200); // Короткая пауза } for(position = 140; position >= 70; position -= 2)
{
s_turn.write(position); // Переместиться на следующую позицию
delay(200); // Короткая пауза
}
}