#define bitMotor1A 2
#define bitMotor1B 3
#define bitMotor2A 1
#define bitMotor2B 4
#define bitMotor3A 5
#define bitMotor3B 7
#define bitMotor4A 0
#define bitMotor4B 6

#define pinSH_CP  4   //Pino Clock  DIR_CLK
#define pinST_CP  12  //Pino Latch  DIR_LATCH
#define pinDS     8   //Pino Data   DIR_SER
#define pinEnable 7   //Pino Enable DIR_EN

#define pinMotor1PWM 11
#define pinMotor2PWM 3
//#define pinMotor3PWM 5
//#define pinMotor4PWM 6

#define PinLineSensor1 A0
#define PinLineSensor2 A1
#define trigPin 10
#define echoPin 9


void ci74HC595Write(byte pino, bool estado);
float le_sensor_ultrassonico();

int valorSensorLinha1;
int valorMotor_1_PWM;
float distancia;

void setup() {
   pinMode(pinSH_CP, OUTPUT);
   pinMode(pinST_CP, OUTPUT);
   pinMode(pinEnable, OUTPUT);
   pinMode(pinDS, OUTPUT);

   pinMode(pinMotor1PWM, OUTPUT);
   digitalWrite(pinEnable, LOW);

   pinMode(PinLineSensor1, INPUT);

   pinMode(trigPin, OUTPUT);
   pinMode(echoPin, INPUT);
   
   Serial.begin(9600);

   valorSensorLinha1 = 0;
   valorMotor_1_PWM = 0;
   ci74HC595Write(bitMotor1A, HIGH);
   ci74HC595Write(bitMotor1B, LOW);
}

void loop() {
  distancia = le_sensor_ultrassonico();
  
  //map(value, fromLow, fromHigh, toLow, toHigh)
  //   value: the number to map.
  //   fromLow: the lower bound of the value’s current range.
  //   fromHigh: the upper bound of the value’s current range.
  //   toLow: the lower bound of the value’s target range.
  //   toHigh: the upper bound of the value’s target range.
  valorMotor_1_PWM = map(distancia, 0, 50, 0 , 255);
  
  if(valorMotor_1_PWM < 0){valorMotor_1_PWM =0;}
  if(valorMotor_1_PWM > 255){valorMotor_1_PWM =255;}
  
  analogWrite(pinMotor1PWM, valorMotor_1_PWM);
  
  
  Serial.print(0);
  Serial.print("  ");
  Serial.print(distancia);
  Serial.print("  ");
  Serial.println(60);
  delay(5);
  
}

float le_sensor_ultrassonico() {
  long duracao;
  //Limpa o pino de Trigger
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  //Coloca o pino de Trigger em HIGH por 10 microseg 
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  //Le o pino de echo, retornando o tempo em microseg da onda sonora ir e vir
  duracao = pulseIn(echoPin, HIGH);
  //Calcula a distancia
  return ((float)duracao)*0.0174;
} 



void ci74HC595Write(byte pino, bool estado) {
  static byte ciBuffer;
  bitWrite(ciBuffer, pino , estado);
  
  digitalWrite(pinST_CP, LOW); //Inicia a Transmissão
  digitalWrite(pinDS, LOW);    //Apaga Tudo para Preparar Transmissão
  digitalWrite(pinSH_CP, LOW);
  for (int nB = 7; nB >= 0; nB--) {
      digitalWrite(pinSH_CP, LOW);  //Baixa o Clock      
      digitalWrite(pinDS,  bitRead(ciBuffer, nB) );     //Escreve o BIT
      digitalWrite(pinSH_CP, HIGH); //Eleva o Clock
      digitalWrite(pinDS, LOW);     //Baixa o Data para Previnir Vazamento      
  }  
  digitalWrite(pinST_CP, HIGH);  //Finaliza a Transmissão
}