UNO programator v2

#include <SoftwareSerial.h>

///Promenne potrebne pro funkci
byte prichozi[64];
long podpisProcesoru = 0;

//piny virtualniho UARTu
SoftwareSerial programator = SoftwareSerial(10, 11);
long baud = 115200;

//Pin ktery pripojite na RESET hosta, tedy arduina ktere bude progamovano
int rst = 2;

void setup() {


 pinMode(rst, OUTPUT);
 digitalWrite(rst, HIGH);

 Serial.begin(9600);


}

void loop() {

    delay(1000);
    if(beginProgramming()){
      Serial.println("Synchronizovan s cipem");
      if(getParams()){
        Serial.println("Parametry ziskany");
        Serial.print("\nPodpis procesoru: ");
        Serial.print(podpisProcesoru, HEX);
        Serial.print("\n");

        }else Serial.println("Chyba ziskavani parametru!");


      }else Serial.println("Chyba synchronizace s cipem!");

 delay(5000);

}

int beginProgramming(){

  vynulujBuffer();

  //Nastavi rychlost UARTu a rst jako vystup
  programator.begin(baud);
  //Posle pulz pro reset procesoru
  digitalWrite(rst, LOW);
  delay(200);
  digitalWrite(rst, HIGH);
  delay(500);


  //Polse 2 prikazy na ktere ma Arduino odpovedet - STK_GET_SYNC, Sync_CRC_EOP
  programator.write(0x30);
  programator.write(0x20);

  //Pocka dokud neodpovi na prikazy a nacte je do bufferu
  while(!programator.available()){;}
  delay(50);
  for(int i = 0; i <= programator.available(); i++){
      prichozi[i] = programator.read();
  }


  //vyhodnoti jestli jsme dostali OK odpoved
  if(prichozi[0] == 0x14 && prichozi[1] == 0x10){
      return 1;
  }else return 0;


}

int getParams(){

    vynulujBuffer();


   ///////////////// DELAY PRO UNO
     delay(20);

   //Programovaci mod
   programator.write(0x50);
   programator.write(0x20);

   //Vycisti softwareSerial buffer, zapise ho do prichozi
   while(!programator.available()){;}
   delay(50);
   for(int j = 0; j <= programator.available(); j++){
      prichozi[j] = programator.read();
   }


    //vyhodnoti jestli jsme dostali OK odpoved
 if(prichozi[0] == 0x14 && prichozi[1] == 0x10){

      vynulujBuffer();


    ///////////////// DELAY PRO UNO
     delay(20);

     //Precti podpis procesoru
     programator.write(0x75);
     programator.write(0x20);

     //Vycisti softwareSerial buffer, zapise ho do prichozi
     while(!programator.available()){;}
     delay(50);
     //programator.available() se zde nechtela obnovit primo ve for(), nevim proc ale musi se to udelat takto
     int byty = programator.available();
     for(int k = 0; k <= byty; k++){
        prichozi[k] = programator.read();
     }


     //Vyzjisti z bufferu device signature
      if(prichozi[0] == 0x14 && prichozi[4] == 0x10){
        podpisProcesoru = ((unsigned long)prichozi[1] << 16) | ((unsigned long)prichozi[2] << 8) | ((unsigned long)prichozi[3]);

        quitProgramM();

        return 1;
      }else return 0;
  }else return 0;
  }


int programFlash(uint8_t pocatecniAdresaA, uint8_t pocatecniAdresaB, uint8_t velikostStrankyA, uint8_t velikostStrankyB){

    vynulujBuffer();

   ///////////////// DELAY PRO UNO
     delay(20);

   //Programovaci mod
   programator.write(0x50);
   programator.write(0x20);

   //Vycisti softwareSerial buffer, zapise ho do prichozi
   while(!programator.available()){;}
   delay(50);
   for(int j = 0; j <= programator.available(); j++){
      prichozi[j] = programator.read();
   }


    //vyhodnoti jestli jsme dostali OK odpoved
 if(prichozi[0] == 0x14 && prichozi[1] == 0x10){

      vynulujBuffer();

     ///////////////// DELAY PRO UNO
     delay(20);

     //Precti podpis procesoru
     programator.write(0x75);
     programator.write(0x20);

     //Vycisti softwareSerial buffer, zapise ho do prichozi
     while(!programator.available()){;}
     delay(50);
     //programator.available() se zde nechtela obnovit primo ve for(), nevim proc ale musi se to udelat takto
     int byty2 = programator.available();
     for(int k = 0; k <= byty2; k++){
        prichozi[k] = programator.read();
     }


     //Vyzjisti z bufferu device signature
      if(prichozi[0] == 0x14 && prichozi[4] == 0x10){

        vynulujBuffer();

        ///////////////// DELAY PRO UNO
        delay(20);

        programator.write(0x55);

        /////////// TYTO DVA BYTY NASTAVUJI POCATECNI POZICI NA KTERE ZACNE BOOTLOADER ZAPISOVAT ///////////
        programator.write(pocatecniAdresaA);
        programator.write(pocatecniAdresaB);
        ///////////////////////////////////////////////////////////////////////////////////////////////////

        ///////////////// DELAY PRO UNO
        delay(20);

        programator.write(0x20);

        //Vycisti softwareSerial buffer, zapise ho do prichozi
        while(!programator.available()){;}
            delay(50);
            //programator.available() se zde nechtela obnovit primo ve for(), nevim proc ale musi se to udelat takto
            int byty3 = programator.available();
            for(int k = 0; k <= byty3; k++){
            prichozi[k] = programator.read();
        }

        if(prichozi[0] == 0x14 && prichozi[1] == 0x10){

          vynulujBuffer();

          ///////////////// DELAY PRO UNO
          delay(20);

          //Programuj stranku
          programator.write(0x64);

          /////////// TYTO DVA BYTY NASTAVUJI VELIKOST STRANKY ///////////
          programator.write(velikostStrankyA);
          programator.write(velikostStrankyB);
          ///////////////////////////////////////////////////////////////////////////////////////////////////


          ///////////////// DELAY PRO UNO
           delay(20);

          programator.write(0x46);






          ///////////////////////////////////////////////////////////////////////////////////////////////////
          ///////////////////////////////////////////////////////////////////////////////////////////////////
          ///// ZDE SI DOPLNTE KOD PRO ZAPISOVANI DAT, DATA JSOU POSILANA PRES programator.write() A    /////
          /////               NASLEDUJI HNED PO PRIKAZECH KTERE JSEM JIZ ZAPSAL                         /////
          ///////////////////////////////////////////////////////////////////////////////////////////////////
          ///////////////////////////////////////////////////////////////////////////////////////////////////






          //Vycisti softwareSerial buffer, zapise ho do prichozi
        while(!programator.available()){;}
            delay(50);
            //programator.available() se zde nechtela obnovit primo ve for(), nevim proc ale musi se to udelat takto
            int byty3 = programator.available();
            for(int k = 0; k <= byty3; k++){
            prichozi[k] = programator.read();
        }

        //Zjisti zda bylo programovani uspesne
        if(prichozi[0] == 0x14 && prichozi[1] == 0x10){
          return 1;
        }else return 0;

        }

      }else return 0;
  }else return 0;
  }


int quitProgramM(){

  vynulujBuffer();


   ///////////////// DELAY PRO UNO
   delay(20);

  //Opustime programovaci mod
  programator.write(0x51);
  programator.write(0x20);

  while(!programator.available()){;}
            delay(50);
            //programator.available() se zde nechtela obnovit primo ve for(), nevim proc ale musi se to udelat takto
            int byty4 = programator.available();
            for(int k = 0; k <= byty4; k++){
            prichozi[k] = programator.read();
        }

        //Zjisti zda bylo programovani uspesne
        if(prichozi[0] == 0x14 && prichozi[1] == 0x10){
          return 1;
        }else return 0;

  }



 void vynulujBuffer(){
  for(int i = 0; i < 64; i++){
      prichozi[i] = 0x00;
    }

   while(programator.available()){
    char c = programator.read();
    }
  }