Модуль управления двигателями Motor Shield v2

[Skanch]

Методом проб и ошибок написал программу для управления ШД и сервой.Планирую использовать ее для управления монитором в машине.Алгоритм таков: одно нажатие кнопки-ШД делает определенное кол-во шагов вперед и останавливается, серва поворачивается на 180 градусов и останавливается, повторное нажатие кнопки - серва отрабатывает движение на 180 градусов назад и останавливается,ШД отрабатывает определенное кол-во шагов назад и останавливается.

#include <AFMotor.h>
#include <Servo.h>

Servo myservo;// создаём объект для контроля сервы 
int pos = 0;     // переменная для хранения позиции сервы 

int inPin = 2; // контакт, к которому подключена кнопка
int outPin = 9; // контакт, к которому подключен светодиод
AF_Stepper motor(200, 2);//Создаем объект для двигателя на 2 канале (M3 и M4)

int state = HIGH; // the current state of the output pin
int reading; // the current reading from the input pin
int previous = LOW; // the previous reading from the input pin

long time = 0; // the last time the output pin was toggled
long debounce = 200; // the debounce time, increase if the output flickers

void setup()
{
pinMode(inPin, INPUT);
pinMode(outPin, OUTPUT);
motor.setSpeed(100); // 100 оборотов в минуту
myservo.attach(10);  // серва подключена к 10-му пину
myservo.writeMicroseconds(500); // устанавливаем серву в крайнее левое положение
}

void loop()
{
reading = digitalRead(inPin);
if (reading == HIGH && previous == LOW && millis() - time > debounce) 
{
if (state == HIGH) {
 motor.step(860, FORWARD, SINGLE); // движение ШД вперед
  for(pos = 0; pos < 180; pos += 1)  // от 0 до 180 градусов
  {                                  // с шагом в 1 градус
    myservo.write(pos);              
    delay(15);    // ждём 15ms пока серва займёт новое положение
  }
 } 
else {
  for(pos = 180; pos>=1; pos-=1)     // от 180 до 0 градусов
  {
    myservo.write(pos);
    delay(15);
  }
 motor.step(860, BACKWARD, SINGLE); // движение ШД назад
 motor.release(); // отключение ШД 
}
if (state == HIGH)
state = LOW;
else
state = HIGH; 
time = millis();
}
digitalWrite(outPin, state);
previous = reading;
}