#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x42);
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x41);
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
// you can also call it with a different address and I2C interface
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, &Wire);

// Depending on your servo make, the pulse width min and max may vary, you
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!
#define LSTOPAMIN 290
#define LSTOPAMAX 340
#define LKOSTKAMIN  190
#define LKOSTKAMAX  400
#define LKOLANOMIN 444
#define LKOLANOMAX 230
#define LUDOMIN 370
#define LUDOMAX 210
#define LMIEDNICAMIN 240
#define LMIEDNICAMAX  360


#define RSTOPAMIN 255
#define RSTOPAMAX 304
#define RKOSTKAMIN  430
#define RKOSTKAMAX  200
#define RKOSTKAW  255
#define RKOLANOMIN 425
#define RKOLANOMAX 195
#define RUDOMIN 290
#define RUDOMAX 130
#define RMIEDNICAMIN 230
#define RMIEDNICAMAX 350




#define LBARKWMIN 95
#define LBARKWMAX 440
#define LBARKZMIN 455
#define LBARKZMAX 105
#define LLOKIECMIN 90
#define LLOKIECMAX 260
#define LNADGARSTEKMIN 105
#define LNADGARSTEKMAX 465


#define RBARKWMIN 105
#define RBARKWMAX 455
#define RBARKZMIN 95
#define RBARKZMAX 455
#define RLOKIECMIN 465
#define RLOKIECMAX 300
#define RNADGARSTEKMIN 445
#define RNADGARSTEKMAX 100
int i = 180;
int pozycja_reka=0;
int pozycja_noga=0;
int LSTOPA = 0;
int LKOSTKA = 0;
int LKOLANO = 0;
int LUDO = 0;
int LMIEDNICA = 0;

int RSTOPA = 0;
int RKOSTKA = 0;
int RKOLANO = 0;
int RUDO = 0;
int RMIEDNICA = 0;



int LBARKW=0;
int LBARKZ=0;
int LLOKIEC=0;
int LNADGARSTEK=0;

int RBARKW=0;
int RBARKZ=0;
int RLOKIEC=0;
int RNADGARSTEK=0;
///////////////kąty dla danego położenia rąk//////////////////////////////////////////////////
int rece_pozycje[8][8] =
{
  {0,0,0,90,0,0,0,90},
  {0,0,90,90,0,0,90,90},
  {180,0,0,90,180,0,0,90},
  };
  /////////////////wartości potrzebne do obliczenia opóznienia ręki///////////////////////////////////
float rece_timer[3][8]
{0,0,0,90,0,0,0,90};
//////////////////////sterowanie opóznieniem ręki ///////////////////////////////////////////////////
unsigned long rece_delay[3][8]=
  {                                                        //// w mikrosekundach, aby nie było problemu z np współczynnikiem 1,125
    {0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0},
  };
unsigned long przerwa=10; /// dzielnik przerwy poszczególnego kroku

///////////////kąty dla danego położenia nóg//////////////////////////////////////////////////
int nogi_pozycje[8][10]=
{{90,130,180,65,90,90,130,180,65,90}, ///// pozycja 0 - wyprostowana pozycja
 {90,5,0,0,90,90,5,0,0,90},
 };

/////////////////////////wartości potrzebne do obliczenia opóznienia nóg///////////////////////////
float nogi_timer[3][10]
{90,130,180,65,90,90,130,180,65,90};

////////////////////////////////sterowanie opoźnienie nóg////////////////////////////////////////////
unsigned long nogi_delay[3][10]=
{                                                        //// w mikrosekundach, aby nie było problemu z np współczynnikiem 1,125
    {0,0,0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0},
  };

unsigned long przerwa_nogi=50;

int D = 6;
void setup() {
  Serial.begin(9600);
  pwm.begin();
  pwm2.begin();
  pwm.setPWMFreq(50);  // Analog servos run at ~60 Hz updates
  pwm2.setPWMFreq(50);
}
void loop() {
  delay(1000);
//  for(pozycja_reka=0;pozycja_reka<=2;pozycja_reka++)
//  {
//    rece(rece_pozycje[pozycja_reka][0],rece_pozycje[pozycja_reka][1],
//        rece_pozycje[pozycja_reka][2],rece_pozycje[pozycja_reka][3],
//        rece_pozycje[pozycja_reka][4],rece_pozycje[pozycja_reka][5],
//        rece_pozycje[pozycja_reka][6],rece_pozycje[pozycja_reka][7]);
//        delay(4000);
//  }
for(pozycja_noga=0;pozycja_noga<=0 ;pozycja_noga++){
  nogi(nogi_pozycje[pozycja_noga][0],nogi_pozycje[pozycja_noga][1],nogi_pozycje[pozycja_noga][2],nogi_pozycje[pozycja_noga][3],nogi_pozycje[pozycja_noga][4],
  nogi_pozycje[pozycja_noga][5],nogi_pozycje[pozycja_noga][6],nogi_pozycje[pozycja_noga][7],nogi_pozycje[pozycja_noga][8],nogi_pozycje[pozycja_noga][9]);
  delay(6000);
  }
  }
void write_data_nogi()
{ pwm.setPWM(0, 0, LSTOPA);
  pwm.setPWM(1, 0, LKOSTKA);
  pwm.setPWM(2, 0, LKOLANO);
  pwm.setPWM(3, 0, LUDO);
  pwm.setPWM(4, 0, LMIEDNICA);
  pwm.setPWM(5, 0, RSTOPA);
  pwm.setPWM(6, 0, RKOSTKA);
  pwm.setPWM(7, 0, RKOLANO);
  pwm.setPWM(8, 0, RUDO);
  pwm.setPWM(9, 0, RMIEDNICA);

}

void write_data_rece()
{
    pwm2.setPWM(0, 0, LBARKW);
  pwm2.setPWM(1, 0, LBARKZ);
  pwm2.setPWM(2, 0, LLOKIEC);
  pwm2.setPWM(3, 0, LNADGARSTEK);
  pwm2.setPWM(4, 0, RBARKW);
  pwm2.setPWM(5, 0, RBARKZ);
  pwm2.setPWM(6, 0, RLOKIEC);
  pwm2.setPWM(7, 0, RNADGARSTEK);
}

int nogi(int a,int b,int c,int d, int e, int f, int g, int h, int i, int j)
{

  /////////////////// różnica zmiany kąta nogi ///////////////////////////////
  nogi_timer[1][0]=fabs(a-nogi_timer[0][0]);
  nogi_timer[1][1]=fabs(b-nogi_timer[0][1]);
  nogi_timer[1][2]=fabs(c-nogi_timer[0][2]);
  nogi_timer[1][3]=fabs(d-nogi_timer[0][3]);
  nogi_timer[1][4]=fabs(e-nogi_timer[0][4]);
   nogi_timer[1][5]=fabs(f-nogi_timer[0][5]);
  nogi_timer[1][6]=fabs(g-nogi_timer[0][6]);
  nogi_timer[1][7]=fabs(h-nogi_timer[0][7]);
  nogi_timer[1][8]=fabs(i-nogi_timer[0][8]);
  nogi_timer[1][9]=fabs(j-nogi_timer[0][9]);
  //////////////////// obliczanie współczynnika opóźnienia nogi //////////////////////////////////////////
  if(nogi_timer[1][0]==0)
  {nogi_timer[2][0]=0;
  LSTOPA=map(a, 0,180, LSTOPAMIN, LSTOPAMAX);}
  else {nogi_timer[2][0]=round(180/nogi_timer[1][0]*1000);}
   //////////////////////////////////
  if(nogi_timer[1][1]==0)
  {nogi_timer[2][1]=0;
  LKOSTKA=map(b, 0,180, LKOSTKAMIN, LKOSTKAMAX);}
  else{nogi_timer[2][1]=round(180/nogi_timer[1][1]*1000);}
  ///////////////////////////////////
  if(nogi_timer[1][2]==0)
  {nogi_timer[2][2]=0;
  LKOLANO=map(c, 0,180, LKOLANOMIN, LKOLANOMAX);}
  else{nogi_timer[2][2]=round(180/nogi_timer[1][2]*1000);}
  ////////////////////////////////////
  if(nogi_timer[1][3]==0)
  {nogi_timer[2][3]=0;
  LUDO=map(d, 0,180, LUDOMIN, LUDOMAX);}
  else{nogi_timer[2][3]=round(180/nogi_timer[1][3]*1000);}
  /////////////////////////////
  if(nogi_timer[1][4]==0)
  {nogi_timer[2][4]=0;
  LMIEDNICA=map(e, 0,180, LMIEDNICAMIN, LMIEDNICAMAX);}
  else{nogi_timer[2][4]=round(180/nogi_timer[1][4]*1000);}
  /////////////////////////////////
  if(nogi_timer[1][5]==0)
  {nogi_timer[2][5]=0;
  RSTOPA=map(f, 0,180, RSTOPAMIN, RSTOPAMAX);}
  else{nogi_timer[2][5]=round(180/nogi_timer[1][5]*1000);}
  ////////////////////////////////////
  if(nogi_timer[1][6]==0)
  {nogi_timer[2][6]=0;
  RKOSTKA=map(g, 0,180, RKOSTKAMIN, RKOSTKAMAX);}
  else{nogi_timer[2][6]=round(180/nogi_timer[1][6]*1000);}
  ////////////////////////
  if(nogi_timer[1][7]==0)
  {nogi_timer[2][7]=0;
  RKOLANO=map(h, 0,180, RKOLANOMIN, RKOLANOMAX);}
  else{nogi_timer[2][7]=round(180/nogi_timer[1][7]*1000);}
  ///////////////////////////////
  if(nogi_timer[1][8]==0)
  {nogi_timer[2][8]=0;
  RUDO=map(i, 0,180, RUDOMIN, RUDOMAX);}
  else{nogi_timer[2][8]=round(180/nogi_timer[1][8]*1000);}
  ///////////////////////
  if(nogi_timer[1][9]==0)
  {nogi_timer[2][9]=0;
  RMIEDNICA=map(j, 0,180, RMIEDNICAMIN, RMIEDNICAMAX);}
  else{nogi_timer[2][9]=round(180/nogi_timer[1][9]*1000);}
//////////////////////konwersja float'a na unsigned long////////////////////////////////////
nogi_delay[0][0]=(unsigned long)nogi_timer[2][0];
nogi_delay[0][1]=(unsigned long)nogi_timer[2][1];
nogi_delay[0][2]=(unsigned long)nogi_timer[2][2];
nogi_delay[0][3]=(unsigned long)nogi_timer[2][3];
nogi_delay[0][4]=(unsigned long)nogi_timer[2][4];
nogi_delay[0][5]=(unsigned long)nogi_timer[2][5];
nogi_delay[0][6]=(unsigned long)nogi_timer[2][6];
nogi_delay[0][7]=(unsigned long)nogi_timer[2][7];
nogi_delay[0][8]=(unsigned long)nogi_timer[2][8];
nogi_delay[0][9]=(unsigned long)nogi_timer[2][9];
for(int x=0;x<=9;x++){
nogi_delay[2][x]=micros();}

do {
///////////////////odczyt czasu zegara wewnętrznego////////////////////////////////////////////////
for(int k=0;k<=9;k++){
nogi_delay[1][k]=micros();
}
//////////////////////////////////////wysyłanie nowych wartości serw/////////////////////////////////////////

/////////////////////////////////sterowanie serwo 0///////////////////////////////////////////////
if((nogi_delay[1][0]-nogi_delay[2][0]>=nogi_delay[0][0]*przerwa_nogi)&&(nogi_delay[0][0]!=0))
{
  LSTOPA=map(nogi_timer[0][0], 0,180, LSTOPAMIN, LSTOPAMAX);
  write_data_nogi();
  nogi_delay[2][0]=nogi_delay[1][0];
  if(a>nogi_timer[0][0]){
  nogi_timer[0][0]++;}
  if(a<nogi_timer[0][0]){
  nogi_timer[0][0]--;}
}

/////////////////////////////////sterowanie serwo 1///////////////////////////////////////////////
if((nogi_delay[1][1]-nogi_delay[2][1]>=nogi_delay[0][1]*przerwa_nogi)&&(nogi_delay[0][1]!=0))
{
  LKOSTKA=map(nogi_timer[0][1], 0,180, LKOSTKAMIN, LKOSTKAMAX);
  write_data_nogi();
  nogi_delay[2][1]=nogi_delay[1][1];
  if(b>nogi_timer[0][1]){
  nogi_timer[0][1]++;}
  if(b<nogi_timer[0][1]){
  nogi_timer[0][1]--;}
}

/////////////////////////////////sterowanie serwo 2///////////////////////////////////////////////
if((nogi_delay[1][2]-nogi_delay[2][2]>=nogi_delay[0][2]*przerwa_nogi)&&(nogi_delay[0][2]!=0))
{
  LKOLANO=map(nogi_timer[0][2], 0,180, LKOLANOMIN, LKOLANOMAX);
  write_data_nogi();
  nogi_delay[2][2]=nogi_delay[1][2];
  if(c>nogi_timer[0][2]){
  nogi_timer[0][2]++;}
  if(c<nogi_timer[0][2]){
  nogi_timer[0][2]--;}
}

/////////////////////////////////sterowanie serwo 3///////////////////////////////////////////////
if((nogi_delay[1][3]-nogi_delay[2][3]>=nogi_delay[0][3]*przerwa_nogi)&&(nogi_delay[0][3]!=0))
{
  LUDO=map(nogi_timer[0][3], 0,180, LUDOMIN, LUDOMAX);
  write_data_nogi();
  nogi_delay[2][3]=nogi_delay[1][3];
  if(d>nogi_timer[0][3]){
  nogi_timer[0][3]++;}
  if(d<nogi_timer[0][3]){
  nogi_timer[0][3]--;}
}

/////////////////////////////////sterowanie serwo 4///////////////////////////////////////////////
if((nogi_delay[1][4]-nogi_delay[2][4]>=nogi_delay[0][4]*przerwa_nogi)&&(nogi_delay[0][4]!=0))
{
  LMIEDNICA=map(nogi_timer[0][4], 0,180, LMIEDNICAMIN, LMIEDNICAMAX);
  write_data_nogi();
  nogi_delay[2][4]=nogi_delay[1][4];
  if(e>nogi_timer[0][4]){
  nogi_timer[0][4]++;}
  if(e<nogi_timer[0][4]){
  nogi_timer[0][4]--;}
}

/////////////////////////////////sterowanie serwo 5///////////////////////////////////////////////
if((nogi_delay[1][5]-nogi_delay[2][5]>=nogi_delay[0][5]*przerwa_nogi)&&(nogi_delay[0][5]!=0))
{
  RSTOPA=map(nogi_timer[0][5], 0,180, RSTOPAMIN, RSTOPAMAX);
  write_data_nogi();
  nogi_delay[2][5]=nogi_delay[1][5];
  if(f>nogi_timer[0][5]){
  nogi_timer[0][5]++;}
  if(f<nogi_timer[0][5]){
  nogi_timer[0][5]--;}
}

/////////////////////////////////sterowanie serwo 6///////////////////////////////////////////////
if((nogi_delay[1][6]-nogi_delay[2][6]>=nogi_delay[0][6]*przerwa_nogi)&&(nogi_delay[0][6]!=0))
{
  RKOSTKA=map(nogi_timer[0][6], 0,180, RKOSTKAMIN, RKOSTKAMAX);
  write_data_nogi();
  nogi_delay[2][6]=nogi_delay[1][6];
  if(g>nogi_timer[0][6]){
  nogi_timer[0][6]++;}
  if(g<nogi_timer[0][6]){
  nogi_timer[0][6]--;}
}

/////////////////////////////////sterowanie serwo 7///////////////////////////////////////////////
if((nogi_delay[1][7]-nogi_delay[2][7]>=nogi_delay[0][7]*przerwa_nogi)&&(nogi_delay[0][7]!=0))
{
  RKOLANO=map(nogi_timer[0][7], 0,180, RKOLANOMIN, RKOLANOMAX);
  write_data_nogi();
  nogi_delay[2][7]=nogi_delay[1][7];
  if(h>nogi_timer[0][7]){
  nogi_timer[0][7]++;}
  if(h<nogi_timer[0][7]){
  nogi_timer[0][7]--;}
}

/////////////////////////////////sterowanie serwo 8///////////////////////////////////////////////
if((nogi_delay[1][8]-nogi_delay[2][8]>=nogi_delay[0][8]*przerwa_nogi)&&(nogi_delay[0][8]!=0))
{
  RUDO=map(nogi_timer[0][8], 0,180, RUDOMIN, RUDOMAX);
  write_data_nogi();
  nogi_delay[2][8]=nogi_delay[1][8];
  if(i>nogi_timer[0][8]){
  nogi_timer[0][8]++;}
  if(i<nogi_timer[0][8]){
  nogi_timer[0][8]--;}
}

/////////////////////////////////sterowanie serwo 9///////////////////////////////////////////////
if((nogi_delay[1][9]-nogi_delay[2][9]>=nogi_delay[0][9]*przerwa_nogi)&&(nogi_delay[0][9]!=0))
{
  RMIEDNICA=map(nogi_timer[0][9], 0,180, RMIEDNICAMIN, RMIEDNICAMAX);
  write_data_nogi();
  nogi_delay[2][9]=nogi_delay[1][9];
  if(j>nogi_timer[0][9]){
  nogi_timer[0][9]++;}
  if(j<nogi_timer[0][9]){
  nogi_timer[0][9]--;}
}
}while((nogi_timer[0][0]!=a)||(nogi_timer[0][1]!=b)||(nogi_timer[0][2]!=c)||(nogi_timer[0][3]!=d)||
(nogi_timer[0][4]!=e)||(nogi_timer[0][5]!=f)||(nogi_timer[0][6]!=g)||(nogi_timer[0][7]!=h)||(nogi_timer[0][8]!=i)||(nogi_timer[0][9]!=j));
nogi_timer[0][0]=a;
nogi_timer[0][1]=b;
nogi_timer[0][2]=c;
nogi_timer[0][3]=d;
nogi_timer[0][4]=e;
nogi_timer[0][5]=f;
nogi_timer[0][6]=g;
nogi_timer[0][7]=h;
nogi_timer[0][8]=i;
nogi_timer[0][9]=j;

}


//LSTOPA=map(a, 0,180, LSTOPAMIN, LSTOPAMAX);
//LKOSTKA=map(b, 0,180, LKOSTKAMIN, LKOSTKAMAX);
//LKOLANO=map(c, 0,180, LKOLANOMIN, LKOLANOMAX);
//LUDO=map(d, 0,180, LUDOMIN, LUDOMAX);
//LMIEDNICA=map(e, 0,180, LMIEDNICAMIN, LMIEDNICAMAX);
//
//RSTOPA=map(f, 0,180, RSTOPAMIN, RSTOPAMAX);
//RKOSTKA=map(g, 0,180, RKOSTKAMIN, RKOSTKAMAX);
//RKOLANO=map(h, 0,180, RKOLANOMIN, RKOLANOMAX);
//RUDO=map(i, 0,180, RUDOMIN, RUDOMAX);
//RMIEDNICA=map(j, 0,180, RMIEDNICAMIN, RMIEDNICAMAX);


 int rece(int a,int b,int c,int d,int e,int f, int g, int h)
{
 a=map(a,0,180,180,0);
////////////// różnica zmiany kąta ręki ////////////////////////////////////////////
rece_timer[1][0]=fabs(a-rece_timer[0][0]);
rece_timer[1][1]=fabs(b-rece_timer[0][1]);
rece_timer[1][2]=fabs(c-rece_timer[0][2]);
rece_timer[1][3]=fabs(d-rece_timer[0][3]);
rece_timer[1][4]=fabs(e-rece_timer[0][4]);
rece_timer[1][5]=fabs(f-rece_timer[0][5]);
rece_timer[1][6]=fabs(g-rece_timer[0][6]);
rece_timer[1][7]=fabs(h-rece_timer[0][7]);
//////////////////// obliczanie współczynnika opóźnienia//////////////////////////////////////////
rece_timer[2][0]=round(180/rece_timer[1][0]*1000);
rece_timer[2][1]=round(180/rece_timer[1][1]*1000);
rece_timer[2][2]=round(90/rece_timer[1][2]*2000);
rece_timer[2][3]=round(180/rece_timer[1][3]*1000);
rece_timer[2][4]=round(180/rece_timer[1][4]*1000);
rece_timer[2][5]=round(180/rece_timer[1][5]*1000);
rece_timer[2][6]=round(90/rece_timer[1][6]*2000);
rece_timer[2][7]=round(180/rece_timer[1][7]*1000);
//////////////////////konwersja float'a na unsigned long////////////////////////////////////
rece_delay[0][0]=(unsigned long)rece_timer[2][0];
rece_delay[0][1]=(unsigned long)rece_timer[2][1];
rece_delay[0][2]=(unsigned long)rece_timer[2][2];
rece_delay[0][3]=(unsigned long)rece_timer[2][3];
rece_delay[0][4]=(unsigned long)rece_timer[2][4];
rece_delay[0][5]=(unsigned long)rece_timer[2][5];
rece_delay[0][6]=(unsigned long)rece_timer[2][6];
rece_delay[0][7]=(unsigned long)rece_timer[2][7];


do {
///////////////////odczyt czasu zegara wewnętrznego////////////////////////////////////////////////
for(int p=0;p<=7;p++){
rece_delay[1][p]=micros();
}
//////////////////////////////////////wysyłanie nowych wartości serw/////////////////////////////////////////

/////////////////////////////////sterowanie serwo 0///////////////////////////////////////////////
if(rece_delay[1][0]-rece_delay[2][0]>=przerwa/rece_delay[0][0])
{
  LBARKW=map(rece_timer[0][0], 0,180, LBARKWMIN, LBARKWMAX);
  write_data_rece();
  rece_delay[2][0]=rece_delay[1][0];
  if(a>rece_timer[0][0]){
  rece_timer[0][0]++;}
  if(a<rece_timer[0][0]){
  rece_timer[0][0]--;}
}

/////////////////////////////////sterowanie serwo 1///////////////////////////////////////////////
  if(rece_delay[1][1]-rece_delay[2][1]>=przerwa/rece_delay[0][1])
{
  LBARKZ=map(rece_timer[0][1], 0,180, LBARKZMIN, LBARKZMAX);
  write_data_rece();
  rece_delay[2][1]=rece_delay[1][1];
  if(b>rece_timer[0][1]){
  rece_timer[0][1]++;}
  if(b<rece_timer[0][1]){
  rece_timer[0][1]--;}
}
/////////////////////////////////sterowanie serwo 2///////////////////////////////////////////////
  if(rece_delay[1][2]-rece_delay[2][2]>=przerwa/rece_delay[0][2])
{
  LLOKIEC=map(rece_timer[0][2], 0,90, LLOKIECMIN, LLOKIECMAX);
  write_data_rece();
  rece_delay[2][2]=rece_delay[1][2];
  if(c>rece_timer[0][2]){
  rece_timer[0][2]++;}
  if(c<rece_timer[0][2]){
  rece_timer[0][2]--;}
}
/////////////////////////////////sterowanie serwo 3///////////////////////////////////////////////
  if(rece_delay[1][3]-rece_delay[2][3]>=przerwa/rece_delay[0][3])
{
  LNADGARSTEK=map(rece_timer[0][3], 0,180, LNADGARSTEKMIN, LNADGARSTEKMAX);
  write_data_rece();
  rece_delay[2][3]=rece_delay[1][3];
  if(d>rece_timer[0][3]){
  rece_timer[0][3]++;}
  if(d<rece_timer[0][3]){
  rece_timer[0][3]--;}
}
/////////////////////////////////sterowanie serwo 4///////////////////////////////////////////////
  if(rece_delay[1][4]-rece_delay[2][4]>=przerwa/rece_delay[0][4])
{
  RBARKW=map(rece_timer[0][4], 0,180, RBARKWMIN, RBARKWMAX);
  write_data_rece();
  rece_delay[2][4]=rece_delay[1][4];
  if(e>rece_timer[0][4]){
  rece_timer[0][4]++;}
  if(e<rece_timer[0][4]){
  rece_timer[0][4]--;}
}
/////////////////////////////////sterowanie serwo 5///////////////////////////////////////////////
  if(rece_delay[1][5]-rece_delay[2][5]>=przerwa/rece_delay[0][5])
{
  RBARKZ=map(rece_timer[0][5], 0,180, RBARKZMIN, RBARKZMAX);
  write_data_rece();
  rece_delay[2][5]=rece_delay[1][5];
  if(f>rece_timer[0][5]){
  rece_timer[0][5]++;}
  if(f<rece_timer[0][5]){
  rece_timer[0][5]--;}
}
/////////////////////////////////sterowanie serwo 6///////////////////////////////////////////////
  if(rece_delay[1][6]-rece_delay[2][6]>=przerwa/rece_delay[0][6])
{
  RLOKIEC=map(rece_timer[0][6], 0,90, RLOKIECMIN, RLOKIECMAX);
  write_data_rece();
  rece_delay[2][6]=rece_delay[1][6];
  if(g>rece_timer[0][6]){
  rece_timer[0][6]++;}
  if(g<rece_timer[0][6]){
  rece_timer[0][6]--;}
}
/////////////////////////////////sterowanie serwo 7///////////////////////////////////////////////
  if(rece_delay[1][7]-rece_delay[2][7]>=przerwa/rece_delay[0][7])
{
  RNADGARSTEK=map(rece_timer[0][7], 0,180, RNADGARSTEKMIN, RNADGARSTEKMAX);
  write_data_rece();
  rece_delay[2][7]=rece_delay[1][7];
  if(h>rece_timer[0][7]){
  rece_timer[0][7]++;}
  if(h<rece_timer[0][7]){
  rece_timer[0][7]--;}
}
}while((rece_timer[0][0]!=a)||(rece_timer[0][1]!=b)||(rece_timer[0][2]!=c)||(rece_timer[0][3]!=d)||
(rece_timer[0][4]!=e)||(rece_timer[0][5]!=f)||(rece_timer[0][6]!=g)||(rece_timer[0][7]!=h));
rece_timer[0][0]=a;
rece_timer[0][1]=b;
rece_timer[0][2]=c;
rece_timer[0][3]=d;
rece_timer[0][4]=e;
rece_timer[0][5]=f;
rece_timer[0][6]=g;
rece_timer[0][7]=h;
  }