O deslocamento determinístico do robô, bem como sua exata localização no ambiente é extremamente importante para uma navegação desejada. Neste trabalho prático a utilização de sensores do tipo break-beam para a construção do shaft-encoders será de extrema importância para tal desenvolvimento.
Os objetivos deste trabalho prático são:
1. Sensores Ópticos
Utilizamos um potenciômetro e um ohmímetro para desenvolver esta atividade. Colocamos o potenciômetro em paralelo à carga (resistência 47k + sensor) e variamos seu valor, lendo constantemente o valor dado pelo sensor. Concluímos que a resistência que melhor se ajustou foi uma equivalente à resistência da carga, ou seja, aproximadamente 47k. Isso pode ser explicado pelo teorema da máxima transferência de potência, onde a potência máxima transferida para uma carga, em paralelo a um circuito eletrônico, se faz quando a resistência da carga é a mesma da resistência equivalente do circuito.
Sabendo que V=5 – 2 =3 volts e que R=330 ohms temos: V=RI I = 3/330 I = 9,1 mA
Sabendo que a handyboar consegue drenar até 100 mA de corrente para cada canal e, sendo os LEDs dispostos em paralelo, o número total de LEDs será: Num = 100 / 9,1 Num = 10 LEDs
2. Simple Feedback Control
Para o desenvolvimento deste exercício implementamos os algoritmos Gentle-Turn, Hard-Turn, Three-State e Progressive propostos no livro. Os códigos podem ser vistos a seguir.
/* Funcoes andar Parede */
void andarParedePogressive() {
int sensor;
while(1)
{
sensor = analog(SENSOR_PAREDE);
if (sensor < 50) { //Virar Direita
POTENCIA_MOTOR_ESQ = 20;
POTENCIA_MOTOR_DIR = 50;
potenciaMotor();
} else if(sensor > 150){ //Virar Esquerda
POTENCIA_MOTOR_ESQ = (int)(50.0*(1.0+((float)(sensor-150)/255.0)));
POTENCIA_MOTOR_DIR = (int)(50.0*(1.0-((float)(sensor-150)/255.0)));
potenciaMotor();
} else { //Reto
POTENCIA_MOTOR_ESQ = 50;
POTENCIA_MOTOR_DIR = 50;
potenciaMotor();
}
}
}
void andarParedeThreeState() {
int sensor;
while(1)
{
sensor = analog(SENSOR_PAREDE);
if (sensor < 50) { //Virar Direita
POTENCIA_MOTOR_ESQ = 20;
POTENCIA_MOTOR_DIR = 50;
potenciaMotor();
} else if(sensor > 150){//Virar Esquerda
POTENCIA_MOTOR_ESQ = 50;
POTENCIA_MOTOR_DIR = 20;
potenciaMotor();
} else { //Reto
POTENCIA_MOTOR_ESQ = 50;
POTENCIA_MOTOR_DIR = 50;
potenciaMotor();
}
}
}
void andarParedeHard() {
while(1)
{
if (analog(SENSOR_PAREDE) < 100) { //Virar Direita
POTENCIA_MOTOR_ESQ = 0;
POTENCIA_MOTOR_DIR = 80;
potenciaMotor();
} else { //Virar Esquerda
POTENCIA_MOTOR_ESQ = 80;
POTENCIA_MOTOR_DIR = 0;
potenciaMotor();
}
}
}
void andarParedeGentle() {
while(1)
{
if (analog(SENSOR_PAREDE) < 100) {//Virar Direita
POTENCIA_MOTOR_ESQ = 0;
POTENCIA_MOTOR_DIR = 40;
potenciaMotor();
} else { //Virar Esquerda
POTENCIA_MOTOR_ESQ = 40;
POTENCIA_MOTOR_DIR = 0;
potenciaMotor();
}
}
}
A diferença básica entre o Hard Turn e o Gentle Turn é que o segundo se movimenta mais suave enquanto se desloca lateralmente à parede. Os outros dois algoritmos se comportam melhor no deslocamento, já que possuem um estado a mais.
3. Controle PD
Para desenvolver o controle utilizamos dois encoders, um para cada roda, de modo que pudessem contar o número de pulsos gerados pela rotação das mesmas. A lógica neste ponto foi definir o setpoint de operação com o valor nulo, de modo que a diferença entre a contagem dos pulsos fosse nula, ou seja, que o giro de uma roda acompanha-se o giro da outra. Os encoders a princípio deveriam ficar nas próprias roda, de modo a contar exatamente os giros destas, porém essa contagem se daria em uma frequência baixa, o que atrapalharia o controle, logo, os encoders foram acoplados em engrenagens com reduções menores. O controlador implementados foi um PD (Proporcional+Derivativo), uma vez que a parte derivativa era de fácil obtenção, já que era relacionada às velocidades de rotação das rodas. Os ganhos do controlador foram determinados empiricamente, de acordo com o conhecimento em teoria de controle e controladores PIDs.
4. Controle Deliberativo
A implementação do algoritmo wavefront foi feita com a finalidade de fazer o robô navegar em um plano cartesiano, de um ponto a outro do plano, sem trombar nos obstáculos com localizações já conhecidas. O plano cartesiano foi dividido um uma matriz de dez por dez, onde cada unidade da matriz era uma unidade do plano. O deslocamento do robô, para uma unidade do plano cartesiano, era de trinta centímetros. Para a navegação foram definidos, já na handyboard, os pontos inicial e final do robô, além do número de obstáculos e da posição destes no plano. O robô passeava pelo plano, desviando dos obstáculos, até chegar à posição final determinada. O código pode ser visto a seguir.
/* Mapa contendo os obstaculos do ambiente */
int map[X_DIM][Y_DIM];
/* Pose = posicao (x y) + orientacao */
/* posicao (x,y) atual + orientacao (N / S / L / O) */ int pose_atual[3]; /* posicao inicial */ int pose_inicial[3]; /* posicao final desejada (ignorar a orientacao) */ int pose_desejada[3];
/* Variaveis extras */
int fifo[100][2]; int indice = 0; int indice2 = 0;
/* Esta funcao deve fazer o seu robo andar 1 unidade de distancia para frente e tambem deve atualizar a variavel pose_atual, de acordo com a orientacao atual */
void goAhead() {
if (pose_atual[2] == LESTE)
pose_atual[0]++;
if (pose_atual[2] == NORTE)
pose_atual[1]++;
if (pose_atual[2] == OESTE)
pose_atual[0]--;
if (pose_atual[2] == SUL)
pose_atual[1]--;
andar(PULSOS_DISTANCIA_30CM, FRENTE);
ao();
}
/* Esta funcao deve fazer o seu robo girar 90 graus para a esquerda e tambem deve atualizar a variavel pose_atual */
void turnLeft() {
pose_atual[2] += 90;
if (pose_atual[2] == 360)
pose_atual[2]= 0;
andar(PULSOS_VIRAR_90GRAUS, ESQUERDA); ao();
}
/* Esta funcao deve fazer o seu robo girar 90 graus para a direita e tambem deve atualizar a varivael pose_atual */
void turnRight() {
pose_atual[2] -= 90;
if (pose_atual[2] == -90)
pose_atual[2]= 270;
andar(PULSOS_VIRAR_90GRAUS, DIREITA);
ao();
}
void push(int x, int y, int valor) {
if (x >= 0 && x < X_DIM && y >=0 && y < Y_DIM && map[x][y] == 0) {
//printf("fifo[%d] <= [%d %d]\n", indice, x, y);
map[x][y] = valor;
fifo[indice][0] = x;
fifo[indice][1] = y;
indice++;
}
}
void pop(int *x, int *y) { x = fifo[indice2][0]; y = fifo[indice2][1]; indice2++; }
/* Esta funcao deve calcular o mapa de distancias usando o algoritmo wavefront. A variavel map deve conter as distancias entre o ponto final e inicial ao final da sua execucao */
void calculaWaveFront() {
int x,y;
int i,j;
int stop = 0;
for (i = 0; i < X_DIM; i++) {
map[i][0] = MAX;
map[i][Y_DIM - 1] = MAX;
}
for (j = 0; j < Y_DIM; j++) {
map[0][j] = MAX;
map[X_DIM- 1][j] = MAX;
}
push(pose_desejada[0], pose_desejada[1], 1);
x = pose_desejada[0], y = pose_desejada[1];
while (!stop) {
pop(&x , &y);
push(x, y - 1, map[x][y] + 1);
push(x, y + 1, map[x][y] + 1);
push(x + 1, y, map[x][y] + 1);
push(x - 1, y, map[x][y] + 1);
if (x == pose_inicial[0] && y == pose_inicial[1])
stop = 1;
}
for (j = 0; j < Y_DIM; j++)
for (i = 0; i < X_DIM; i++)
if (map[i][j] == 0) map[i][j] = MAX;
}
/* Esta funcao deve retornar a direcao que o robo deve seguir para sair do quadrante atual para chegar ate o proximo quadrante, obedecendo o mapa criado pelo algoritmo wavefront */
int calculaDirecao() {
int x, y, xa, ya, xl, yl, xr, yr;
x = pose_atual[0];
y = pose_atual[1];
xa = x; xl = x;
xr = x; ya = y;
yl = y; yr = y;
if (pose_atual[2] == LESTE) {
xa = x + 1;
yl = y + 1;
yr = y - 1;
}
if (pose_atual[2] == NORTE) {
ya = y + 1;
xl = x - 1;
xr = x + 1;
}
if (pose_atual[2] == OESTE) {
xa = x - 1;
yl = y - 1;
yr = y + 1;
}
if (pose_atual[2] == SUL) {
ya = y - 1;
xl = x + 1;
xr = x - 1;
}
if (map[xa][ya] < map[x][y])
return FRENTE;
if (map[xl][yl] < map[x][y])
return ESQUERDA;
if (map[xr][yr] < map[x][y])
return DIREITA;
}
/* Imprime o mapa na tela. util para realizar a depuracao (so funciona no PC). */
void printMap() {
int i, j;
for (j = 0; j < Y_DIM; j++) {
for (i = 0; i < X_DIM; i++) {
printf("%3d ", map[i][j]);
}
printf("\n");
}
}
void escolhePosicao() {
int posicao;
pose_inicial[0] = getKnob("X inicial: ", X_DIM);
pose_inicial[1] = getKnob("Y inicial: ", Y_DIM);
pose_atual[0] = pose_inicial[0];
pose_atual[1] = pose_inicial[1];
pose_desejada[0] = getKnob("X desejado: ",X_DIM);
pose_desejada[1] = getKnob("Y desejado: ",Y_DIM);
}
void escolheMapa() {
int x, y;
int n, i;
n = getKnob("Numero de obstaculos: ", 20);
for (i = 0; i < n; i++) {
x = getKnob("X obstaculo: ",X_DIM);
y = getKnob("Y obstaculo: ",Y_DIM);
map[x][y] = MAX;
}
}
int wavefront() {
int direcao;
int stop = 0;
printf("Wavefront\n");
msleep(1000L);
escolhePosicao();
escolheMapa();
pose_inicial[2] = 0;
printf("Comecando em [%d %d], alvo: [%d %d]\n",
pose_inicial[0], pose_inicial[1],
pose_desejada[0], pose_desejada[1]);
calculaWaveFront();
//printMap();
while (!stop) {
direcao = calculaDirecao();
if (direcao == ESQUERDA)
turnLeft();
if (direcao == DIREITA)
turnRight();
goAhead();
printf("Estou em [%d %d]\n", pose_atual[0], pose_atual[1]);
/* Verifica se ja chegou no alvo*/
if (pose_atual[0] == pose_desejada[0] &&
pose_atual[1] == pose_desejada[1])
stop = 1;
}
return 1;
}