#define bitMotor1A 2
#define bitMotor1B 3
#define bitMotor2A 1
#define bitMotor2B 4
#define pinMotor1PWM 11
#define pinMotor2PWM 3

#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 sensorDireita A0
#define sensorEsquerda A1
#define desvio 80
#define V0 100
#define V_max 250
#define V_min 0

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

float valorMotor_PWM_Esq,valorMotor_PWM_Dir;
int leituraEsquerda, leituraDireita;
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 nos sensores de linha
   pinMode(sensorEsquerda, INPUT);
   pinMode(sensorDireita, INPUT); 
//inicializa e configura comunicação serial
   Serial.begin(9600);
//inicializa as variaveis dos motores
   valorMotor_PWM_Esq = V0;
   valorMotor_PWM_Dir = 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 = 1.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 linha
  leituraDireita = analogRead(sensorDireita)-desvio;
  leituraEsquerda = analogRead(sensorEsquerda)-desvio;
  Serial.print("S_Esq= ");
  Serial.print(leituraEsquerda);
  Serial.print(" S_Dir= ");
  Serial.print(leituraDireita);
// fim de LÊ OS SENSORES ----------------------
  

// CALCULA O CONTROLE  ------------------------ 
  // NESTE controle, vamos assumir 
  // que virar para a esquerda é positivo
  //calcula os termos do PID
  erro = leituraDireita - leituraEsquerda; 
  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_Esq =(int)(V0 + PID);
  valorMotor_PWM_Dir =(int)(V0 - PID);  
// fim de CALCULA O CONTROLE  ------------------------ 
  
  
// ATUALIZA VALOR DOS ATUADORES  --------------------- 
  //verifica saturacao dos motores
  if (valorMotor_PWM_Esq>V_max){
    valorMotor_PWM_Esq = V_max;
  }if (valorMotor_PWM_Esq<V_min){
    valorMotor_PWM_Esq = V_min;
  }if (valorMotor_PWM_Dir>V_max){
    valorMotor_PWM_Dir = V_max;
  }if (valorMotor_PWM_Dir<V_min){
    valorMotor_PWM_Dir = V_min;
  }
  //atualiza a velocidade dos motores
  analogWrite(pinMotor1PWM,int(valorMotor_PWM_Esq)); //MOTOR E PWM
  analogWrite(pinMotor2PWM,int(valorMotor_PWM_Dir)); //MOTOR D PWM
  Serial.print("  M_Esq= ");
  Serial.print(valorMotor_PWM_Esq);
  Serial.print(" M_Dir= ");
  Serial.println(valorMotor_PWM_Dir);
// fim de ATUALIZA VALOR DOS ATUADORES  ---------------------  
  delay(50);
}



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
}