#include <FastLED.h>

#define echoPin1   2
#define trigPin1   3
#define echoPin2   4
#define trigPin2   5

#define IN1 7
#define IN2 6

#define DATA_PIN    9
#define NUM_LEDS    30
#define BRIGHTNESS  50
#define LED_TYPE    WS2812B
#define COLOR_ORDER GRB


CRGB leds[NUM_LEDS];
#define UPDATES_PER_SECOND 100

long duration1;
long duration2;
int distance1;
int distance2;

//int LPWM = 7; //   LPWM connect to pin 5 on arduino
//int RPWM = 6; // RPWM connect to pin 6 on arduino
int timeA = 5000;    // how long do you want to run your motor?

void setup()
{
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  // pinMode(LPWM, OUTPUT);
  // pinMode(RPWM, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 0);

  Serial.begin(9600);

  FastLED.addLeds<LED_TYPE, DATA_PIN, COLOR_ORDER>(leds, NUM_LEDS);
  FastLED.setBrightness(BRIGHTNESS);
}

void loop()
{
  US1();
  US2();

  if (  distance1 < 10 ) {
    digitalWrite(IN1, 0);
    digitalWrite(IN2, 1);
    //delay(timeA);
    //digitalWrite(RPWM, LOW);
    //digitalWrite(LPWM, HIGH);
  }
  else {
    digitalWrite(IN1, 1);
    digitalWrite(IN2, 0);
    //delay(timeA);
    //digitalWrite(RPWM, HIGH);  //move forward and backward
    //digitalWrite(LPWM, LOW);
  }
  if (  distance2 < 10 ) {

    for (int i = 0; i < NUM_LEDS; i++) {
      leds[i] = CRGB::Red;
      FastLED.show();
    }
  }
  else {
    for (int i = 0; i < NUM_LEDS; i++) {
      leds[i] = CRGB::Green;
      FastLED.show();
    }
  }
}


void US1() {
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);
  duration1 = pulseIn(echoPin1, HIGH);
  distance1 = duration1 * 0.034 / 2;
  Serial.print("Distance: ");
  Serial.print(distance1);
  Serial.println(" cm");
}

void US2() {
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  duration2 = pulseIn(echoPin2, HIGH);
  distance2 = duration2 * 0.034 / 2;
  Serial.print("Distance: ");
  Serial.print(distance2);
  Serial.println(" cm");
}