import com.ridgesoft.intellibrain.

IntelliBrain;
import com.ridgesoft.io.Display;
import com.ridgesoft.robotics.Motor;
import com.ridgesoft.robotics.ContinuousRotationServo;
import com.ridgesoft.robotics.AnalogInput;
import com.ridgesoft.robotics.RangeFinder;
import com.ridgesoft.robotics.sensors.ParallaxPing;
import com.ridgesoft.intellibrain.IntelliBrainDigitalIO;
import com.ridgesoft.io.LED;
public class Bazzinga {
// Constantes
private static final int LIMITE_LINHA = 100;
// Valor a partir do qual se considera que ha chama
private static final int HA_CHAMA = 900;
private static final int PERTO_CHAMA = 300;
// Velocidades base para mover em frente e para rodar
private static final int VELO_BASE = 8;
private static final int VELO_RODAR = 5;
// Ganho para os algoritmos de seguir paredes
private static final float GANHO = 1.3f;
// Fator para converter graus em tempo
private static final float FATOR_RODAR = 10.5f;
private static final int DIST_BASE_E = 13;
private static final int DIST_BASE_F = 15;
private static final int DIST_BASE_D = 15;
private static final int ESPERAR = 0;
private static final int NAVEGAR_E = 1;
private static final int CENTRAR = 2;
private static final int APAGAR = 3;
private static final int PARAR = 4;
private static final int DETETAR = 5;
private static Display display;
private static Motor motorE;
private static Motor motorD;
private static Motor ventoinha;
private static AnalogInput sensorLinha;
private static AnalogInput SUVTron;
private static AnalogInput[] sensorChama;
private static int dirChama; // dirChama = 2 a do meio
private static int valChama;
private static RangeFinder sonarE;
private static RangeFinder sonarD;
private static RangeFinder sonarF;
private static IntelliBrainDigitalIO botaoVerde;
private static IntelliBrainDigitalIO botaoVermelho;
private static IntelliBrainDigitalIO led;
private static boolean estadolinha = false;
private static boolean linha = false;
private static IntelliBrainDigitalIO bumperE;
private static IntelliBrainDigitalIO bumperD;
public static void main(String[] args) {
// Estrutura da maquina de estados finitos
try {
// LCD
display = IntelliBrain.getLcdDisplay();
display.print(0, "Bazinga");
// Motores Rodas
motorE = new ContinuousRotationServo(IntelliBrain.getSer
vo(1),
false, 14);
motorD = new ContinuousRotationServo(IntelliBrain.getSer
vo(2),
true, 14);
// Motor Ventoinha
ventoinha = IntelliBrain.getMotor(2);
// Sensores Chama
sensorChama = new AnalogInput[5];
sensorChama[0] = IntelliBrain.getAnalogInput(1);
sensorChama[1] = IntelliBrain.getAnalogInput(2);
sensorChama[2] = IntelliBrain.getAnalogInput(3);
sensorChama[3] = IntelliBrain.getAnalogInput(4);
sensorChama[4] = IntelliBrain.getAnalogInput(5);
dirChama = -1;
valChama = 0;
// Sensor linha
sensorLinha = IntelliBrain.getAnalogInput(7);
boolean linha = false;
// Sensor SUVTron
SUVTron = IntelliBrain.getAnalogInput(6);
// Sensores Sonar
sonarE = new ParallaxPing(IntelliBrain.getDigitalIO(3));
sonarD = new ParallaxPing(IntelliBrain.getDigitalIO(5));
sonarF = new ParallaxPing(IntelliBrain.getDigitalIO(4));
int distE;
int distD;
int distF;
// Botoes
botaoVerde = IntelliBrain.getDigitalIO(12);
botaoVerde.setPullUp(true);
botaoVermelho = IntelliBrain.getDigitalIO(13);
botaoVermelho.setPullUp(true);
// LED (Sinal de Chama)
led = IntelliBrain.getDigitalIO(8);
led.setDirection(true);
// Bumpers
bumperE = IntelliBrain.getDigitalIO(10);
bumperE.setPullUp(true);
bumperD = IntelliBrain.getDigitalIO(11);
bumperD.setPullUp(true);
int estado = ESPERAR;
//display.print(0, "v1: " + sensorLinha.toString());
display.print(0, "v1: " +sensorChama[2].toString());
while (true) {
// leitura dos sensores
// linha = (sensorLinha.sample() < LIMITE_LINHA)
;
detetarChama();
sonarE.ping();
sonarD.ping();
Thread.sleep(30);
distE = (int) (sonarE.getDistanceCm() + 0.5f);
distD = (int) (sonarD.getDistanceCm() + 0.5f);
sonarF.ping();
Thread.sleep(30);
distF = (int) (sonarF.getDistanceCm() + 0.5f);
//
if (!botaoVermelho.isSet()) {
estado = ESPERAR;
}
//mostrarValoresEmLinha(dirChama, valChama);
mostrarEstado(estado);
switch (estado) {
case ESPERAR:
// Comportamento ESPERAR
// ***** Comportamento
parar();
while (botaoVerde.isSet()) {
;
}
mover(16, 0);
Thread.sleep(2000);
// Condies
estado = NAVEGAR_E;
break;
case NAVEGAR_E:
// Comportamento NAVEGAR_E
// Bumpers - Se estiverem activo um ou o
outro faz:
if (!bumperE.isSet() || !bumperD.isSet()
) { // de o bumper Esquerdo
// ou Direito estiver
// activo faz
parar(); // para
mover(-9, 0); // anda para traz
durante
Thread.sleep(500); // 450ms
parar();
}
// Controlo proporcional
float erro = distE - DIST_BASE_E;
int delta = (int) (erro * GANHO);
if (delta > 7) {
// caso especial de erro muito g
rande devido a perda
// subita de parede
delta = 7;
}
mover(VELO_BASE, delta);
if (distF < DIST_BASE_F) {
// caso especial de obstaculo em
frente
rodar(-85);
}
if (valChama > HA_CHAMA)
estado = CENTRAR;
// Condicao a implementar:
// Se ponto de partida
// estado = ESPERAR;
// voltar a ilha
// Quando v a linha
linha = (sensorLinha.sample() < LIMITE_L
INHA);
if (linha == true) {
parar();
mover(16, 0);
Thread.sleep(150);
linha = (sensorLinha.sample() <
LIMITE_LINHA); //2 Teste Linha
if (linha == true) {
estado = PARAR; // Depoi
s do segundo teste se
// detetar linha Para
} else {
estado = DETETAR;
}
}
break;
case CENTRAR:
// . . .
// Comportamento CENTRAR
// ***** Comportamento
led.set();//activa o led
switch (dirChama) {
case 0:
mover(6, -4);
break;
case 1:
mover(6, -2);
break;
case 2:
mover(6, 0);
break;
case 3:
mover(6, -2);
break;
case 4:
mover(6, -4);
break;
}
if (dirChama == -1) {
led.clear(); //desactiva o led
// perdeu a vela
// ...
// no implementado
}
if (valChama > PERTO_CHAMA)
estado = APAGAR;
break;
case APAGAR:
// . . .
// Comportamento APAGAR
// ***** Comportamento
apagarVela(2);
Thread.sleep(1000);
detetarChama();
// ***** Condies
if (valChama > HA_CHAMA)
estado = CENTRAR;
else
led.clear(); //desactiva o led
estado = NAVEGAR_E;
break;
case PARAR:
mover(0, 0);
break;
case DETETAR:
// Detectou a linha do quarto
if (estadolinha == false) { // a entrar
no quarto
parar();
if (SUVTron.sample() > 100) { //
com vela no quato
estadolinha = true;// se
for maior que 951 significa
// que ha chama logo continua
// no seu estado
estado = NAVEGAR_E;
} else { // sem vela no quarto
if (!botaoVermelho.isSet
()) {
estado = ESPERAR
;
}
mover(-7, 0); // recua
Thread.sleep(1800);// du
rante 500ms 1800
rodar(-185); //
// rodar(-60); // roda -
85
mover(16, 0); // anda em
frente durante
Thread.sleep(1600);
estadolinha = false;// 1
500 ms
estado = NAVEGAR_E; // V
olta ao estado Navegar_E
}
} else { // a sair do quarto
// monobra se ne
cessrio
estadolinha = false;
estado = NAVEGAR_E; // Volta ao
estado Navegar_E
}
break;
}
}
} catch (Throwable t) {
t.printStackTrace();
}
}
// Funes Auxiliares
public static void mover(int power, int delta) {
motorE.setPower(power - delta);
motorD.setPower(power + delta);
}
public static void rodar(int graus) {
if (graus < 0) {
graus = -graus;
motorE.setPower(VELO_RODAR);
motorD.setPower(-VELO_RODAR);
} else {
motorE.setPower(-VELO_RODAR);
motorD.setPower(VELO_RODAR);
}
try {
Thread.sleep((int) (graus * FATOR_RODAR));
} catch (Throwable t) {
t.printStackTrace();
}
parar();
}
public static void parar() {
motorE.setPower(0);
motorD.setPower(0);
}
public static void detetarChama() {
int max = 0;
int val = 0;
int dir = -1;
for (int i = 0; i < 5; i++) {
val = sensorChama[i].sample();
// System.out.println("v: " + valor);
if (val > max) {
max = val;
dir = i;
}
}
valChama = max;
if (max > HA_CHAMA)
dirChama = dir;
else
dirChama = -1;
}
public static void apagarVela(int v) {
ventoinha.setPower(16);
for (int i = 0; i < v; i++) {
rodar(10);
rodar (-20);
rodar (10);
}
ventoinha.setPower(0);
}
public static void mostrarValoresEmColuna(int v1, int v2) {
display.print(0, "v1: " + Integer.toString(v1));
display.print(1, "v2: " + Integer.toString(v2));
}
public static void mostrarValoresEmLinha(int v1, int v2) {
display.print(0,
"v1: " + Integer.toString(v1) + " v2: " + Intege
r.toString(v2));
}
public static void mostrarEstado(int estado) {
switch (estado) {
case ESPERAR:
display.print(1, "ESPERAR");
break;
case NAVEGAR_E:
display.print(1, "NAVEGAR_E");
break;
case CENTRAR:
display.print(1, "CENTRAR");
break;
case APAGAR:
display.print(1, "APAGAR");
break;
case PARAR:
display.print(1, "PARAR");
break;
case DETETAR:
display.print(1, "DETETAR");
}
}
}