/*

Codigo fonte do Robo Braddock

Este codigo implementa o controle das seguintes funcionalidades:

- Acionamento dos 4 leds dianteiros (FAROL_DIANTEIRO)

- Acionamento dos 2 leds traseiros (FAROL_TRASEIRO)
- Acionamento dos 2 leds vermelhos (FREIO)
- Buzina de alerta (Buzina)
- Controle de PAN e TILT para a cmera
- Controle de PWM dos motores

Desenvolvimento: William Cruz de Oliveira

Criacao: 21 Abril 2017

*/

// BIBLIOTECAS
#include <Servo.h> //Servo Driver

//DEFINICOES PARA O SERVO

Servo pan, tilt; // Nomes dos Servos
int panMIN = 0; // Valor minimo para o movimento PAN
int panMAX = 180; // Valor maximo para o movimento PAN
int tiltMIN = 90; // Valor minimo para o movimento TILT
int tiltMAX = 180; // Valor maximo para o movimento TILT
int contPAN = 90; // Valor inicial para PAN
int contTILT = 90; // Valor inicial para TILT

// DEFINICOES DOS MOTORES

#define BRAKE 0
#define CW 1
#define CCW 2
#define ESQUERDA 3
#define DIREITA 4
#define FRENTE 5
#define TRAS 6
#define PARADO 7

// DEFINICAO PARA CONTROLE DA CORRENTE NOS MOTORES

#define CS_THRESHOLD 15

//MOTOR 1
#define MOTOR_A1_PIN 7
#define MOTOR_B1_PIN 8

//MOTOR 2
#define MOTOR_A2_PIN 4
#define MOTOR_B2_PIN 9

//PWM DOS MOTORES

#define PWM_MOTOR_1 5
#define PWM_MOTOR_2 6

//CONTROLE DAS CORRENTES

#define CURRENT_SEN_1 A2
#define CURRENT_SEN_2 A3

//HABILITAR OS MOTORES
#define EN_PIN_1 A0
#define EN_PIN_2 A1

// NUMERANDO OS MOTORES
#define MOTOR_1 0
#define MOTOR_2 1

short usSpeed = 180; //VELOCIDADE PADRAO DOS MOTORES

unsigned short usMotor_Status_M1 = BRAKE; //MOTOR 1 INICIA PARADO
unsigned short usMotor_Status_M2 = BRAKE; //MOTOR 1 INICIA PARADO
unsigned short sentido; //SENTIDO DO MOVIMENTO DO ROBO

//BLUETOOTH
char buffer[67];

//DEFINICAO DOS NOMES DAS PORTAS PARA FAROL E LASER

# define FAROL_DIANTEIRO A4
# define FAROL_TRASEIRO A5
# define FREIO 12
# define BUZZER 3 // MUDAR PARA LASER

int frequencia = 0;

// INICIALIZANDO O PROGRAMA
void setup()
{
//CONFIGURACAO DA PORTA SERIAL
Serial.begin(9600);
Serial.flush();

// DEFINICAO DAS PORTAS PARA CONTROLE DOS MOTORES

 pinMode(MOTOR_A1_PIN, OUTPUT);
pinMode(MOTOR_B1_PIN, OUTPUT);

pinMode(MOTOR_A2_PIN, OUTPUT);
pinMode(MOTOR_B2_PIN, OUTPUT);

pinMode(PWM_MOTOR_1, OUTPUT);
pinMode(PWM_MOTOR_2, OUTPUT);

pinMode(CURRENT_SEN_1, OUTPUT);
pinMode(CURRENT_SEN_2, OUTPUT);

pinMode(EN_PIN_1, OUTPUT);
pinMode(EN_PIN_2, OUTPUT);

pinMode(FAROL_DIANTEIRO, OUTPUT);
pinMode(FAROL_TRASEIRO, OUTPUT);
pinMode(FREIO, OUTPUT);
pinMode(BUZZER, OUTPUT);

sentido = PARADO;

// CHECANDO O SISTEMA
test_farol_dianteiro();
test_farol_traseiro();
test_freio();
delay(50);

// POSICIONANDO O PAN TILT

pan.attach(10); //DEFINICAO DA PORTA 10 PARA PAN
tilt.attach(11); //DEFINICAO DA PORTA 11 PARA TILT
pan.write(90);
delay(50);
tilt.write(90);
delay(50);

// SINAL PARA LIBERAR O ROBO

test_buzzer();
}

void loop()
{
if (Serial.available()>0)
{
int index=0;
 // AGUARDA 80ms PARA ENCHER O BUFFER
delay(80);

// LE O VALOR ENVIADO NA COMUN, SERIAL (BLUETOOTH)

int numChar = Serial.available();

// NAO PERMITE QUE A QUANTIADADE DIGITADA ESTOURE O BUFFFER

if(numChar>65)
{
numChar=65;
}

// PREENCHE O VETOR BUFFER

while(numChar--)
{
buffer[index++] = Serial.read();
}

// CHAMA FUNCAO PARA REALIZAR A ACAO

splitString(buffer);

// HABILITA OS DOIS MOTORES PARA RODAR

digitalWrite(EN_PIN_1, HIGH);
digitalWrite(EN_PIN_2, HIGH);
}
}

// FUNCAO PARA VERIFICAR OS FAROIS DIANTEIROS

void test_farol_dianteiro()
{
for (int x=0; x<=8; x++)
{
digitalWrite(FAROL_DIANTEIRO, HIGH);
delay(50);
digitalWrite(FAROL_DIANTEIRO, LOW);
delay(50);
}
}

//FUNCAO PARA VERIFICAR OS FAROIS TRASEIROS

void test_farol_traseiro()
{
for (int x=0; x<=8; x++)
{
digitalWrite(FAROL_TRASEIRO, HIGH);
 delay(50);
digitalWrite(FAROL_TRASEIRO, LOW);
delay(50);
}
}

// FUNCAO PARA VERIFICAR AS LANTERNAS DE FREIO

void test_freio()
{
for (int x=0; x<=8; x++)
{
digitalWrite(FREIO, HIGH);
delay(50);
digitalWrite(FREIO, LOW);
delay(50);
}
}

// FUNCAO ACIONA BUZZER

void test_buzzer()
{
for (int x=0; x<=2; x++)
{
tone(BUZZER,1500);
delay(100);
noTone(BUZZER);
delay(100);
}
}

/* FUNCAO QUE LE OS DADOS ENVIADOS PELO BLUETOOTH

ORGANIZA E ENVIA PARA A FUNCAO DO ROBO.
APAGANDO OS DADOS EM SEGUIDA.
*/
void splitString(char* data)
{
char* parameter;
parameter= strtok (data, " ,");
while(parameter != NULL)
{
RoboAction(parameter);
parameter = strtok (NULL, " ,");
}

//LIMPA O TEXTO E OS BUFFER SERIAIS

for (int x=0; x<66; x++)
 {
buffer[x]='\0';
}
Serial.flush();
}

// FUNCAO DE COMANDO PARA O ROBO

void RoboAction(char* data)
{
// ROBO FRENTE + FAROL DIANTEIRO
if ((data[0] =='j') || (data[0] == 'J'))
{
analogWrite(FAROL_DIANTEIRO, 255);
analogWrite(FAROL_TRASEIRO, 0);
analogWrite(FREIO, 0);
sentido = FRENTE;
usMotor_Status_M1 = CW;
usMotor_Status_M2 = CCW;
motorGo(MOTOR_1,usMotor_Status_M1, usSpeed);
motorGo(MOTOR_2,usMotor_Status_M2, usSpeed);
}

// ROBO RE + FAROL TRASEIRO

if ((data[0] =='k') || (data[0] == 'K'))
{
analogWrite(FAROL_DIANTEIRO, 0);
analogWrite(FAROL_TRASEIRO, 255);
analogWrite(FREIO, 0);
sentido = TRAS;
usMotor_Status_M1 = CCW;
usMotor_Status_M2 = CW;
motorGo(MOTOR_1,usMotor_Status_M1, usSpeed);
motorGo(MOTOR_2,usMotor_Status_M2, usSpeed);
}

// ROBO ESQUERDA
if ((data[0] =='g') || (data[0] == 'G'))
{
if (sentido == FRENTE)
{
usMotor_Status_M1 = BRAKE;
usMotor_Status_M2 = CCW;
motorGo(MOTOR_1,usMotor_Status_M1, usSpeed);
motorGo(MOTOR_2,usMotor_Status_M2, usSpeed);
}
else if (sentido == TRAS)
 {
usMotor_Status_M1 = BRAKE;
usMotor_Status_M2 = CW;
motorGo(MOTOR_1,usMotor_Status_M1, usSpeed);
motorGo(MOTOR_2,usMotor_Status_M2, usSpeed);
}
}

// ROBO DIREITA
if ((data[0] =='h') || (data[0] == 'H'))
{
if (sentido == FRENTE)
{
usMotor_Status_M1 = CW;
usMotor_Status_M2 = BRAKE;
motorGo(MOTOR_1,usMotor_Status_M1, usSpeed);
motorGo(MOTOR_2,usMotor_Status_M2, usSpeed);
}
else if (sentido == TRAS)
{
usMotor_Status_M1 = CCW;
usMotor_Status_M2 = BRAKE;
motorGo(MOTOR_1,usMotor_Status_M1, usSpeed);
motorGo(MOTOR_2,usMotor_Status_M2, usSpeed);
}
}

// ROBO STOP + LUZ DE FREIO

if ((data[0] =='i') || (data[0] == 'I'))
{
analogWrite(FAROL_DIANTEIRO, 0);
analogWrite(FAROL_TRASEIRO, 0);
analogWrite(FREIO, 255);
sentido = PARADO;
usMotor_Status_M1 = BRAKE;
usMotor_Status_M2 = BRAKE;
motorGo(MOTOR_1,usMotor_Status_M1, 0);
motorGo(MOTOR_2,usMotor_Status_M2, 0);
}

// INCREMENTAR VELOCIDADE
if ((data[0] =='v') || (data[0] == 'V'))
{
usSpeed = usSpeed + 10;

if(usSpeed > 180)

 {
usSpeed = 180;
}
motorGo(MOTOR_1, usMotor_Status_M1, usSpeed);
motorGo(MOTOR_2, usMotor_Status_M2, usSpeed);
}

// DECREMENTAR VELOCIDADE
if ((data[0] =='x') || (data[0] == 'X'))
{
usSpeed = usSpeed - 10;

if(usSpeed < 50)

{
usSpeed = 50;
}
motorGo(MOTOR_1, usMotor_Status_M1, usSpeed);
motorGo(MOTOR_2, usMotor_Status_M2, usSpeed);
}

// BUZZER
if ((data[0] =='d') || (data[0] == 'D'))
{
int Ans = strtol(data+1, NULL, 10);
Ans = constrain(Ans,0,1);
if (Ans == 1)
{
tone(BUZZER,1500);
}
else
{
noTone(BUZZER);
}
}

// // CONTROLE PAN - ESQUERDA

// if ((data[0] =='p') || (data[0] == 'P'))
// {
// while ((data[0] == 'p') || (data[0] == 'P'))
// {
// if (contPAN <= panMAX)
// {
// contPAN = contPAN + 1;
// pan.write(contPAN);
// delay(15);
// }
// }
// }

// // CONTROLE PAN - DIREITA

// if ((data[0] =='q') || (data[0] == 'Q'))
// {
// while ((data[0] == 'q') || (data[0] == 'Q'))
// {
// if (contPAN >= panMIN)
// {
// contPAN = contPAN - 1;
// pan.write(contPAN);
// delay(15);
// }
// }
// }

// // CONTROLE TILT - CIMA

// if ((data[0] =='r') || (data[0] == 'R'))
// {
// while ((data[0] == 'r') || (data[0] == 'R'))
// {
// if (contTILT <= tiltMAX)
// {
// contTILT = contTILT + 1;
// tilt.write(contTILT);
// delay(15);
// }
// }
// }

// // CONTROLE TILT - BAIXO

// if ((data[0] =='s') || (data[0] == 'S'))
// {
// while ((data[0] == 's') || (data[0] == 'S'))
// {
// if (contTILT >= tiltMIN)
// {
// contTILT = contTILT - 1;
// tilt.write(contTILT);
// delay(15);
// }
// }
// }
 // CONTROLE PAN - ESQUERDA
if ((data[0] =='p') || (data[0] == 'P'))
{
if (contPAN <= panMAX)
{
contPAN = contPAN + 1;
pan.write(contPAN);
delay(15);
}
}

// CONTROLE PAN - DIREITA

if ((data[0] =='q') || (data[0] == 'Q'))
{
if (contPAN >= panMIN)
{
contPAN = contPAN - 1;
pan.write(contPAN);
delay(15);
}
}

// CONTROLE TILT - CIMA

if ((data[0] =='r') || (data[0] == 'R'))
{
if (contTILT <= tiltMAX)
{
contTILT = contTILT + 1;
tilt.write(contTILT);
delay(15);
}
}

// CONTROLE TILT - BAIXO

if ((data[0] =='s') || (data[0] == 'S'))
{
if (contTILT >= tiltMIN)
{
contTILT = contTILT - 1;
tilt.write(contTILT);
delay(15);
}
}
}

//FUNCAO QUE CONTROLA AS VARIAVEIS: motor(0 ou 1), direcao (cw ou ccw) e pwm (entre
0 e 255);
void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm)
{
if(motor == MOTOR_1)
{
if(direct == CW)
{
digitalWrite(MOTOR_A1_PIN, HIGH);
digitalWrite(MOTOR_B1_PIN, LOW);
}
else if(direct == CCW)
{
digitalWrite(MOTOR_A1_PIN, LOW);
digitalWrite(MOTOR_B1_PIN, HIGH);
}
else
{
digitalWrite(MOTOR_A1_PIN, LOW);
digitalWrite(MOTOR_B1_PIN, LOW);
}

analogWrite(PWM_MOTOR_1, pwm);
}
else if(motor == MOTOR_2)
{
if(direct == CW)
{
digitalWrite(MOTOR_A2_PIN, HIGH);
digitalWrite(MOTOR_B2_PIN, LOW);
}
else if(direct == CCW)
{
digitalWrite(MOTOR_A2_PIN, LOW);
digitalWrite(MOTOR_B2_PIN, HIGH);
}
else
{
digitalWrite(MOTOR_A2_PIN, LOW);
digitalWrite(MOTOR_B2_PIN, LOW);
}

analogWrite(PWM_MOTOR_2, pwm);
}
}