Nunchuk remote operated car

#include Wire.h
#include utility/twi.h
#include EEPROM.h
#include "Nunchuk.h"
Nunchuk chuk;
int VM1 = 10;           //viteza motor 1
int VM2 = 11;           //viteza motor 2
int DM1 = 12;           //directie motor 1
int DM2 = 13;           //directie motor 2
int x = 0;              //axa x joystick
int y = 0;              //axa y joystick
int stare_buton_Z = 1;  // variabila pentru citirea starii butonului Z
int stare_led1 = 0;     // variabila aratand starea led-ului
int schimbare1 = 0;
int stare_buton_C = 1;  // variabila pentru citirea starii butonului C
int stare_led2 = 0;     // variabila aratand starea led-ului
int schimbare2 = 0;
int SenS = 8;           //  senzor stanga
int SenD = 9;           //  senzor dreapta
int Directie = 0;
int   viteza = 0;
int   VitezaSpate = 0;
int   VitezaStanga = 0;
int   VitezaDreapta = 0;
void setup()
{
  pinMode(10, OUTPUT);   //viteza motor 1
  pinMode(11, OUTPUT);   //viteza motor 2
  pinMode(12, OUTPUT);   //directie motor 1
  pinMode(13, OUTPUT);   //directie motor 2
  pinMode(6, OUTPUT);    //pin led 1
  pinMode(7, OUTPUT);    //pin led 2 
  pinMode(SenS, INPUT);  //intrare senzor stanga
  pinMode(SenD, INPUT);  //intrare senzor dreapta
  Serial.begin(115200);
  Wire.begin();
  // use 400 kHz for the nunchuck, because it doesn't work reliable with 100 kHz
  TWBR = ((CPU_FREQ / 400000L) - 16) / 2;
  delay(100);
}
void loop()
{
  if(!chuk.ok())
  {
    //Serial.println("Initing nunchuk, it's broken!");
    chuk.init();
  }

  if(chuk.read())
  {
    //Serial.print(chuk.joyX(), DEC);
    //Serial.write(' ');
    //Serial.print(chuk.joyY(), DEC);
    //Serial.write(' ');
    //Serial.print(chuk.buttonZ(), DEC);
    //Serial.write(' ');
    //Serial.print(chuk.buttonC(), DEC);
    //Serial.write(' ');
    //Serial.print(chuk.accX(), DEC);
    //Serial.write(' ');
    //Serial.print(chuk.accY(), DEC);
    //Serial.write(' ');
    //Serial.print(chuk.accZ(), DEC);
    //Serial.println();
  }
  else
  {
    //Serial.println("chuck.read() failed");
  }
  stare_buton_Z = chuk.buttonZ();
  if (stare_buton_Z == 0) {              // Verifica daca butonul Z este apasat
    // Yes, input is 0
    // If LED is on, turn it off
    // If LED is off, turn it on
    if ((stare_led1 == 0) && (schimbare1 == 0)){
      digitalWrite(6, HIGH);           // "Aprinde" led-ul, control din joystick
      stare_led1 = 1;
      schimbare1 = 1;                       // Set change, prevent new change
    }
    if ((stare_led1 == 1) && (schimbare1 == 0)){
      digitalWrite(6, LOW);            // "Stinge" led-ul, control din accelerometru
      stare_led1 = 0;
      schimbare1 = 1;                   // Set change, prevent new change
    }
  }
  else {                               // Butonul Z nu este apasat,
    schimbare1 = 0;                     // intrarea este HIGH din nou, reseteaza "schimbare",
  }    
  stare_buton_C = chuk.buttonC();
  if (stare_buton_C == 0) {              // Verifica daca butonul C este apasat
    // Yes, input is 0
    // If LED is on, turn it off
    // If LED is off, turn it on
    if ((stare_led2 == 0) && (schimbare2 == 0)){
      digitalWrite(7, HIGH);           // "Aprinde" led-ul, control manual
      stare_led2 = 1;
      schimbare2 = 1;                       // Set change, prevent new change
    }
    if ((stare_led2 == 1) && (schimbare2 == 0)){
      digitalWrite(7, LOW);            // "Stinge" led-ul, control automat
      stare_led2 = 0;
      schimbare2 = 1;                   // Set change, prevent new change
    }
  }
  else {                               // Butonul C nu este apasat,
    schimbare2 = 0;                     // intrarea este HIGH din nou, reseteaza "schimbare",
  }   

  if(stare_led1 == 0){                  // Control din accelerometru
    x = (chuk.accX());
    y = (chuk.accY());
    x = constrain(x, 300, 700);       //restrângem valorile pe axa x
    //if (x >= 573)
    if (x >= 550)
    {
      //x = map(x, 573, 700, 0, 127); 
      x = map(x, 550, 700, 30, 127); 
    }
    //else if (x <= 427)
    else if (x <= 450)
    {
      // x = map(x, 300, 427, -127, 0); 
      x = map(x, 300, 450, -127, 30); 
    }  
    else
    {
      x = 0;
    }
    y = constrain(y, 300, 700);

    //if (y >= 573)
    if (y >= 550)
    {
      //y = map(y, 573, 700, 0, 127); 
      y = map(y, 550, 700, 30, 127);
    }
    //else if (y <= 427)
    else if (y <= 450)
    {
      //y = map(y, 300, 427, -127, 0); 
      y = map(y, 300, 450, -127, 30);
    }  
    else
    {
      y = 0;
    }
  }
  if(stare_led1 == 1){                   // Control din joystick
    x = ((chuk.joyX()) - 1);            // Aducere la 0 a axei x
    y = (chuk.joyY());
  }
  Serial.print("X = ");
  Serial.print(x, DEC);
  Serial.write(' ');
  Serial.print("Y = ");
  Serial.print(y, DEC);
  Serial.println();
  if(stare_led2 == 1){           // daca led-ul 2 este "aprins" control manual
    viteza = 0;                    //resetam viteza automata la 0
    VitezaSpate = 0;
    VitezaStanga = 0;
    VitezaDreapta = 0;
    if ( x == 0 && y == 0)       // daca x si y = 0
    {
      digitalWrite(VM1, LOW);    // opreste motoarele
      digitalWrite(VM2, LOW);
    }
    else if (y >= 0)       //daca y >= 0
    {
      int sf = ((2 * y) + x);    //stanga fata
      sf = constrain(sf, -255, 255);
      int df = ((2 * y) - x);    //dreapta fata
      df = constrain(df, -255, 255); 
      Serial.print("SF = ");
      Serial.print(sf, DEC);
      Serial.write(' ');
      Serial.print("DF = ");
      Serial.print(df, DEC);
      Serial.println();
      if (sf > 0)                  // daca stanga fata > 0
      {
        digitalWrite(DM1, LOW);   // porneste motorul 1 in fata
        analogWrite(VM1, sf);     // viteza motor 1
      }
      else if (sf < 0)           // daca stanga fata < 0
      {
        digitalWrite(DM1, HIGH);   // porneste motorul 1 in spate 
        analogWrite(VM1, (map(sf, 0, -255, 0, 255)));     // viteza motor 1 
      }
      if (df > 0)                  // daca dreapta fata > 0
      {
        digitalWrite(DM2, LOW);   // porneste motorul 2 in fata
        analogWrite(VM2, df);     // viteza motor 2
      }
      else if (df < 0)            // daca dreapta fata < 0
      {
        digitalWrite(DM2, HIGH);   // porneste motorul 2 in spate 
        analogWrite(VM2, (map(df, 0, -255, 0, 255)));     // viteza motor 2 
      }
    }
    else if (y < 0)         //daca < 0
    {
      int ss = (-y + x);   //stanga spate
      ss = constrain(ss, -255, 255); 
      int ds = (-y - x);   //dreapta spate
      ss = constrain(ss, -255, 255); 
      Serial.print("SS = ");
      Serial.print(ss, DEC);
      Serial.write(' ');
      Serial.print("DS = ");
      Serial.print(ds, DEC);
      Serial.println();
      if (ss > 0)                  // daca stanga spate > 0
      {
        digitalWrite(DM1, HIGH);   // porneste motorul 1 in spate
        analogWrite(VM1, ss);     // viteza motor 1
      }
      else if (ss < 0)            // daca stanga spate < 0
      {
        digitalWrite(DM1, LOW);   // porneste motorul 1 in fata 
        analogWrite(VM1, (map(ss, 0, -255, 0, 255)));     // viteza motor 1 
      }

      if (ds > 0)                  // daca dreapta spate > 0
      {
        digitalWrite(DM2, HIGH);   // porneste motorul 2 in spate
        analogWrite(VM2, ds);      // viteza motor 2
      }
      else if (ds < 0)            // daca dreapta spate < 0
      {
        digitalWrite(DM2, LOW);   // porneste motorul 2 in fata 
        analogWrite(VM2, (map(ds, 0, -255, 0, 255)));     //  viteza motor 2
      }
    }
  }
  else if(stare_led2 == 0){         //daca led-ul 2 este "stins" control automat
    if ( digitalRead(SenS) == 1 && digitalRead(SenD) == 1 && Directie == 0)  { //mers in fata
      VitezaSpate = 0;              //resetam viteza spate la 0
      VitezaStanga = 0;             //resetam stanga spate la 0
      VitezaDreapta = 0;            //resetam dreapta spate la 0
      digitalWrite(DM1, LOW);       // porneste motorul 1 in fata
      digitalWrite(DM2, LOW);       // porneste motorul 2 in fata
      viteza += 1;
      viteza = min(viteza, 200);    // viteza nu va depasi 200
      analogWrite(VM1, viteza);     // viteza motor 1
      analogWrite(VM2, viteza);     // viteza motor 2
      Serial.print("Viteza 1 = ");
      Serial.print(viteza, DEC);
      Serial.write(' ');
      Serial.print("Viteza 2 = ");
      Serial.print(viteza, DEC);
      Serial.println();
      delay(5);
    }
    else if ( digitalRead(SenS) == 0 && digitalRead(SenD) == 0)  {//daca este un obstacol in fata
      viteza = 0;                   //setam viteza automata la 0
      VitezaStanga = 0;             //resetam stanga spate la 0
      VitezaDreapta = 0;            //resetam dreapta spate la 0
      Directie = 1;
    }
    else if ( digitalRead(SenS) == 0 && digitalRead(SenD) == 1)  {//daca este un obstacol in stanga
      viteza = 0;                   //setam viteza automata la 0
      VitezaSpate = 0;              //resetam viteza spate la 0
      VitezaDreapta = 0;            //resetam dreapta spate la 0
      Directie = 2;
    }
    else if ( digitalRead(SenS) == 1 && digitalRead(SenD) == 0)  {//daca este un obstacol in dreapta
      viteza = 0;                   //setam viteza automata la 0
      VitezaStanga = 0;             //resetam stanga spate la 0
      VitezaSpate = 0;              //resetam viteza spate la 0
      Directie = 3;
    }
    if (Directie == 1)                     //mers in spate
    {
      digitalWrite(DM1, HIGH);    // porneste motorul 1 in spate
      digitalWrite(DM2, HIGH);    // porneste motorul 2 in spate
      VitezaSpate += 1;
      VitezaSpate = min(VitezaSpate, 200);    // viteza nu va depasi 200
      analogWrite(VM1, VitezaSpate);          // viteza motor 1
      analogWrite(VM2, VitezaSpate / 2);      // viteza motor 2
      if (VitezaSpate == 200) { 
        Directie = 0;
      }
      //delay(5);
      //Serial.print("Viteza 1 = ");
      //Serial.print(VitezaSpate, DEC);
      //Serial.write(' ');
      //Serial.print("Viteza 2 = ");
      //Serial.print(VitezaSpate / 2, DEC);
      //Serial.println();
    }
    else if (Directie == 2)            // 
    {
      //digitalWrite(DM1, LOW);     // porneste motorul 1 in fata
      digitalWrite(DM1, HIGH);    // porneste motorul 1 in spate
      digitalWrite(DM2, HIGH);    // porneste motorul 2 in spate
      VitezaStanga += 1;
      VitezaStanga = min(VitezaStanga, 200);   // viteza nu va depasi 200
      analogWrite(VM1, VitezaStanga);      // viteza motor 1
      analogWrite(VM2, VitezaStanga / 2);          // viteza motor 2
      if (VitezaStanga == 200) { 
        Directie = 0;
      }
      //delay(5);
    }
    else if (Directie == 3)            // 
    {
      digitalWrite(DM1, HIGH);    // porneste motorul 1 in spate
      digitalWrite(DM2, HIGH);    // porneste motorul 2 in spate
      //digitalWrite(DM2, LOW);    // porneste motorul 2 in fata
      VitezaDreapta += 1;
      VitezaDreapta = min(VitezaDreapta, 200);  // viteza nu va depasi 200
      analogWrite(VM1, VitezaDreapta / 2);          // viteza motor 1
      analogWrite(VM2, VitezaDreapta);      // viteza motor 2
      if (VitezaDreapta == 200) { 
        Directie = 0;  
      }
      //delay(5);
      //Serial.print("Viteza 1 = ");
      //Serial.print(VitezaDreapta, DEC);
      //Serial.write(' ');
      //Serial.print("Viteza 2 = ");
      //Serial.print(VitezaDreapta / 2, DEC);
      //Serial.println();
    }
  }
  Serial.print("Directie = ");
  Serial.print(Directie, DEC);
  Serial.println();
  delay(5);
}