RC#1

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Servo.h>

#define opto 7
#define silnik 3
#define doprzodu 4
#define dotylu 5

Servo Serwo;
RF24 radio(9, 10);

const uint64_t pipe = 0xF0F0F0F0F0;
int servo = 0;
int servoo = 120;
int value = 0;
int valuee = 515;
int obroty = 0;
int stan = 0;
int blokada = 0;

unsigned long czas;
unsigned long czas1;

struct package
{
  int X = 512;
  int Y = 512;
};
typedef struct package Package;
Package data;

void setup()
{
  czas = millis();
  Serial.begin(115200);
  delay(100);

  pinMode(doprzodu, INPUT);
  pinMode(dotylu, INPUT);
  pinMode(opto, INPUT_PULLUP);
  pinMode(silnik, OUTPUT);
  Serwo.attach(6);

  radio.begin();
  radio.setDataRate(RF24_2MBPS);
  radio.setPALevel(RF24_PA_HIGH);
  radio.openReadingPipe(1, pipe);
  radio.startListening();
}

void loop()
{
  if (millis() - czas1 > 100)
  {
    if ( radio.available())
    {
      while (radio.available())
      {
        radio.read(&data, sizeof(data));
      }
      Serial.println("Radio dostępne");
      int X = data.X;
      int Y = data.Y;
      value = Y;

      if ( (value > 475) && (value < 555) )
      {
        digitalWrite(doprzodu, LOW);
        digitalWrite(dotylu, LOW);
      }
      else if (value > 555)
      {
        digitalWrite(doprzodu, HIGH);
        digitalWrite(dotylu, LOW);
        value = map(value, 555, 1023, 0, 255);
        analogWrite(silnik, value);
      }
      else if (value < 475)
      {
        digitalWrite(doprzodu, LOW);
        digitalWrite(dotylu, HIGH);
        value = map(value, 0, 475, 255, 0);
        analogWrite(silnik, value);
      }
      servo = map(X, 0, 1023, 90, 145);
      Serwo.write(servo);
    }
    else
    {
      Serial.println("Radio nie dostępne");
      Serwo.write(servoo);
      value = valuee;
    }
    czas1 = millis();
  }

  stan = digitalRead(opto);
  if (stan == HIGH && blokada == 0)
  {
    obroty = obroty + 1;
    blokada = 1;
  }
  if (stan == LOW)  {
    blokada = 0;
  }
  if (millis() - czas > 1000)
  {
    obroty = 0;
    czas = millis();
  }

  float rpm = (obroty * 60) / 4;
  float rps = obroty / 4;
  float km = (rpm * 16) * 60 / 100000;

  if (millis() - czas > 999)
  {
    Serial.println("RPM:");
    Serial.print(rpm);
    Serial.println();
    Serial.println("RPS:");
    Serial.print(rps);
    Serial.println();
    Serial.println("KM/H:");
    Serial.print(km);
    Serial.println();
    Serial.print("X:");
    Serial.print(data.X);
    Serial.print("    Y:");
    Serial.print(data.Y);
    Serial.println();
  }
}