Eng Elet - Kit Lego Mindstorm "Resgarte em Marte"

Código Final - SLAVE

2008-11-28T17:49:00.000-03:00

//SLAVE

#define BT_CONN 1
#define INBOX 5
#define OUTBOX 1

bool sensor, sentido, touch, ativado;
int move_time,count, max_busca, x=0;
mutex semaforo;

#define velocidade 50

#define esquerda true
#define direita false

//MENSAGENS RECEBIDAS
#define VAI 1
#define PARA 2

//MENSAGENS ENVIADAS
#define PEGUEI 3

// VERIFICA SE HA CONEXAO BLUETOOTH ATIVADA
sub BTCheck(int conn){
if (!BluetoothStatus(conn)==NO_ERR){
TextOut(0,LCD_LINE1,"Impossivel conectar");
Wait(1000);
Stop(true);
}
}

// THREAD QUE REALIZA COMUNICACAO COM MASTER
task procura_master(){
int in;

// FICA SEMPRE AGUARDANDO CONTATO DO MASTER
while(true){
if (ReceiveRemoteNumber(INBOX, true, in) != STAT_MSG_EMPTY_MAILBOX) {
// O MASTER MANDOU O ESCRAVO INICIAR O RESGATE
if (in == VAI) {
// AINDA NÃO CAPTUREI NENHUM BONECO,
// VOU PROCURAR ALGUÉM
if ((x == 0) && (ativado == false)) {
TextOut(0,LCD_LINE1,"Indo ao resgate ...");
OnFwd(OUT_BC, velocidade);
ativado = true;
}
// JÁ TENHO UM BONECO CAPTURADO
// O MASTER SÓ ENTRARÁ EM CONTATO COM O SLAVE
// SE A DISTANCIA ENTRE ELES FOR PEQUENA,
// LOGO, SE HOUVER CONTATO, PODE SOLTAR O REFÉM
else if (x == 1) {
// ENTRA NA REGIAO MUTUAMENTE EXCLUSIVA,
// INTERROMPE OS MOTORES, SOLTA O BONECO,
// INFORMA AO MESTRE E SE POSICIONA PARA
// POSSIVEL NOVO RESGATE
Acquire(semaforo);
ativado = false;
Off(OUT_BC);

TextOut(0,LCD_LINE1,"Devolvendo boneco ...");
SendRemoteNumber(BT_CONN,OUTBOX,PEGUEI);
//SendRemoteString(BT_CONN,OUTBOX,PEGUEI);

// ABRE A GARRA
OnRev(OUT_A, 50);
Wait(500);
Off(OUT_A);
x=0;

// SE POSICIONA
OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
Wait(500);
until (sensor){
OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
}

Wait(1000);
Release(semaforo);
}

}
// RECEBI ORDEM DE FINALIZACAO
if (in == PARA) {
TextOut(0,LCD_LINE1,"Resgate finalizado ...");
Stop(true);
}
}
}
}
}

// VERIFICA SENSORES
task testa_sensor(){
while(true){
sensor = SENSOR_3;
touch = SENSOR_1;
}
}

// REALIZA MOVIMENTACAO DO ROBÔ
task atua_motores(){
while(true){
Acquire(semaforo);
// SE NÃO RECEBEU ORDEM DE RESGATE, NÃO FAZ NADA
if (!ativado) {
Release(semaforo);
continue;
}

// ESTOU FORA DA LINHA :(
if (!sensor) {
while ((count < move_time) && (!sensor)) {
// viro para a esquerda
if (sentido) {
OnFwd(OUT_B, velocidade);
OnRev(OUT_C, velocidade);
}
// viro para a direita
else {
OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
}
count++;

// achei a linha
if (sensor) break;
}

// PROXIMA VEZ QUE SAIR DA LINHA
// VIVAREI NO SENTIDO OPOSTO
sentido = !sentido;

if (max_busca < 5)
move_time = move_time + 150;
else {
max_busca =0;
move_time = move_time + 2000;
sentido = esquerda;
}

max_busca++;
count = 0;
}
// ESTOU NA LINHA
else {
OnFwd(OUT_BC, velocidade);
count = 0;
move_time = 150;
max_busca = 0;

//sentido = !sentido;
}

Release(semaforo);
}
}

// REALIZA O RESGATE DO BONECO
task garra(){
while(true){
Acquire(semaforo);
if (touch == true && x==0){
Off(OUT_BC);
OnFwd(OUT_A, 35);
Wait(1000);
x=1;

OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
Wait(500); until (sensor){
OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
}

}
Release(semaforo);
}
}

task main()
{
max_busca = 0;
count = 0;
sentido = esquerda; //false = direita, true = esquerda
ativado = false;

SetSensorType(IN_1,SENSOR_TYPE_TOUCH); //sensor de toque
SetSensorMode(IN_1,SENSOR_MODE_BOOL);
SetSensorType(IN_3,SENSOR_TYPE_LIGHT_ACTIVE); //sensor de luz
SetSensorMode(IN_3,SENSOR_MODE_BOOL);

move_time = 150;

BTCheck(1);

// LEVANTA A GARRA
OnRev(OUT_A, 50);
Wait(500);
Off(OUT_A);

Precedes(testa_sensor, atua_motores, garra, procura_master);
}

Código Final - MASTER

2008-11-28T17:47:00.000-03:00

//MASTER

#define BT_CONN 0
#define INBOX 1
#define OUTBOX 5

//NUMERO DE BONECOS A SEREM RESGATADOS
#define BONECOS 5

//DISTANCIA
#define DISTANCIA 200 //mm

//MENSAGENS ENVIADAS
#define VAI 1
#define PARA 2

//MENSAGENS RECEBIDAS
#define PEGUEI 3

// VERIFICA SE HÁ UMA CONEXAO BLUETOOTH ATIVADA
sub BTCheck(int conn){
if (!BluetoothStatus(conn)==NO_ERR){
TextOut(0,LCD_LINE2,"Impossivel conectar");
Wait(1000);
Stop(true);
}
}

// THREAD PRINCIPAL
// FAZ TODA COMUNICACAO COM ESCRAVO
task main(){
int contador = 0 ;

int in;

// ATIVA SENSOR ULTRASONICO
SetSensorLowspeed(IN_4);

// VERIFICA SE HÁ UMA CONEXAO BLUETOOTH ATIVADA
BTCheck(0);

TextOut(0,LCD_LINE1,"Master iniciado ...");
while(true){
// SE O FOR DETECADO ALGUMA COISA A UMA DISTANCIA INFERIOR A 'DISTANCIA'
// TENTA UMA COMUNICACAO
if (SensorUS(IN_4) < DISTANCIA) {
// CASO A QUANTIDADE DE BONECOS RECEBIDOS SEJA IGUAL
// OU SUPERIOR A DE BONECOS ESPERADOS, ENCERRA RESGATE
if (contador >= BONECOS) {
TextOut(0,LCD_LINE1,"Enviando comando de encerramento");
SendRemoteNumber(BT_CONN,OUTBOX,PARA);

Wait(1000);
Stop(true);

// CASO CONTRARIO, ENVIA A ORDEM DE RESGATE
} else {
TextOut(0,LCD_LINE1,"Enviando comando de iniciar resgate");
SendRemoteNumber(BT_CONN,OUTBOX,VAI);
}
}

// VERIFICA SE RECEBEU UMA RESPOSTA DO ESCRAVO
if (ReceiveRemoteNumber(INBOX, true, in) != STAT_MSG_EMPTY_MAILBOX) {
// O ESCRAVO RESPONDEU QUE DEVOLVEU UM BONECO
if (in == PEGUEI) {
TextOut(0,LCD_LINE1,"Boneco recebido");
contador++;
Wait(1000);
}
}
Wait(100);
}
}
}

Foto Protótipo Final

2008-11-28T17:44:00.002-03:00

Vídeo - Versão final

2008-11-16T19:47:00.002-03:00

Agora funfou!!!

CÓDIGO

2008-11-13T19:36:00.004-03:00

bool sensor, sentido, touch;
int move_time,count, max_busca, x=0;
mutex semaforo;

#define velocidade 50

#define esquerda true
#define direita false

task testa_sensor(){
while(true){
sensor = SENSOR_3;
touch = SENSOR_1;
}
}

task atua_motores(){
while(true){
Acquire(semaforo);
if (!sensor){
while((count < move_time) && (!sensor)){
if (sentido){
OnFwd(OUT_B, velocidade);
OnRev(OUT_C, velocidade);
}
else {
OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
}
count++;
if (sensor)
break;
}
sentido = !sentido;
if (max_busca < 5)
move_time = move_time + 150;
else{
max_busca =0;
move_time = move_time + 2000;
sentido = esquerda;
}

max_busca++;
count = 0;
}
else {
OnFwd(OUT_BC, velocidade);
count = 0;
move_time = 150;
max_busca = 0;
sentido = !sentido;
}
Release(semaforo);
}
}

task garra(){
while(true){
Acquire(semaforo);
if (touch == true && x==0){
Off(OUT_BC);
OnFwd(OUT_A, 35);
Wait(1000);
x=1;

OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
Wait(500);
until (sensor){
OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
}

}
else if (x != 1) {
Off(OUT_A);
}
Release(semaforo);
}
}

task main()
{
max_busca = 0;
count = 0;
sentido = esquerda; //false = direita true = esquerda

SetSensorType(IN_1,SENSOR_TYPE_TOUCH); //sensor de toque
SetSensorMode(IN_1,SENSOR_MODE_BOOL);
SetSensorType(IN_3,SENSOR_TYPE_LIGHT_ACTIVE); //sensor de luz
SetSensorMode(IN_3,SENSOR_MODE_BOOL);
move_time = 150;

OnFwd(OUT_BC, velocidade);
OnRev(OUT_A, 50);
Wait(500);
Off(OUT_A);

Precedes(testa_sensor, atua_motores, garra);
}

---------------------------------------------------------------------------

Esta é o código que segue a linha preta através dos motores B e C e o sensor de luz, faz as leituras dos sensores em THREADS em conjunto com as outras rotinas e também realiza a captura do boneco através do sensor de toque e o motor A.
Quando o sensor de toque é ativado (presença do sobrevivente), é então acionado o motor que aprisiona o boneco em uma estrutura altamente avançada e rebusta ( =]] ). O robô, por conseguinte, gira 180º e continua sua rota de volta ao encrontro do MASTER.

Falta agora implementar a comunicação entre o MESTRE e o ESCRAVO via "DENTE AZUL" (BlueTooth).

Estamos caminhando...

4º perfil de montagem - INSUCESSO também!!!!

2008-11-13T19:29:00.002-03:00

Presença de dois eixos rotativos localizados nas partes laterais do Robô.
Problemas na hora de realizar curvas.

SOLUÇÂO: VOLTAMOS A MONTAR O PRIMEIRO PROTOTIPO REALIZADO EM SETEMBRO (VER FOTOS E VIDEOS) QUE FUNCIONA TUDO... FOLLOWLINE... GARRA.... TUDINHO.. SO NÃO CONSEGUIA SUBIR A LEDEIRA... PORÉM COM AS LIXAS INSTALADAS, FOI POSSÍVEL REALIZÁ-LA!! SUCESSO TOTAL AGORA!!

OBS: NOVAS FOTOS E VIDEOS DA VERSÃO FINAL VIRÃO!!

=]]]]]]]]]

AEEEEEEEEEEEEEEEHHHHHHHHHHHHHHHHHHHHHHHHHH!!!!!!!!!!!!!!!

Tentativa de "baliza" para o 3º perfil = Tristeza total!!!!!!

2008-11-07T20:48:00.004-03:00

Comentário:

"Pelo menos fizesse uma gravação um tanto quanto mais ou menos!!.
O carrinho não funfou o video ficou pior ainda =]]]"

3º perfil de montagem

2008-11-04T20:45:00.003-03:00

3ª tentativa com 3 motores: 1 para ir para frente e para trás, outro para fazer rotação (direita e esquerda) e outro para garra!!!
Porém, tivemos difuculdades para caminhar sobre a linha e consequentemente capturar o sobrevivente. Este conseguiu subir e descer a rampa mas não conseguiu realizar as tarefas ditas anteriormente!!

2ª montagem para o prototipo!!

2008-10-30T19:01:00.004-03:00

FALHA!!!!
Não Funfou!!
Problemas ao realizar curvas!!
-> derrapando e interferindo a rotina de caminhar sobre o caminho

Teste -Programa em campo para o resgate

2008-10-14T18:02:00.004-03:00

Simulando o programa de segmento do robô pela linha mais a rotina de captura do sobrevivente (que no futuro poderá não estar mais vivo) em threads (Mutex - Semaforo)

Problemas encontrados:

-> Dificuldade em subir e descer na rampa devido a estrutura física do robô
-> Falta de atrito entre as rodas e a rampa
-> Captura do cidadão que impede também a subida do robô, devido ao fato deste ficar na frente do móvel.

Tentativas para solução:

-> Modificação da estrutura
-> Modificação da garra de captura
-> Mexer na altura da estrutura frontal que segura as partes do sensor de toque e luz
-> Eixo traseiro

Tentativa 1 para resgate do boneco

2008-10-10T16:37:00.002-03:00

Montagem 1

Agora funfou!! [Teste SensorLight]

2008-09-30T19:19:00.004-03:00

Testando sensor de Luz

2008-09-16T13:12:00.000-03:00

Sensorlight + Caminho pela linha

Montagem 1

2008-09-16T12:59:00.001-03:00

Construção inicial do protótipo após inúmeros testes de montagens.