Arduino.ru

Уважаемые коллеги,

прошу подсказки по управлению серво.

Имеется простая прога:

#include <Servo.h>   //библиотека серво
#define trigPin 13   //пин передатчика
#define echoPin 12   //пин приемника
#define MAXSERVO 4   //число серв в проекте
#define MAXANGLE 180 //максимальный угол° поворота сервы
#define MINANGLE 0   //минимальный  угол° поворота сервы

Servo      servo[MAXSERVO];             //задание массива серв
int    servo_pin[MAXSERVO] = {2,3,4,5}; //задание пинов подключения серв
long servo_angle[MAXSERVO] = {MINANGLE, //задание начального угла разворота сервы №1
                              MINANGLE, //задание начального угла разворота сервы №2
                              MINANGLE, //задание начального угла разворота сервы №3
                              MINANGLE};//задание начального угла разворота сервы №4
int servo_direct[MAXSERVO] = {0,0,0,0}; //начальное направление вращения каждой сервы

void AngleServo(int servo_num);               //функция разворота каждой сервы на заданный угол (по закоу: вращаем на MAXANGLE-MINANGLE в одну сторону и обратно)
long GetDistance(int trig_pin, int echo_pin); //функция вычисления дистанции (надстройка над функциями библиотеки серво)

void setup() 
{
  pinMode(trigPin, OUTPUT);         //инициализация ультразвукового дальномера
  pinMode(echoPin, INPUT);
  for(int i=0; i<MAXSERVO; i++) 
  {
    servo[i].attach(servo_pin[i]);  //инициализация серв
    servo[i].write(servo_angle[i]); //(строка нужна, если начальный угол серв отличен от 0°, по умолчанию компилятор выводит все серво на 0°)
  }
}
void loop() 
{
  long distance = GetDistance(trigPin,echoPin); //вычисление дистанции дальномером
  if(distance<50) AngleServo(0);                //если дистанция меньше 50 см - вращаем серву №1
  if(distance<30) AngleServo(1);                //если дистанция меньше 30 см - вращаем серву №1,2
  if(distance<10) AngleServo(2);                //если дистанция меньше 10 см - вращаем серву №1,2,3
  if(distance< 5) AngleServo(3);                //если дистанция меньше 5  см - вращаем серву №1,2,3,4
  delay(15);
} 

void AngleServo(int servo_num)
{
    if(servo_direct[servo_num]==1 && servo_angle[servo_num] <MAXANGLE) servo_angle [servo_num]++;
    if(servo_direct[servo_num]==1 && servo_angle[servo_num]==MAXANGLE) servo_direct[servo_num]=0;
    if(servo_direct[servo_num]==0 && servo_angle[servo_num] >MINANGLE) servo_angle [servo_num]--;
    if(servo_direct[servo_num]==0 && servo_angle[servo_num]==MINANGLE) servo_direct[servo_num]=1;
    servo[servo_num].write(servo_angle[servo_num]);
    return;
}

long GetDistance(int trig_pin, int echo_pin)
{
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig_pin, LOW);
  long duration = pulseIn(echo_pin, HIGH);
  return ((duration/2) / 29.1);
}

- при минимальном и максимальном значении угла 0 и 180° все 4 сервы работают как надо, но при изменении пределов, например от 160 до 180°- одна или две еле дергаются, не отрабатывают свои 20°. В чем засада?

Спасибо.