Дергается сервопривод

#define REMOTEXY_MODE__SOFTSERIAL
#include <SoftwareSerial.h> 
 
#include <RemoteXY.h> 
 
// настройки соединения  
#define REMOTEXY_SERIAL_RX 10 
#define REMOTEXY_SERIAL_TX 11 
#define REMOTEXY_SERIAL_SPEED 9600 
 
 
// конфигурация интерфейса   
#pragma pack(push, 1) 
uint8_t RemoteXY_CONF[] = 
  { 255,4,0,0,0,48,0,8,13,0,
  4,0,3,3,19,56,2,26,4,0,
  78,2,19,56,2,26,4,160,25,41,
  52,17,2,26,1,0,42,9,16,16,
  2,31,208,151,208,176,209,133,208,178,
  208,176,209,130,0 }; 
   
// структура определяет все переменные вашего интерфейса управления  
struct { 
 
    // input variable
  int8_t head; // =0..100 положение слайдера 
  int8_t body; // =0..100 положение слайдера 
  int8_t leg; // =-100..100 положение слайдера 
  uint8_t arm; // =1 если кнопка нажата, иначе =0 
 
    // other variable
  uint8_t connect_flag;  // =1 if wire connected, else =0 
 
} RemoteXY; 
#pragma pack(pop) 
 
///////////////////////////////////////////// 
//           END RemoteXY include          // 
///////////////////////////////////////////// 
 
#include <Servo.h>
 
Servo Sarm;
Servo Sbody;
Servo Shead;
Servo Sleg;
bool isOpen = true;
 
 
void setup()  
{ 
  RemoteXY_Init ();  
   
  
  Shead.attach(6);
  Sbody.attach(5);
   Serial.begin(9600);
  // TODO you setup code 
   
} 
 
void loop()  
{  
  RemoteXY_Handler (); 
  int headAngle = map(RemoteXY.head, 0, 100, 50, 180);
  int bodyAngle = map(RemoteXY.body, 0, 100, 0, 100);
  int legAngle = map(RemoteXY.leg, -100, 100, 0, 180);
  Shead.write(headAngle);
  Sbody.write(bodyAngle);
  Sleg.write(legAngle);
  Serial.println(bodyAngle);
 
  if(RemoteXY.arm == 1 && isOpen == 0){
    Sarm.write(50);  
    isOpen = !isOpen;
  }
 
  if(RemoteXY.arm == 1 && isOpen == 1){
    Sarm.write(180);  
    isOpen = !isOpen;
  }
   delay(10);
  // TODO you loop code 
  // используйте структуру RemoteXY для передачи данных 
 
 
}