Diferenças

Esta página mostra as diferenças entre a revisão do documento que escolheu e a versão actual.


Ligação para esta vista de comparação

Ambos os lados da revisão anterior Revisão anterior
ibbot_docs [2016/05/12 17:08]
ccarreto
ibbot_docs [2016/05/12 17:41] (Actual)
ccarreto [Controlador Baseado em Comportamentos]
Linha 29: Linha 29:
  
  
-<​sxh ​C>+<​sxh ​Java> 
 +/** 
 +* Este programa implementa um controlador baseado em comportamentos para o robô  
 +* bombeiro usado nas aulas, de modo a que este consiga navegar até ao quarto ilha,  
 +* extinguir a chama da vela nesse quarto e regressar ao ponto de partida. 
 +* O controlador foi implementado com base numa máquina de estados finitos.  
 +
 +* @author ​ Carlos Carreto 
 +* @version 12/​05/​2016 
 +*/ 
 + 
 +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;​ 
 + 
 +public class RBv1 { 
 + 
 + // Declaração dos parâmetros 
 + // ================================================================================ 
 +    // Os parâmetros devem ser "​afinados"​ para cada robô em particular. 
 +  
 + // Valor a partir do qual se considera que é linha branca 
 + //O sensor de linha deve estar a uma distância do chão entre 5 a 10 mm.  
 + private static final int LIMITE_LINHA = 100; 
 + 
 + // Valor a partir do qual se considera que é chama 
 + private static final int LIMITE_CHAMA = 500; 
 + private static final int LIMITE_PERTO_CHAMA = 1000; 
 + 
 + // 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 = 0.9f; 
 + private static final int LIMITE_DELTA = 4; 
 + 
 + // Fator para converter graus em tempo 
 + private static final float FATOR_RODAR = 10.5f; 
 + 
 + // Distâncias para os algoritmos de seguir paredes 
 + private static final int DIST_BASE_E = 15; 
 + private static final int DIST_BASE_F = 15; 
 + 
 +  
 + // Declaração identificadores 
 +    // ================================================================================ 
 + 
 + // Estados possiveis 
 + 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 TESTES = 4; 
 + 
 + // Marcas possiveis 
 + private static final int SEM_MARCA = 0; 
 + private static final int LINHA = 1; 
 + private static final int CIRCULO = 2; 
 + 
 +  
 + // Declaração dos Objetos e Variáveis 
 + // ================================================================================ 
 + 
 + // LCD 
 + private static Display display; 
 +  
 + //​ThumbWheel 
 + private static AnalogInput thumbWheel;​ 
 + 
 + // Motores 
 + private static Motor motorE; 
 + private static Motor motorD; 
 + 
 + // Ventoinha 
 + private static Motor ventoinha;​ 
 + 
 + // Sensor de linha 
 + private static AnalogInput sensorLinha;​ 
 + private static int marca; ​ //​Identificador da marca detetada pelo sensor de linha.  
 + 
 + // Sensor da chama 
 + private static AnalogInput[] sensorChama;​ 
 + private static int dirChama; //Direção da chama da vela. 
 + private static int valChama; //Valor máximo obtido a partir do sensor de chama. 
 + 
 + // Sensores SONAR 
 + private static RangeFinder sonarE; 
 + private static RangeFinder sonarF; 
 + private static RangeFinder sonarD; 
 + private static int distE; ​ //​Distância lida pelo SONAR da esquerda. 
 + private static int distF; ​ //​Distância lida pelo SONAR da frente. 
 + private static int distD; ​ //​Distância lida pelo SONAR da direita. 
 + 
 + // Botão START (verde) 
 + private static IntelliBrainDigitalIO botaoSTART;​ 
 + 
 + public static void main(String[] args) { 
 + try { 
 + 
 + // Criação dos objectos 
 + // LCD 
 + display = IntelliBrain.getLcdDisplay();​ 
 + display.print(0,​ "Robô Bombeiro"​);​ 
 +  
 + //​ThumbWheel 
 + thumbWheel =  IntelliBrain.getThumbWheel();​ 
 +  
 + // Motors Rodas 
 + motorE = new ContinuousRotationServo(IntelliBrain.getServo(1),​ false, 14); 
 + motorD = new ContinuousRotationServo(IntelliBrain.getServo(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);​ 
 + marca = 0; 
 + 
 + // Sensores Sonar 
 + sonarE = new ParallaxPing(IntelliBrain.getDigitalIO(3));​ 
 + sonarF = new ParallaxPing(IntelliBrain.getDigitalIO(4));​ 
 + sonarD = new ParallaxPing(IntelliBrain.getDigitalIO(5));​ 
 + 
 + // Botões 
 + botaoSTART = IntelliBrain.getDigitalIO(10);​ 
 + botaoSTART.setPullUp(true);​ 
 + 
 + // Estado inicial 
 + int estado = ESPERAR; 
 + while (true) { 
 + // Leitura dos sensores SONAR 
 + 
 + distE = (int) lerDist(sonarE);​ 
 + if (distE < 0) 
 + distE = 100; 
 + 
 + distF = (int) lerDist(sonarF);​ 
 + if (distF < 0) 
 + distF = 100; 
 + 
 + // mostrarEstado(estado);​ 
 + switch (estado) { 
 + case ESPERAR: 
 + estado = estadoEsperar();​ 
 + break;​ 
 + case NAVEGAR_E:​ 
 + estado = estadoNavegarE();​ 
 + break;​ 
 + case CENTRAR: 
 + estado = estadoCentrar();​ 
 + break;​ 
 + case APAGAR: 
 + estado = estadoApagar();​ 
 + break;​ 
 + case TESTES: 
 + estado = estadoTestes();​ 
 + break;​ 
 +
 +
 + } catch (Throwable t) { 
 + t.printStackTrace();​ 
 +
 + 
 +
 + 
 + // Funções que implementam os estados da MEF 
 + // ================================================================================ 
 + 
 + /** 
 + * Esta função implementa o estado ESPERAR. 
 + * @return estado  
 + */ 
 + private static int estadoEsperar() { 
 + // Esperar que o botão START seja premido 
 + while (botaoSTART.isSet()) { 
 + //Este ciclo pode ser usado para para mostrar valores de sensores. 
 + //Eis alguns exemplos: 
 +  
 + // distE = (int) lerDist(sonarE);​ 
 + //distF = (int) lerDist(sonarF);​ 
 + //​mostrarValoresEmColuna(distF,​ distE); 
 + 
 + mostrarValoresEmColuna(sensorLinha.sample(),​ 0); 
 + 
 + //​detetarChama();​ 
 + //​mostrarValoresEmColuna(dirChama,​ valChama);​ 
 +
 + 
 + // Avançar durante 2 segundos para sair da zona branca 
 + mover(16, 0); 
 + esperar(2000);​ 
 + 
 + return NAVEGAR_E;​ 
 + //return TESTES; 
 +
 + 
 + /** 
 + * Esta função implementa o estado NAVEGAR_E 
 + * @return estado 
 + */ 
 + private static int estadoNavegarE() { 
 + // === Comportamento === 
 + // Controlo proporcional 
 + float erro = distE - DIST_BASE_E;​ 
 + int delta = (int) (erro * GANHO); 
 + 
 + // Caso especial de erro muito grande devido a perda 
 + // súbita de parede 
 + if (delta > LIMITE_DELTA) 
 + delta = LIMITE_DELTA;​ 
 + 
 + mover(VELO_BASE,​ delta); 
 + 
 + // Caso especial de parede em frente 
 + if (distF < DIST_BASE_F) 
 + rodarAng(-90);​ 
 + 
 + // Bumpers 
 + // ... 
 + 
 + // === Condições === 
 + // Se detetar chama 
 + detetarChama();​ 
 + if (valChama > LIMITE_CHAMA) 
 + return CENTRAR; 
 + 
 + // Se detetar círculo 
 + marca = detetarMarca();​ 
 + if (marca == CIRCULO) 
 + return ESPERAR; 
 + else if (marca == LINHA) { 
 + // A implementar... 
 + // Para parar e verificar se a chama está nesse quarto ou 
 + // Para indicar se está a entrar no quart ou a sair do quarto 
 + // etc. 
 +
 +
 + 
 + return NAVEGAR_E;​ 
 +
 + 
 + /** 
 + * Esta função implementa o estado CENTRAR. 
 + * @return estado 
 + */ 
 + private static int estadoCentrar() { 
 + // === Comportamento === 
 + detetarChama();​ 
 + 
 + switch (dirChama) { 
 + case 0: 
 + mover(5, -3); 
 + break; 
 + case 1: 
 + mover(5, -2); 
 + break; 
 + case 2: 
 + mover(5, 0); 
 + break; 
 + case 3: 
 + mover(5, 2); 
 + break; 
 + case 4: 
 + mover(5, 3); 
 + break; 
 +
 + 
 + // === Condições === 
 + if (dirChama == -1) { 
 + // Perdeu a chama da vela. 
 + // ... 
 + // A implementar. Por exemplo, o robô roda sobre si próprio até voltar a encontrar. 
 +
 + 
 + if (valChama > LIMITE_PERTO_CHAMA) { 
 + parar();​ 
 + return APAGAR; 
 + }  
 + 
 + return CENTRAR; 
 +
 + 
 + /** 
 + * Esta função implementa o estado APAGAR; 
 + * @return estado 
 + */ 
 + private static int estadoApagar() { 
 + // === Comportamento === 
 + apagarVela(2);​ 
 + esperar(2000);​ 
 + 
 + // Atualizar variáveis da chama para verificar se o apagar foi bem sucedido. 
 + detetarChama();​ 
 + 
 + // === Condições === 
 + if (valChama > LIMITE_CHAMA) 
 + return CENTRAR; 
 + else 
 + return NAVEGAR_E;​ 
 +
 + 
 + /** 
 + * Esta função implemeta um estado de testes, para facilitar a implementação de  
 + * testes de funções e ideias. 
 + *   
 + * @return estado 
 + */ 
 + private static int estadoTestes() { 
 + //Exemplo de chamada de um estado para testar o comportamento APAGAR.  
 + apagarVela(2);​ 
 + 
 + return ESPERAR; 
 +
 + 
 + // Funções Auxiliares 
 + // ================================================================================ 
 + 
 + /** 
 + * Esta função devolve a distância medida por um dado sensor SONAR. 
 + * @param s - Sensor sonar 
 + * @return distância medida pelo sensor s 
 + */ 
 + private static float lerDist(RangeFinder s) { 
 + s.ping();​ 
 + esperar(35);​ 
 + return s.getDistanceCm();​ 
 +
 + 
 + /** 
 + * Esta função lê os valores do sensor de chama 
 + * e atualiza as variáveis valChama e dirChama. 
 + */ 
 + private 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 > LIMITE_CHAMA) 
 + dirChama = dir; 
 + else 
 + dirChama = -1; 
 +
 + 
 + /** 
 + * Esta função liga a ventoinha para extinguir a chama da vela. 
 + * @param v - número de vezes que o robô executa o movimento. 
 + */ 
 + private static void apagarVela(int v) { 
 + ventoinha.setPower(16);​ 
 + for (int i = 0; i < v; i++) { 
 + rodar(20);​ 
 + rodar(-40);​ 
 + rodar(20);​ 
 +
 + ventoinha.setPower(0);​ 
 +
 + 
 + /** 
 + * Esta função verifica que tipo de marca branca está a ser detetada. 
 + * @return - tipo de marca branca. 
 + */ 
 + private static int detetarMarca() { 
 + if (sensorLinha.sample() < LIMITE_LINHA) { 
 + // É marca branca. 
 + // Verificar se é linha ou círculo. 
 + mover(8, 0); 
 + esperar(500);​ 
 + parar();​ 
 + if (sensorLinha.sample() < LIMITE_LINHA) 
 + return CIRCULO; 
 + else 
 + return LINHA; 
 +
 + return SEM_MARCA; // Não é marca abranca. 
 +
 + 
 + /** 
 + * Esta função move o robô em linha reta (se delta=0) ou descrevendo uma curva. 
 + * @param power- velocidade base. 
 + * @param delta - valor a somar e subtrair à velocidade base para definir uma curva. 
 + */ 
 + private static void mover(int power, int delta) { 
 + motorE.setPower(power - delta); 
 + motorD.setPower(power + delta); 
 +
 + 
 + /** 
 + * Esta função roda o robô sobre si próprio. 
 + * @param velo - velocidade de rotação. Roda CW se valor negativo. 
 + */ 
 + private static void rodar(int velo) { 
 + motorE.setPower(-velo);​ 
 + motorD.setPower(velo);​ 
 +
 + 
 + /** 
 + * Esta função roda o robô sobre si próprio um dada ângulo. 
 + * @param graus - gruas a rodar (aproximadamente). Roda CW se valor negativo. 
 + */ 
 + private static void rodarAng(int graus) { 
 + if (graus < 0) { 
 + graus = -graus; 
 + rodar(-VELO_RODAR);​ 
 + } else { 
 + rodar(VELO_RODAR);​ 
 +
 + 
 + esperar((int) (graus * FATOR_RODAR));​ 
 + parar();​ 
 + 
 +
 + 
 + /** 
 + * Esta função pára os motores. 
 + */ 
 + private static void parar() { 
 + motorE.setPower(0);​ 
 + motorD.setPower(0);​ 
 +
 + 
 + /** 
 + * Esta função faz uma pausa na execução do programa. 
 + * @param ms- número de milisegundos a esperar. 
 + */ 
 + private static void esperar(int ms) { 
 + try { 
 + Thread.sleep(ms);​ 
 + } catch (Throwable t) { 
 + t.printStackTrace();​ 
 +
 +
 + 
 + /** 
 + * Esta função escreve dois valores respetivamente nas linha 0 e 1 do LCD.  
 + * @param v1 - valor a escrever na linha 0. 
 + * @param v2 - valor a escrever na linha 1. 
 + */ 
 + private static void mostrarValoresEmColuna(int v1, int v2) { 
 + display.print(0,​ "v1: " + Integer.toString(v1));​ 
 + display.print(1,​ "v2: " + Integer.toString(v2));​ 
 +
 + 
 + /** 
 + * Esta função escreve dois valores na linha 0 do LCD.  
 + * @param v1 - valor a escrever em primeiro lugar. 
 + * @param v2 - valor a escrever em segundo lugar. 
 + */ 
 + private static void mostrarValoresEmLinha(int v1, int v2) { 
 + display.print(0,​ "v1: " + Integer.toString(v1) + " v2: " + Integer.toString(v2));​ 
 +
 + 
 + /** 
 + * Esta função escreve um título na linha 0 e dois valores na linha 1 do LCD. 
 + * @param titulo - titulo 
 + * @param lv1 - label do valor v1 
 + * @param v1 - valor v1 
 + * @param lv2 - label do valor v2 
 + * @param v2 - valor v2 
 + */ 
 + private static void mostrarValoresEmLinha(String titulo, String lv1, int v1, String lv2, int v2) { 
 + display.print(0,​ titulo); 
 + display.print(1,​ lv2 + v1 + " " + lv2 + v2); 
 + 
 +
 + 
 + /** 
 + * Esta função escreve na linha 1 do LCD o nome do estado corrente. 
 + * @param estado - identificador do estado corrente. 
 + */ 
 + private 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 TESTES: 
 + display.print(1,​ "​TESTES"​);​ 
 + break; 
 +
 +
 + 
 + /** 
 + * Esta função mostra os valores lidos pelos sensores. 
 + * Os valores a mostrar são selecionados com o thumbWheel. 
 + * Chamar dentro de um ciclo. 
 + */ 
 + public void mostrarValoresSensores() { 
 + int indiceAnterior = -1; 
 + 
 + int indiceAtual = scaleInput(thumbWheel,​ 4); 
 + if (indiceAtual != indiceAnterior) { 
 + indiceAnterior = indiceAtual;​ 
 + switch (indiceAtual) { 
 + case 0: 
 + distE = (int)lerDist(sonarE);​ 
 + distF = (int)lerDist(sonarF);​ 
 + esperar(100);​ 
 + mostrarValoresEmLinha("​SONAR",​ "​E:",​ distE, "​F:",​ distF); 
 + break;​ 
 + case 1: 
 + distF = (int)lerDist(sonarF);​ 
 + distD = (int)lerDist(sonarD);​ 
 + esperar(100);​ 
 + mostrarValoresEmLinha("​SONAR",​ "​F:",​ distE, "​D:",​ distD); 
 + break;​  
 + case 2: 
 + mostrarValoresEmLinha("​Linha",​ "​v:",​ sensorLinha.sample(),​ "​--",​ 0); 
 + break;​ 
 + case 3: 
 + detetarChama();​ 
 + mostrarValoresEmLinha("​Chama",​ "​val:",​ valChama, "​dir:",​ dirChama);​ 
 + break;​ 
 +
 +
 +
 + 
 + /** 
 + * Esta função lê o valor analógico para devolver um dado número de valores discretos. 
 + * @param ​  - numeroDeValores (1 - 256) 
 + * @return ​ - Um valor com base na posição do thumbwheel onde 0 <= valor < numeroDeValores 
 + */ 
 + public static int scaleInput(AnalogInput input, int numeroDeValores) { 
 + return input.sample() * numeroDeValores / (input.getMaximum() + 1); 
 +
 + 
 +}
  
 </​sxh>​ </​sxh>​
Recent changes RSS feed Creative Commons License Donate Minima Template by Wikidesign Driven by DokuWiki