// Algoritmo de controle

//Importacao de bibliotecas

#include // Biblioteca do sensor
#include // Biblioteca do servo


//Pinos usados
// Ponte H
#define pot1Pin 9 //potencia motor1
#define dir1Pin 13 //sentido motor1
#define pot2Pin 10 //potencia motor2
#define dir2Pin 8 //sentido motor2

//Servo
#define servoPin 3 // Servo motor
Servo servo;

//Sensor ultrasonico

#define echoPin 5 // echo - comunicacao com o sensor
#define trigPin 6 // trigger - gatilho para ativar o sensor
#define vccPin 7 // vcc -Alimentacao do sensor
Ultrasonic ultrasonic(trigPin,echoPin);     //Instacia o objeto que controla o sensor


//Variaveis

char buffer[18];
int cordx,cordy,cordz;

void setup() {
 // Configuracao dos pinos (entrada e saida)

 pinMode(dir1Pin,OUTPUT);
 pinMode(dir2Pin,OUTPUT);

 pinMode(echoPin,INPUT);
 pinMode(trigPin,OUTPUT);
 pinMode(vccPin,OUTPUT);

 //Sensor ultrassonico
 digitalWrite(vccPin,HIGH);    // Define sinal alto para servir como alimentacao

 //Definimos um sinal alto por 10ms para ativar o sensor
 digitalWrite(trigPin, HIGH);
 delayMicroseconds(10);
 digitalWrite(trigPin, LOW);

 //Servo
 servo.attach(servoPin);     // Instancia o objeto servo
 servo.write(90);     // Posiciona o servo a 90º


//Comunicacao serial
Serial.begin(57600);     // Inicia a comunicacao serial com 57600 pulsos por segundo
}


//Criamos as funcões que definem os movimentos do robô
//void movimento(){
// digitalWrite(pino,ESTADO); Definimos o estado do motor em HIGH(frente) ou LOW(tras)
// analogWrite(pino,POTENCIA); Definimos velocidade do motor(0-255) usando PWM.


void frente(){
 digitalWrite(dir1Pin,HIGH);
 analogWrite(pot1Pin,0);
}
void tras(){
 digitalWrite(dir1Pin,LOW);
 analogWrite(pot1Pin,255);
}
void parar(){
 digitalWrite(dir1Pin,HIGH);
 analogWrite(pot1Pin,255);
}
void direita(){
 digitalWrite(dir2Pin,HIGH);
 analogWrite(pot2Pin,0);
}
void esquerda(){
 digitalWrite(dir2Pin,LOW);
 analogWrite(pot2Pin,255);
}
void centro(){
 digitalWrite(dir2Pin,LOW);
 analogWrite(pot2Pin,0);
}

//Funcao que retorna a distancia do robo ate o obstaculo
int sensor() {
 delay(30);
 int d = ultrasonic.Ranging(CM);    // Faz a medicao em centimetros
 return d;
}

//Funcao que faz com que o sensor se vire em determinado agulo e retorne o valor da distancia.
int visao(int angulo){
 servo.write(angulo);
 delay(500);
 int distancia = sensor();
 Serial.println(distancia);
 return distancia;
}

// Funcao que separa as coordenas(x,y,x)
void dividir(char* data){
 char * parameter;
 parameter = strtok(data, " ,");
 while(parameter != NULL){
  motor(parameter);
  parameter = strtok(NULL, " ,");
 }
 for(int x=0; x < 16; x++){
  buffer[x]='{TEXTO}';
 }
 Serial.flush();
}

// Funcao que chama as funcoes de acordo com as coordenadas: X=Direcao,Y=Sentido.Z=Posicao do servo

void motor(char* data){
 if ((data[0] == 'x') || (data[0] == 'X')){     // Se o determinante for igual a x ou X
  cordx = strtol(data+1, NULL, 10);     // Coordenada de x recebe o valor que acompanha o detereminante (x255 cordx = 255)
 }
 else if ((data[0] == 'y') || (data[0] == 'Y')){
  cordy = strtol(data+1, NULL, 10);
 }
 else if ((data[0] == 'z') || (data[0] == 'z')){
  cordz = strtol(data+1, NULL, 10);
  visao(cordz);     //Chama a funcao visao e passa a posicao do servo
 }
 if(cordy > 0){
  frente();
 }else if(cordy < 0){     // Coordenada de y > 0 va para frente
  tras();     // Coodenada de y < 0 va para tras
 }else if(cordy == 0){
  parar();     // Coordenada de y = 0 fique parado
 }
 if(cordx > 0 ){
  esquerda();
 }else if(cordx < 0){
  direita();
 }else if(cordx == 0){
  centro();
 }

}

// O loop e onde o programa roda chamando as funcoes

void loop() {
 if(Serial.available() > 0){     //Se houver dados
  int index=0;
  delay(100);
  int numChar = Serial.available();     //Tamanho do buffer
  while (numChar--){     // Enquanto o buffer e maior que zero
   buffer[index++] = Serial.read();     // Buffer recebe caracetere
  }
  dividir(buffer);
  Serial.flush();
 }
}