#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 trigPin 10
#define echoPin 9


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

int valorMotor_PWM;
float distancia;
float erro,integral,der,erro_anterior,PID;
float Kp,Ki,Kd;

void setup() {
   //inicializa e configura pinos usados shield de controle do motor
   inicializa_Motor_Shield();
   //inicializa e configura pinos usados no sensor ultrassonico
   pinMode(trigPin, OUTPUT);
   pinMode(echoPin, INPUT);
   //inicializa e configura comunicação serial
   Serial.begin(9600);
   //inicializa as variaveis do sensor ultrassonico
   distancia = le_sensor_ultrassonico();
   //inicializa as variaveis dos motores
   valorMotor_PWM = 0;
   ci74HC595Write(bitMotor1A, HIGH);
   ci74HC595Write(bitMotor1B, LOW);
   ci74HC595Write(bitMotor2A, HIGH);
   ci74HC595Write(bitMotor2B, LOW);
   //inicializa as variaveis do PID
   erro = 0;
   integral = 0;
   der = 0;
   erro_anterior = 0;
   Kp = 30.0;
   Kd = 0.0;
   Ki = 0.0;
   Serial.println("Esperando 1 segundos");
   delay(1000);
   Serial.println("Setup completo");
}


void loop() {
  // LÊ OS SENSORES -----------------------------
  //Lê o senor de distancia ultrassonico
  distancia = le_sensor_ultrassonico();
  Serial.print(" Dist=");
  Serial.print(distancia);
  Serial.print("cm ");
// fim de LÊ OS SENSORES ----------------------  

// CALCULA O CONTROLE  ------------------------ 
  // NESTE controle queremos para a 5 cm do anteparo
  erro = distancia - 5.0; 
  integral = integral + erro;
  der = erro - erro_anterior;
  erro_anterior = erro;
  //calcula o valor do PID e a velocidade dos motores
  PID = Kp*erro + Ki*integral + Kd*der;
  valorMotor_PWM =(int)(PID); 
// fim de CALCULA O CONTROLE  ------------------------ 

// ATUALIZA VALOR DOS ATUADORES  --------------------- 
  //verifica saturacao dos motores
  if(valorMotor_PWM > 0){
    ci74HC595Write(bitMotor1A, HIGH);
    ci74HC595Write(bitMotor1B, LOW);
    ci74HC595Write(bitMotor2A, HIGH);
    ci74HC595Write(bitMotor2B, LOW); 
    Serial.print("  FRENTE,  PWM= ");
  } 
  if(valorMotor_PWM < 0){
    ci74HC595Write(bitMotor1A, LOW);
    ci74HC595Write(bitMotor1B, HIGH);
    ci74HC595Write(bitMotor2A, LOW);
    ci74HC595Write(bitMotor2B, HIGH);
    valorMotor_PWM =-valorMotor_PWM;
    Serial.print("  RE,  PWM= ");
  }
  if(valorMotor_PWM > 255){valorMotor_PWM =255;}
  //atualiza a velocidade dos motores
  analogWrite(pinMotor1PWM, valorMotor_PWM);
  analogWrite(pinMotor2PWM, valorMotor_PWM);
 
  Serial.println(valorMotor_PWM);
// fim de ATUALIZA VALOR DOS ATUADORES  ---------------------  
  delay(10);   
}





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 inicializa_Motor_Shield(){
     pinMode(pinSH_CP, OUTPUT);
     pinMode(pinST_CP, OUTPUT);
     pinMode(pinEnable, OUTPUT);
     pinMode(pinDS, OUTPUT);

     pinMode(pinMotor1PWM, OUTPUT);
     pinMode(pinMotor2PWM, OUTPUT);
//     pinMode(pinMotor3PWM, OUTPUT);
//     pinMode(pinMotor4PWM, OUTPUT);
     digitalWrite(pinEnable, LOW); 
}
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
}