Arduino.ru

Пишу небольшой автопилот. Возникла небольшая проблема с подергиванием серв.  Вот код:

#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <Servo.h> 

//объекты серво
Servo servo_1;
Servo servo_2;

//объявление постоянных констант
static const int RXPin = 11, TXPin = 12;            //пины для serial gps
static const unsigned long GPSBaud = 9600;          //baudrate для gps
static const unsigned long COMBaud = 9600;          //baudrate для com

//объявление константных переменных финальной точки
const double fin_lat = 54.512431;	//финальные координаты LAT
const double fin_lon = 36.274083;	//финальные координаты LON

//переменные
double lat, lon;          //текущие lat, lon
double course, azimuth;   //курс, азимут
float alt, speed;         //высота, скорость
int er_com = 10;	  //переменная погрешность при сравнивании углов
int servo_pos;//текущая позиция сервы

//объекты
TinyGPSPlus gps;                     	    //новый объект для GPS
SoftwareSerial gpsSerial(RXPin, TXPin);     //программное сериал соединение для GPS

void setup()
{
  Serial.begin(COMBaud);			//открываем serial с компьютером
  gpsSerial.begin(GPSBaud);			//serial для GPS	
  
  //объекты серв
  servo_1.attach(9);
  servo_2.attach(10);
  
  //полет по прямой до получения координат GPS
  servo_1.write(90); 
  servo_2.write(90); 
  
  Serial.println("START");
  //отсылаем финальные координаты
  Serial.print("F_LAT=");  Serial.print(fin_lat, 6); Serial.print(" ");
  Serial.print("F_LON="); Serial.print(fin_lon, 6); Serial.print(" ");
  Serial.println();
}

void loop()
{
  unsigned long start = millis();
  if (gpsSerial.available() > 0)
	if (gps.encode(gpsSerial.read())){
		get_gps_info();                   //получение данных GPS
		servo_set();                      //выбор направления
	}
}//endloop


//функция получения данных по GPS
void get_gps_info()
{
  lat = gps.location.lat();         //определение LAT
  lon = gps.location.lng();         //определение LON
  alt = gps.altitude.meters();       //высота
  speed = gps.speed.kmph();          //скорость
  course = gps.course.deg();               //курс

  unsigned long date = gps.date.value();   //дата
  unsigned long time = gps.time.value();   //время

  azimuth = gps.courseTo(gps.location.lat(), gps.location.lng(), fin_lat, fin_lon);
}

//перевод серв
void servo_set(){
  //выставление серв
  if (abs(course - azimuth) <= er_com / 2){
      servo_1.write(90); 
      servo_2.write(90); 
      Serial.println("SERVO_POS=middle;"); // Отладка
  } else if (sin((azimuth - course)*PI/180.0) <= 0.0000001){
      servo_1.write(180); 
      servo_2.write(90); 
      Serial.println("SERVO_POS=left;"); // Отладка
  }else{
      servo_1.write(90); 
      servo_2.write(180); 
      Serial.println("SERVO_POS=right;"); // Отладка
  }
}

Вот видео работы:

http://www.youtube.com/watch?v=yoa7B7GMUyc

каждую секунду приходит пакет и серва дергается, почему так?