#include <HCSR04.h>

// https://github.com/gamegine/HCSR04-ultrasonic-sensor-lib

const int pintrigger = 9;
const int pinecho = 10;
const int pinred = 6;
const int pingreen = 5;

HCSR04 hc(9, 10); //initialisation class HCSR04 (trig pin , echo pin)

void setup()
{
  Serial.begin(9600);
  pinMode(pinred, OUTPUT);
  pinMode(pingreen, OUTPUT);

}

void loop()
{
  if (hc.dist() > 0)
  {
    color(1, 1);
    delay(hc.dist()*5);
    color(0, 1);
    delay(hc.dist()*5);
    Serial.print("Vehiculo a ");
    Serial.print(hc.dist());
    Serial.print(" cms");
    Serial.println();
  }
  else
  {
    color(2, 1);
  }

}

void color(int colour, int comun) //colour: 0= off 1= red 2= green 3= blue| comun: 0= catodo 1= anodo
{
  if (colour == 0)
  {
    if (comun == 0)
    {
      digitalWrite(pinred, LOW);
      digitalWrite(pingreen, LOW);
    }
    else
    {
      digitalWrite(pinred, HIGH);
      digitalWrite(pingreen, HIGH);
    }
  }

  if ((colour == 1 and comun == 0) or (colour == 2 and comun == 1) )
  {
    digitalWrite(pinred, HIGH);
    digitalWrite(pingreen, LOW);
  }
  if ((colour == 2 and comun == 0) or (colour == 1 and comun == 1) )
  {
    digitalWrite(pinred, LOW);
    digitalWrite(pingreen, HIGH);
  }


}