// Definicoes #define LEFT_SENSOR 5 #define RIGHT_SENSOR 6
#define LEFT_MOTOR 3 #define RIGHT_MOTOR 1
#define LINHA 1 #define CHAO 0
// Estados #define INICIAL 0 #define ESQ_LINHA 1 #define DIR_LINHA 2 #define AMBOS_LINHA 3 #define NENHUM_LINHA 4
// Variaveis persistentes // Representam as cores dos blocos persistent int L_LINE_SETPOINT; persistent int L_FLOOR_SETPOINT; persistent int R_LINE_SETPOINT; persistent int R_FLOOR_SETPOINT; persistent int R_DIF; persistent int L_DIF;
persistent int LEFT_POWER; persistent int RIGHT_POWER;
// Estados dos sensores int sensor_esq; int sensor_dir;
void CalibrarLinha(){
// Variaveis auxiliares int i;
// Calibra o sensor esquerdo
// Espera para ser colocado na linha preta
printf ("\nSensor Esquerdo Linha preta");
while (!start_button());
printf ("\nCalibrando..");
L_LINE_SETPOINT = 0;
for (i=0; i<50; i++){
L_LINE_SETPOINT = L_LINE_SETPOINT + analog(LEFT_SENSOR);
sleep(0.05);
}
L_LINE_SETPOINT = (int) L_LINE_SETPOINT / 50;
printf ("\nLeft Line: %d", L_LINE_SETPOINT);
while (!start_button());
sleep(0.5);
beep();
// Espera para ser colocado no chao
printf ("\nSensor Esquerdo Chao");
while (!start_button());
printf ("\nCalibrando..");
L_FLOOR_SETPOINT = 0;
for (i=0; i<50; i++){
L_FLOOR_SETPOINT = L_FLOOR_SETPOINT + analog(LEFT_SENSOR);
sleep(0.05);
}
L_FLOOR_SETPOINT= L_FLOOR_SETPOINT/50;
printf ("\nLeft Floor: %d", L_FLOOR_SETPOINT);
while (!start_button());
sleep(0.5);
beep();
// Calibra o sensor direito
// Espera para ser colocado na linha preta
printf ("\nSensor Direito Linha preta");
while (!start_button());
printf ("\nCalibrando..");
R_LINE_SETPOINT = 0;
for (i=0; i<50; i++){
R_LINE_SETPOINT = R_LINE_SETPOINT + analog(RIGHT_SENSOR);
sleep(0.05);
}
R_LINE_SETPOINT = (int) R_LINE_SETPOINT / 50;
printf ("\nRight Line: %d", R_LINE_SETPOINT);
while (!start_button());
sleep(0.5);
beep();
// Espera para ser colocado no chao
printf ("\nSensor Direito Chao");
while (!start_button());
printf ("\nCalibrando..");
R_FLOOR_SETPOINT = 0;
for (i=0; i<50; i++){
R_FLOOR_SETPOINT = R_FLOOR_SETPOINT + analog(RIGHT_SENSOR);
sleep(0.05);
}
R_FLOOR_SETPOINT = R_FLOOR_SETPOINT/50;
printf ("\nRight Floor: %d", R_FLOOR_SETPOINT);
beep();
// Calcula as diferencas L_DIF = (L_LINE_SETPOINT - L_FLOOR_SETPOINT) / 2; R_DIF = (R_LINE_SETPOINT - R_FLOOR_SETPOINT) / 2; }
void SeguirLinha(){
int pid;
// Espera precionar start antes de comecar
sleep(0.5);
printf ("\nSeguir Linha");
while(!start_button());
sleep(0.5);
pid = start_process(Seguir());
while(!stop_button()); kill_process(pid); ao();
}
void Seguir(){
// Variaveis auxiliares int estado_anterior = INICIAL;
// Define as potencias iniciais LEFT_POWER = 15; RIGHT_POWER = 15;
// Segue a linha ate o botao stop ser precionado
while (1){
// Pega o estado dos sensores
GetPosicao();
// Se somente o sensor esq tah nah linha
if ((sensor_esq == LINHA) && (sensor_dir == CHAO)){
printf ("\n ESQUERDO LINHA");
// Vai para frente
AndarReto();
// Define o estado anterior
estado_anterior = ESQ_LINHA;
}
else{
// Se somente o sensor direito tah nah linha
if ((sensor_esq == CHAO) && (sensor_dir == LINHA)){
printf ("\n DIREITO LINHA");
// Vai para frente
AndarReto();
// Define o estado anterior
estado_anterior = DIR_LINHA;
}
else{
// Se ambos estao na linha
if ((sensor_esq == LINHA) && (sensor_dir == LINHA)){
printf ("\n AMBOS LINHA");
if (estado_anterior == DIR_LINHA){
while ((sensor_esq == LINHA) && (sensor_dir == LINHA) ){
// Vira para esquerda
VirarEsq();
// Pega os novos estados
GetPosicao();
}
}
else{
if (estado_anterior == ESQ_LINHA){
while ((sensor_esq == LINHA) && (sensor_dir == LINHA) ){
// Vira para direita
VirarDir();
// Pega os novos estados
GetPosicao();
}
}
else{
while ((sensor_esq == LINHA) && (sensor_dir == LINHA) ){
// Vira para direita
VirarDir();
// Pega os novos estados
GetPosicao();
}
}
}
estado_anterior = AMBOS_LINHA;
}
else{
printf ("\n NENHUM LINHA");
if (estado_anterior == DIR_LINHA){
// Vira para direita
VirarDir();
while ((sensor_esq == CHAO) && (sensor_dir == CHAO) ){
// Pega os novos estados
GetPosicao();
}
}
else{
if (estado_anterior == ESQ_LINHA){
// Vira para esquerda
VirarEsq();
while ((sensor_esq == CHAO) && (sensor_dir == CHAO) ){
// Pega os novos estados
GetPosicao();
}
}
else{
// Faz um espiral
motor (LEFT_MOTOR, LEFT_POWER*2);
motor (RIGHT_MOTOR, RIGHT_POWER);
while ((sensor_esq == CHAO) && (sensor_dir == CHAO) ){
// Pega os novos estados
GetPosicao();
}
}
}
estado_anterior = NENHUM_LINHA;
}
}
}
}
}
void PararMotores(){
motor (LEFT_MOTOR, 0);
motor (RIGHT_MOTOR, 0);
}
// Define os estados dos sensores
void GetPosicao(){
// Define o estado do sensor direito
if ((PosicaoDir()+(R_DIF/2)) > R_LINE_SETPOINT)
sensor_dir = LINHA;
else{
if ((PosicaoDir()-(R_DIF/2)) < R_FLOOR_SETPOINT)
sensor_dir = CHAO;
}
// Define o estado do sensor esquerdo
if ((PosicaoEsq()+(L_DIF/2)) > L_LINE_SETPOINT)
sensor_esq = LINHA;
else{
if ((PosicaoEsq()-(L_DIF/2)) < L_FLOOR_SETPOINT)
sensor_esq = CHAO;
}
}
// Calcula a posicao do sensor esquerdo
int PosicaoEsq(){
int pos;
pos = analog(LEFT_SENSOR);
return pos;
}
// Calcula a posicao do sensor direito
int PosicaoDir(){
int pos;
pos = analog(RIGHT_SENSOR);
return pos;
}
// Vira para direita
void VirarDir(){
motor (LEFT_MOTOR, (int)((float)LEFT_POWER*1.5));
motor (RIGHT_MOTOR, 0);
}
// Vira para esquerda
void VirarEsq(){
motor (LEFT_MOTOR, 0);
motor (RIGHT_MOTOR, (int)((float)RIGHT_POWER*1.5));
}
// Anda Retor
void AndarReto(){
motor (LEFT_MOTOR, LEFT_POWER);
motor (RIGHT_MOTOR, RIGHT_POWER);
}