Управление сервоприводом от акселерометра

[Smail95]

#include <io.h>
#include <stdio.h>
#include <alcd.h>
#include <delay.h>
#include <stdlib.h>
#include <math.h>
#define addr_mpu 0xD0

typedef unsigned char byte;

#asm
.equ __i2c_port=0x15
.equ __sda_bit=0
.equ __scl_bit=1
#endasm

#include <i2c.h>

byte read_mpu(byte addr_byte)
{ 
 byte data;
 
 i2c_start();
 
 i2c_write(addr_mpu);
 i2c_write(addr_byte);
 
 i2c_start();
 
 i2c_write(addr_mpu | 1);   
 
 data=i2c_read(0);
 
 i2c_stop();
 
 return data;
} 

void write_mpu(byte addr_byte, byte data)
{
 i2c_start();
 
 i2c_write(addr_mpu);
 i2c_write(addr_byte);

 i2c_write(data);   

 i2c_stop();
 delay_ms(10);
}

byte read_xh(byte addr_byte)
{
  byte data;
 
 i2c_start();
 
 i2c_write(addr_mpu);
 i2c_write(addr_byte);
 
 i2c_start();
 
 i2c_write(addr_mpu | 1);   
 
 data=i2c_read(0);
 
 i2c_stop();
 
 return data;
}

byte read_xl(byte addr_byte)
{
  byte data;
 
 i2c_start();
 
 i2c_write(addr_mpu);
 i2c_write(addr_byte);
 
 i2c_start();
 
 i2c_write(addr_mpu | 1);   
 
 data=i2c_read(0);
 
 i2c_stop();
 
 return data;;
}

byte read_yh(byte addr_byte)
{
  byte data;
 
 i2c_start();
 
 i2c_write(addr_mpu);
 i2c_write(addr_byte);
 
 i2c_start();
 
 i2c_write(addr_mpu | 1);   
 
 data=i2c_read(0);
 
 i2c_stop();
 
 return data;;
}

byte read_yl(byte addr_byte)
{
  byte data;
 
 i2c_start();
 
 i2c_write(addr_mpu);
 i2c_write(addr_byte);
 
 i2c_start();
 
 i2c_write(addr_mpu | 1);   
 
 data=i2c_read(0);
 
 i2c_stop();
 
 return data;
}

byte read_zh(byte addr_byte)
{
  byte data;
 
 i2c_start();
 
 i2c_write(addr_mpu);
 i2c_write(addr_byte);
 
 i2c_start();
 
 i2c_write(addr_mpu | 1);   
 
 data=i2c_read(0);
 
 i2c_stop();
 
 return data;
}

byte read_zl(byte addr_byte)
{
  byte data;
 
 i2c_start();
 
 i2c_write(addr_mpu);
 i2c_write(addr_byte);
 
 i2c_start();
 
 i2c_write(addr_mpu | 1);   
 
 data=i2c_read(0);
 
 i2c_stop();
 
 return data;
}

byte read_th(byte addr_byte)
{
  byte data;
 
 i2c_start();
 
 i2c_write(addr_mpu);
 i2c_write(addr_byte);
 
 i2c_start();
 
 i2c_write(addr_mpu | 1);   
 
 data=i2c_read(0);
 
 i2c_stop();
 
 return data;
}

byte read_tl(byte addr_byte)
{
  byte data;
 
 i2c_start();
 
 i2c_write(addr_mpu);
 i2c_write(addr_byte);
 
 i2c_start();
 
 i2c_write(addr_mpu | 1);   
 
 data=i2c_read(0);
 
 i2c_stop();
 
 return data;
}

void main(void){

char buff[15];
int q=0x5DC,x=0,y=0,z=0,te=0,v=0;
byte r,xh,yh,zh,xl,yl,zl,th,tl;

PORTA=00000000; 
DDRA=11110111;
PORTC=0b00000000;
DDRC=0b00000000;

PORTD=0b00000000;
DDRD=0b00100000;

TCCR1A=0b10000010;
TCCR1B=0b00011010;

ICR1H=0x4E;
ICR1L=0x20;

OCR1AH=0x5;
OCR1AL=0xDC;

i2c_init();
delay_ms(100);
lcd_init(16);
// go on the first LCD line
lcd_clear(); // очистка         
// Global enable interrupts
#asm("sei")
 
while (1)
{
write_mpu(0x6B,0x00);
write_mpu(0x1C,0x10);  // g - ускорение

r=read_mpu(0x1C);

xh=read_xh(0x3B);
yh=read_yh(0x3d);
zh=read_zh(0x3f);
xl=read_xl(0x3C);
yl=read_yl(0x3E);
zl=read_zl(0x40);

th=read_th(0x41);
tl=read_tl(0x42);

 scanf ("%d",&r);

itoa(r,buff,2); // Error: too many arguments in function call

delay_ms(70);

lcd_gotoxy(0,0);           
sprintf (buff,"t=%i",q);
lcd_puts(buff);

lcd_gotoxy(7,0);
sprintf (buff,"ve=%i",v);
lcd_puts(buff);

lcd_gotoxy(0,1);
sprintf (buff,"%i",x);
lcd_puts(buff);

lcd_gotoxy(6,1);
sprintf (buff,"%i",y);
lcd_puts(buff);

lcd_gotoxy(12,1);
sprintf (buff,"%x",zh);
lcd_puts(buff);

lcd_gotoxy(14,1);
sprintf (buff,"%x",zl);
lcd_puts(buff);

        if(PIND.0==0)
        {   
        q=q+30;
        OCR1A=q;
        delay_ms(70);      
         
        }
        
        else 
        { 
                }; 
        
         if(PIND.1==0)
        {
        q=q-30;
        OCR1A=q; 
        delay_ms(70);

              }
         
        else 
        { 
               };       
       
      }
}

Выдаёт ошибку что в функции вызова много символов(ошибка в комментариях) подсказали что управление можно осуществить сравнением старшего бита, если 0, то получается положительное число, никаких действий не предпринимается и значение по оси Х прибавляется к OCR1A (OCR1A=1500), если 1 то от числа отнимаем 1, инвертируем и отнимаем это значение от OCR1A. Для этого использую функцию itoa. Получается что при выполнении этой функции число переходит в бинарное, но надо же каждый бит вывести в отдельную ячейку массива, чтобы сравнить старший бит. НЕ могу в этом разобраться, может есть какой способ полегче? Много перелазил сайтов и форумов, нету прямого объяснения, как управляют серво, посредством акселлерометра, есть видео на ютубе. но там только, что сделали показывают. Подскажите.