robot

#include <NewPing.h>                  //knihovna pro ultrazvukovy senzor HC SR-04
#include <Wire.h>                     //knihovna Wire
#include <SoftwareSerial.h>
#include "HCPCA9685.h"                //knihovna pca9685 pouzivam HC-pca
#define  I2CAdd 0x40                  // I2C adresa PCA9685
HCPCA9685 HCPCA9685(I2CAdd);          /* Include the HCPCA9685 library */
#define SERVOMIN  10               // this is the 'minimum' pulse length(out of 4096)
#define SERVOMAX  180              // this is the 'maximum' pulse length(out of 4096)

#define TRIG_PIN A0                // trig pin hc sr-04 pripojen A0
#define ECHO_PIN A1                // echo pin hc sr-04 pripojen A1
#define MAX_DISTANCE 200            // maximalni vzdalenost 90 cm

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);  //propojeni knihovny a modulu HCSR-04

SoftwareSerial Bluetooth(0, 1); // RX | TX   // nastaveni bt HC05 jako komunikacni na pin 0 a 1
//char c = "a";
int led = 13;                        // promenna led13 interni D13
int piezo = 3;                       // promenna pro piezo pin D3
int servonum;                        // promenna cislo serva
int servoangle;                      // promenna uhel serva

void setup()
{
  Serial.begin(9600);                // zapnuti komunikace serial monitor
  Bluetooth.begin(9600);             // zapnuti komunikace bt modulu
  pinMode(led, OUTPUT);              //Sets digital pin 13 as output pin
  digitalWrite(led, LOW);            // nastaveni pin led13 na 0
  pinMode(piezo, OUTPUT);            // nastaveni pieza jako vystup
  digitalWrite(piezo, LOW);          // nastaveni pieza na 0
  HCPCA9685.Init(SERVO_MODE);        // prepnuti pca9685 do modu-SERVO
  HCPCA9685.Sleep(false);            // vypnout spanek PCA9685
}
void loop()
{
  if (Bluetooth.available () == 2);               //pokud bt prijal 4 byty
  {

    uint8_t l1 = Bluetooth.read();               // prvni bit
    uint8_t h1 = Bluetooth.read();               // druhy bit
    //uint8_t l2 = Bluetooth.read();               // treti bit
    //uint8_t h2 = Bluetooth.read();               // ctvrty bit
    servonum = l1; // + (h1 << 8);                   // cislo serva podle promenne
    servoangle = h1;
    //servoangle = l2 + (h2 << 8);                 // uhel serva podle promennne


    unsigned int distance = sonar.ping_cm();     //vypocet vzdalenosti prekazky

    Serial.print(l1);
    Serial.print("- ");
    Serial.print(h1);
    Serial.print("- ");
/*    Serial.print(l2);
    Serial.print("- ");
    Serial.print(h2);
    Serial.print("- ");
*/    Serial.print(distance);
    Serial.println(" cm");

  }
  HCPCA9685.Servo(servonum, servoangle);          // nastavi servo cislo a uhel podle vstupu
  delay(10);                       //prodleva
}