#include <SPI.h>
#include "RF24.h"
#define TRIG_PIN A5
#define ECHO_PIN 2
#define TIME_OUT 5000
#define TRIG1_PIN 1
#define ECHO1_PIN 0

const uint64_t pipe = 0xE8E8F0F0E1LL; // địa chỉ phát
RF24 radio(9,10);//thay 10 thành 53 với mega
byte msg[3];
int led_st = 0;
 //Motor A
int enA = 8;
int in1 = 7;
int in2 = 6;

//Motor B
int in3 = 5;
int in4 = 4;
int enB = 3;


float GetDistance()
{
	long duration, distanceCm;

	digitalWrite(TRIG_PIN, LOW);
	delayMicroseconds(2);
	digitalWrite(TRIG_PIN, HIGH);
	delayMicroseconds(10);
	digitalWrite(TRIG_PIN, LOW);

	duration = pulseIn(ECHO_PIN, HIGH, TIME_OUT);

	// convert to distance
	distanceCm = duration / 29.1 / 2;

	return distanceCm;
}

float GetDistance1()
{
	long duration, distanceCm1;

	digitalWrite(TRIG1_PIN, LOW);
	delayMicroseconds(2);
	digitalWrite(TRIG1_PIN, HIGH);
	delayMicroseconds(10);
	digitalWrite(TRIG1_PIN, LOW);

	duration = pulseIn(ECHO1_PIN, HIGH, TIME_OUT);

	// convert to distance1
	distanceCm1 = duration / 29.1 / 2;

	return distanceCm1;
}

void setup(){
  Serial.begin(115200);
  radio.begin();
  radio.setAutoAck(1);
  radio.setDataRate(RF24_1MBPS);    // Tốc độ dữ liệu
  radio.setChannel(10);               // Đặt kênh
  radio.openReadingPipe(1,pipe);
  radio.startListening();
   pinMode(enA, OUTPUT);
   pinMode(in1, OUTPUT);
   pinMode(in2, OUTPUT);
   pinMode(TRIG_PIN, OUTPUT);
   pinMode(ECHO_PIN, INPUT);
   pinMode(TRIG1_PIN, OUTPUT);
   pinMode(ECHO1_PIN, INPUT);
   pinMode(enB,OUTPUT);
   pinMode(in3, OUTPUT);
   pinMode(in4, OUTPUT);
}

void loop(){
//  if (radio.available()){
//    while (radio.available()){
//      radio.read(&msg, sizeof(msg));
//      Serial.println(msg[0]);
//      if (msg[0]==0) {
//        digitalWrite(in1, LOW);
//        digitalWrite(in2, LOW);
//        analogWrite(enA, 200);
//      }
//      if (msg[0]==1) {
//        digitalWrite(in1, LOW);
//        digitalWrite(in2, HIGH);
//        analogWrite(enA, 200);
//      }
//
//      if (msg[0]==2) {
//        digitalWrite(in1, HIGH);
//        digitalWrite(in2, LOW);
//        analogWrite(enA, 200);
//      }
//    }
//  }
  long distance = GetDistance();
  long distance1 = GetDistance1();
  if ((distance >= 10) && (distance1 <=3)) {
    if (radio.available()) {
    while (radio.available()){
      radio.read(&msg, msg[0]);
      Serial.println(msg[0]);
      if (msg[0]==0) {
        //Running motor A
        digitalWrite(in1, HIGH);
        digitalWrite(in2, LOW);
        analogWrite(enA,200);

        //Speed PWM: 0~255
        //Running motor B
        digitalWrite(in3, HIGH);
        digitalWrite(in4, LOW);
        analogWrite(enB, 255);

      }
      if (msg[0]==1) {
        //Running motor A
        digitalWrite(in1, HIGH);
        digitalWrite(in2, LOW);
        analogWrite(enA,200);

        //Speed PWM: 0~255
        //Running motor B
        digitalWrite(in3, LOW);
        digitalWrite(in4, HIGH);
        analogWrite(enB, 255);

      }

      if (msg[0]==2) {
        //Running motor A
        digitalWrite(in1, LOW);
        digitalWrite(in2, HIGH);
        analogWrite(enA,200);

        //Speed PWM: 0~255
        //Running motor B
        digitalWrite(in3, HIGH);
        digitalWrite(in4, LOW);
        analogWrite(enB, 255);

      }
    }
    }
  }
  Serial.println(distance);
  Serial.print(distance1);
  delay(50);
}