NAVEGACIÓN ROBÓTICA CON FISCHERTECHNIK -rbGetInputs()- (26-Marzo-07)

Una vez armado el prototipo del robot móvil, se ha procedido a la programación de la tarea de navegación que ha de realizar. Esta tarea consiste en la navegación autónoma del robot, utilizando como sensor únicamente el de toque. El algoritmo consiste en realizar la tarea hasta que se oprima la tecla ESC para terminarla. Se comienza avanzando hasta que uno de los tres sensores sea activado (el robot lleva dos sensores enfrente y uno atras), si el sensor de la derecha es activado, el robot retrocederá y girará hacia la izquierda en busca de zonas libres de obstáculos, si el sensor de la izquierda es activado, hará lo contrario. Si el sensor de atras se activa, avanzará hacia adelante. Finalmente, si los dos sensores de enfrente son activados al mismo tiempo, el robot retrocederá.

#include "Windows.h"
#include "iostream.h"
#include "conio.h"
#include "stdio.h"
#include "umFish40VC.H"

void derecha(int interfaz){
rbSetMotorEx(interfaz, 1, 1, 2);
rbSetMotorEx(interfaz, 2, 2, 2);
}

void izquierda(int interfaz){
rbSetMotorEx(interfaz, 1, 2, 2);//rbSetMotorEx permite manipular un motor variando su velocidad
rbSetMotorEx(interfaz, 2, 1, 2);//rbSetMotorEx(iHnadle,motNr, Dir, Vel)
}

void atras(int interfaz){
rbSetMotorEx(interfaz, 1, 2, 2);
rbSetMotorEx(interfaz, 2, 2, 2);
}

void adelante(int interfaz){
rbSetMotorEx(interfaz, 1, 1, 2);
rbSetMotorEx(interfaz, 2, 1, 2);
}

void main() {

char Ende;
cout << "Corriendo Programa" <<>
int iHandle = rbOpenInterfaceUSB(ftROBO_first_USB, 0);//iHandle será el manejador de la interfaz actual

if (iHandle == rbFehler) { //rbFehler es el código de error, es 0 cuando no logra conectar con la interfaz
cout << "Imposible conectar con la interfaz (enter para terminar)" <<>
cin.get(Ende);
return;
}

do{

while( !rbGetInputs(iHandle) )//mientras no se active ningún pulsador
adelante(iHandle);

if( rbGetInput(iHandle, 1) ){ // si el pulsador 1 es activado
atras(iHandle);
Sleep(800);
izquierda(iHandle);
Sleep(500);
}

else if( rbGetInput(iHandle, 2) ){//si el pulsador 2 es activado
atras(iHandle);
Sleep(800);
derecha(iHandle);
Sleep(500);
}

else if( rbGetInput(iHandle, 3) ){//si el pulsador 3 es activado
while( !rbGetInputs(iHandle) )
adelante(iHandle);
}

else if( (rbGetInput(iHandle, 1)) && (rbGetInput(iHandle, 2)) ){
while( !rbGetInputs(iHandle) )
atras(iHandle);
}

}while(!kbhit());

rbCloseInterface(iHandle);//finaliza la conexión con la interfaz
cout <<>
getch();

}