CAPÍTULO 3: DESENVOLVIMENTO DO AMBIENTE K++
4.4. Programação avançada usando o Visual C++
Além das funcionalidades do ambiente de programação K++, a classe
Khepera usada no ambiente permite também que outras aplicações possam ser
desenvolvidas usando o Visual C++. A grande vantagem desta abordagem está no fato de que a biblioteca de operações do K++ pode ser expandida de acordo com a necessidade das aplicações, bastando para isso, o conhecimento do programador para desenvolver novas operações. Apesar do K++ ser um software aberto, fazer alterações em suas bibliotecas requer uma certa habilidade do programador. Mas o fato do K++ ter sido desenvolvido usando os conceitos da programação orientada a objetos permite através do reuso de código, o desenvolvimento de novas operações de maneira bastante produtiva.
Neste sentido, o exemplo a seguir mostra uma aplicação complexa usando a classe Khepera. O objetivo aqui é mostrar que é possível criar algoritmos para aplicações complexas a partir das operações já definidas pela classe Khepera. A figura 4.16 ilustra o exemplo em que o robô irá se deslocar até um dado ponto P.
Figura 4.16. O robô se move até o ponto P
Também é possível observar na figura 4.16 que caso haja algum obstáculo o robô irá desviar da trajetória desse obstáculo e seguir novamente em direção ao ponto P. O algoritmo dessa aplicação é apresentado em forma de fluxograma na figura 4.17, o que facilita a compreensão do mesmo.
Figura 4.17. Fluxograma da aplicação que move robô até o ponto P
Inicialmente, o robô é colocado em um determinado ponto do ambiente dado pela coordenada (0,0). Neste exemplo, o ponto P fica acima e a direita do ponto inicial, assim o robô girará para o lado direito até apontar sua frente em direção ao ponto P, e em seguida irá se locomover até atingir o ponto final. Nesse exemplo nota-se a existência de um obstáculo na trajetória do robô. Portanto, para atingir o
ponto P, o robô precisa primeiro se desviar deste obstáculo, para depois seguir até o ponto final. Neste caso, quanto o robô encontra o obstáculo, pára, gira 90° para direita e anda a distância equivalente ao seu diâmetro, com o objetivo de tirá-lo da trajetória do obstáculo.
O próximo passo é calcular a nova posição do robô e determinar qual o ângulo de giro que o robô deve efetuar para que sua frente aponte novamente para o ponto P. Feito isso, o robô irá se locomover novamente em direção ao ponto final. Vale lembrar que, caso haja um novo obstáculo à frente do robô, ele irá repetir todo o procedimento de desvio e cálculo de posição atual até que o robô atinja seu alvo, o ponto P.
Nesta aplicação o cálculo da posição atual do robô é baseado somente na leitura dos encoders das rodas através do uso do método ReadMotorPosition. A dificuldade de basear-se somente na leitura dos encoders está no fato de que, por exemplo, quando o robô encontra um obstáculo, ele girará sobre seu eixo, fazendo com que o valor dos encoders sejam alterados, sendo que o robô não saiu do seu lugar geométrico. Portanto, foi necessária uma série de cálculos e considerações para resolução deste problema. De qualquer forma, os resultados obtidos com o uso deste algoritmo atingiram o objetivo principal, que era o desenvolvimento de aplicações complexas usando os métodos da classe Khepera. No anexo II, é possível observar a complexidade do algoritmo que executa esta aplicação.
É importante salientar que, dependendo da habilidade do programador, a classe Khepera pode ser adaptada para ser utilizada em alguma outra linguagem de programação como Java, por exemplo, que é uma linguagem extremamente usada para desenvolver aplicações para Internet. Neste sentido, uma aplicação desenvolvida em Java e que use as classes Khepera e Serial poderia controlar um robô Khepera através da Internet. Vale lembrar também que a classe Serial desenvolvida para fazer a comunicação com o robô Khepera através da porta serial, pode ser usada para qualquer outro tipo de robô que também use este tipo de interface.
Desta forma, o uso das classes Khepera e Serial permite expandir o universo de aplicações para robôs móveis, seja através da criação de novas operações no ambiente K++ ou pelo uso dessas classes em alguma outra linguagem de programação.
CONCLUSÃO
Nos dias atuais, ainda se perde muito tempo no estudo e desenvolvimento de aplicações para robôs móveis. Assim, o presente trabalho contempla uma real necessidade no desenvolvimento de softwares específicos para a área de robótica móvel. Com o uso de um ambiente de programação como o K++, que incorpora a produtividade da programação orientada a objetos e a versatilidade da programação visual, a distância existente entre as etapas do projeto podem ser minimizadas.
Além disso, a utilização deste ambiente estende seus benefícios a um grupo de pessoas que necessariamente não precisam ser especialistas em programação, ampliando desta forma a utilização da ferramenta. Isto se deve ao fato de que o ambiente K++ possui uma interface amigável, que facilita a programação e o desenvolvimento de aplicações para o robô khepera. A escolha da metodologia orientada a objetos e da linguagem de programação C++, atendeu as necessidades do projeto, que levou em consideração a capacidade de reusar códigos.
Outro aspecto importante do K++ é a possibilidade de desenvolver novas ferramentas para trabalhos futuros baseados em robôs programados em C, através da modelagem de uma nova classe que contemple os aspectos funcionais deste robô, ampliando o universo de aplicações para diferentes tipos de robôs.
Também é importante salientar que os resultados obtidos nos testes com o ambiente de programação K++ foram satisfatórios. Através do desenvolvimento das aplicações para o robô, as vantagens do uso da programação visual e da programação orientada a objetos tornaram-se evidentes.
Melhorias futuras e possíveis desdobramentos
A facilidade de criar aplicações é resultado de uma boa interface visual e de um bom projeto de software. Apesar disto, o ambiente K++ possui ainda algumas limitações que dificultam o desenvolvimento de aplicações mais complexas. Atualmente, o ambiente suporta somente dados dos tipos real e inteiro. Apesar de estar projetado para trabalhar com dados booleanos e vetores, pequenas modificações devem ser feitas para que o ambiente possa trabalhar com esses outros tipos de dados. Apesar de não ter sido comentado durante o texto, o ambiente K++ ainda permite converter parcialmente o programa criado visualmente em um código C, faltando ainda ajustar o mecanismo de tradução. Como o objetivo deste trabalho era efetuar a controlar o robô em tempo real, não houve preocupação em gerar o código C da aplicação. Em um trabalho futuro, a implementação da geração do código C permitirá que o programa simulado no robô seja enviado a sua memória para que o mesmo trabalhe em modo autônomo.
Outra importante melhoria pode ser obtida através da inclusão de novos elementos ao robô como visão, atuadores (garras) e rádio-controle. A inclusão de novos elementos ao ambiente K++ é uma relativamente complexa e exige um bom conhecimento do ambiente Visual C++ e da estrutura interna do K++. De qualquer maneira, a grande vantagem de um ambiente como o K++ é o fato de ser um software que permite que o programador possa modificá-lo de acordo com as suas necessidades.
Por fim, um importante desdobramento deste trabalho é a sua aplicação como ferramenta de apoio ao ensino de programação. O uso de um ambiente de programação visual para robôs móveis pode facilitar a compreensão dos algoritmos, melhorando desta forma o aprendizado, pois concilia os conceitos de programação com uma aplicação do mundo real.
REFERÊNCIAS BIBLIOGRÁFICAS
ASIMOV, I., 1994, “Visões de robô”, São Paulo: Círculo do Livro, 424 p.
BIANCHI, R.A.C., Simões, A.S., Costa, A.H.R., 2001, “Comportamentos Reativos para Seguir Pistas em Robô Móvel Guiado por Visão”, V Simpósio Brasileiro de Automação Inteligente.
BOTELHO, S. S. C., 1996, “Desenvolvimento de sistemas inteligentes para controle de robôs móveis”, Dissertação de mestrado em Ciências da Computação – Instituto de Informática, Universidade Federal do Rio Grande do Sul, Porto Alegre, 124 p.
BRAITENBERG, V., 1984, “Vehicles: experiments in synthetic psychology”, Cambridge, MIT Press, 152 p.
BRUMITT, B. L., Coulter, R. C., Stentz, A., 1992, “Dynamic trajectory planning for a cross-country navigator”, Boston Proceedings of SPIE Symposium on Mobile Robots.
BURNETT, M. M., et al, 1995. Visual Object-Programming: Concepts and Environments, Prentice Hall.
CALLONI, B. A., Bargert, D. J., 1994, “Iconic programming in BACCII vs Textual Programming: which is a better Learning Environment?”, SIGCSE Bulletin vol. 26, n° 1, pp. 188-192.
CAMPBELL, J., 1993, “C Programmer's Guide to Serial Communication”, Sams, pp. 407-598.
CYBERBOTICS, 2003, “Webots Reference Manual”, Cyberbotics, Disponível Em http://
cyberboticspc1.epfl.ch/cdrom/common/doc/webots/reference/reference.pdf
DI PAOLO E. A.,1998, “An investigation into the evolution of communication”, Adaptive Behavior journal, vol. 6, n° 2, pp. 285-324
FIRBY, R. James, 1993, “An architecture for a synthetic vacuum cleaner”, AAAI Fall Symposium, Series Workshop on instantiating realworlds agents. 9 p.
FLOREANO, D., Mondana, F., 1996, “Evolution of homing navigation in a real mobile robot”, IEEE Transactions on Systems, Man, and Cybernetics – Part B, vol. 26, pp. 396-407
GROOVER, M. P., WEISS, M., NAGEL, R. N., et al, 1988, “Robótica: tecnologia e programação”, São Paulo: McGraw-Hill, 401 p.
GUDWIN, R. R., 1997, “Linguagens de programação”, Notas de aula, 17 p.
Disponível em http://www.eng.uerj.br/~araujo/disciplinas/Caract/ling_prog.pdf HARVEY, B., 1997, “Computer Science Logo Style”, MIT Press, vol. 1, Cambridge,
pp. 17-39
HOLZNER, S., 2001, “C++ Black Book”, Makron Books, 646 p.
JONES, M. J., 2001, “Fundamentos do desenho orientado a objeto com UML”, Makron Books, 462 p.
KRUGLINSKI, D. J., 1996, “Inside Visual C++”, Microsoft Press, 843 p.
K-TEAM, 1999a, “Khepera BIOS Manual”, K-Team, Switzerland, 121 p. Disponível em http://www.k- team.com/download/khepera.html
K-TEAM, 1999b, “Khepera User Manual”, K-Team, Switzerland, 52 p. Disponível em http://www.k- team.com/download/khepera.html
K-TEAM, 2002a, “IR Sensors Report”, K-Team, Switzerland, 19 p. Disponível em http://www.k-team.com/download/khepera.html
K-TEAM, 2002b, “Khepera Programming Manual”, K-Team, Switzerland, 48 p. Disponível em http://www.k- team.com/download/khepera.html
LEE, R. C., Tepfenhart, W. M., 2002. “UML and C++ - Guia prático de desenvolvimento orientado a objeto”, Makron Books, pp. 23-39.
LUND, H.H., Cuenta, E.V., Hallan, J., 1996, “A Simple Mobile Real-team Robot Tracking System”, Technical Paper 41, Department Artificial of Intelligence, University of Edinburgh.
MÄCHLER P., 1995, "Detection Technologies for Anti-Personnel Mines", Autonomous Vehicles in Mine Countermeasures Symposium, Monterey, California, pp. 150-154.
MAIOR, D. S., 1998, “Os robôs estão chegando”, Ciência Hoje - suplemento tecnologia, vol. 23, n°136.
MONDANA, F., Wrinkled, E., Ienne, P., 1993, “Mobile Robot Miniaturization: A Tool For Investigation In Control Algorithms”, Experimental Robotics III - Proceedings of the Third International Symposium on Experimental Robotics, October 28-30, Kyoto,Japan, pp. 501-513
MONDANA, F., Floreano, D., 1996, “Evolution and mobile autonomous robotics”, Towards Evolvable Hardware; The Evolutionary Engineering Approach, Berlin, pp. 221-249
NASSI, I., Shnneiderman, B., 1984, “Flowchart Techniques for Structured Programming”, ACM Sigplan Notices, Vol. 8, n° 8, p. 723-728.
NATIONAL, 2000, “LABVIEW® Manuals”, National Instruments, 272 p
NITZAN, D., 1985, “Development of intelligent robots: achievements and issues”, IEEE Journal of Robotics and Automation, vol.1, n°1, pp. 3-13.
NOVELETTO F., Sousa, A. H., Hounsell, M. S., 2003, “Desenvolvimento de um Ambiente de Programação Visual para Robôs Móveis”, III Congresso Brasileiro de Computação – CBComp, pp. 914-924.
NOVELETTO F., Sousa, A. H., Hounsell, M. S., 2003, “Desenvolvimento de um Ambiente de Programação Visual para Robôs Móveis”, I2TS'2003 – International Information and Telecommunication Technologies Symposium – (IEEE Seção Sul Brasil) – Workshop III CBComp Best Papers.
NOVELETTO F., Sousa, A. H., Hounsell, M. S., 2003, “The development of a visual programming environment for mobile robots”, COBEM-2003 – 17° Congresso Brasileiro de Engenharia Mecânica .
PAPERT, S., 1985, “Logo: Computadores e Educação”, Editora Brasiliense, 254 p. PRESSMAN, R. S., 1995, “Engenharia de Software”, Makron Books, 1056 p
RAJEEV K. P., Burnett, M. M., 1993, “Is It Easier to Write Matrix Manipulation Programs Visually or Textually? An Empirical Study”, Proceedings of the 1993
IEEE Symposium on Visual Languages, pp. 344-351.
RUSSEL, S. J., Norvig, P.,1995, “Artificial Intelligence: A Modern Approach”, Prentice Hall, 932 p.
SCALAN, D. A., 1989, “Structured Flowcharts Outperform Pseudocodes: An Experimental Comparation”, IEEE Software vol. 6, n° 5, pp. 28-36
SETHI, R., 1996, "Programming Languages: Concepts and Constructs", Second edition, Addison – Wesley, 624 p.
SHU N.C., 1988. Visual Programming, Van Nostrand Reinhold Company Inc.,New York.
SOUSA, A. H., Ferreira, E. C., Silva, M. A. Vieira, 1996a, “ONAGRO – A Graphical Environment to the Development of Microcontrollers Software”, Proceedings of WAC-96: Second World Automation Congress, Montpellier-France, p. 301 – 306 SOUSA, A. H., Ferreira, E. C., 1998, “O++: A Visual Object-Oriented Language for
Embedded Systems”, Proceedings of ISSCI-98: International Symposium on Soft Computing for Industry, Achorage/AK/USA.
SOUSA, A. H., “Uma proposta de linguagem visual orientada a objetos para programação de microcontroladores”, Campinas, 1999, 116 p. Tese de Doutorado – Engenharia Elétrica – Universidade Estadual de Campinas.
SPRINGER, S. P., Deutsch, G., 1985, “Left Brain, Right Brain”, W. H. Freeman and Company, New York
VOSS, G, 1991, “Object-Oriented Programming: An Introduction”, McGraw-Hill, 584 p WASSERMAN, A. I., et al, 1990, “The Object-Oriented Structured Design Notation for
Software Design Representation”, IEEE Computer, vol. 23, n° 3, p 50 – 63 WHITLEY K. N., 1996, “Visual Programming Languages and the Empirical Evidence
for and Against”, Journal of Visual Languages and Computing, vol. 8, 19 p
WINBLAND, A. L., Edwards, S. D., King, D. R., 1993, “Software Orientado ao Objeto”, Makron Books, 314 p.
ANEXO I
Implementação da classe Serial em C++ Arquivo: serial.cpp
ARQUIVO SERIAL.CPP
// Serial.cpp
// by ANTONIO HERONALDO DE SOUSA E FABRICIO NOVELETTO // FACULDADE DE ENGENHARIA DE JOINVILLE
// SETEMBRO 2002 #include "stdafx.h" #include "Serial.h" CSerial::CSerial() {
memset( &m_OverlappedRead, 0, sizeof( OVERLAPPED ) ); memset( &m_OverlappedWrite, 0, sizeof( OVERLAPPED ) );
m_hIDComDev = NULL; m_bOpened = FALSE; } CSerial::~CSerial() { Close(); }
BOOL CSerial::Open( int nPort, int nBaud ) {
if( m_bOpened ) return( TRUE ); char szPort[15];
char szComParams[50]; DCB dcb;
wsprintf( szPort, "COM%d", nPort );
m_hIDComDev = CreateFile( szPort, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, NULL );
if( m_hIDComDev == NULL ) return( FALSE );
memset( &m_OverlappedRead, 0, sizeof( OVERLAPPED ) ); memset( &m_OverlappedWrite, 0, sizeof( OVERLAPPED ) ); COMMTIMEOUTS CommTimeOuts; CommTimeOuts.ReadIntervalTimeout = 0xFFFFFFFF; CommTimeOuts.ReadTotalTimeoutMultiplier = 0; CommTimeOuts.ReadTotalTimeoutConstant = 0; CommTimeOuts.WriteTotalTimeoutMultiplier = 0; CommTimeOuts.WriteTotalTimeoutConstant = 5000; SetCommTimeouts( m_hIDComDev, &CommTimeOuts ); wsprintf( szComParams, "COM%d:%d,n,8,1", nPort, nBaud );
m_OverlappedRead.hEvent = CreateEvent( NULL, TRUE, FALSE, NULL ); m_OverlappedWrite.hEvent = CreateEvent( NULL, TRUE, FALSE, NULL ); dcb.DCBlength = sizeof( DCB );
GetCommState( m_hIDComDev, &dcb ); dcb.BaudRate = nBaud;
dcb.ByteSize = 8;
unsigned char ucSet;
ucSet = (unsigned char) ( ( FC_RTSCTS & FC_DTRDSR ) != 0 ); ucSet = (unsigned char) ( ( FC_RTSCTS & FC_RTSCTS ) != 0 ); ucSet = (unsigned char) ( ( FC_RTSCTS & FC_XONXOFF ) != 0 ); if( !SetCommState( m_hIDComDev, &dcb ) ||
!SetupComm( m_hIDComDev, 10000, 10000 ) || m_OverlappedRead.hEvent == NULL ||
m_OverlappedWrite.hEvent == NULL ){ DWORD dwError = GetLastError();
if( m_OverlappedRead.hEvent != NULL ) CloseHandle( m_OverlappedRead.hEvent ); if( m_OverlappedWrite.hEvent != NULL ) CloseHandle( m_OverlappedWrite.hEvent ); CloseHandle( m_hIDComDev ); return( FALSE ); } m_bOpened = TRUE; return( m_bOpened ); }
BOOL CSerial::Close( void ) {
if( !m_bOpened || m_hIDComDev == NULL ) return( TRUE );
if( m_OverlappedRead.hEvent != NULL ) CloseHandle( m_OverlappedRead.hEvent ); if( m_OverlappedWrite.hEvent != NULL ) CloseHandle( m_OverlappedWrite.hEvent ); CloseHandle( m_hIDComDev );
m_bOpened = FALSE; m_hIDComDev = NULL; return( TRUE );
}
BOOL CSerial::WriteCommByte( unsigned char ucByte ) {
BOOL bWriteStat;
DWORD dwBytesWritten;
bWriteStat = WriteFile( m_hIDComDev, (LPSTR) &ucByte, 1, &dwBytesWritten, &m_OverlappedWrite );
if( !bWriteStat && ( GetLastError() == ERROR_IO_PENDING ) ){
if( WaitForSingleObject( m_OverlappedWrite.hEvent, 1000 ) ) dwBytesWritten = 0; else{
GetOverlappedResult( m_hIDComDev, &m_OverlappedWrite, &dwBytesWritten, FALSE ); m_OverlappedWrite.Offset += dwBytesWritten; } } return( TRUE ); }
int CSerial::SendData( const char *buffer, int size ) {
if( !m_bOpened || m_hIDComDev == NULL ) return( 0 );
DWORD dwBytesWritten = 0; int i;
for( i=0; i<size; i++ ){
WriteCommByte( buffer[i] ); dwBytesWritten++;
}
return( (int) dwBytesWritten ); }
int CSerial::ReadDataWaiting( void ) {
if( !m_bOpened || m_hIDComDev == NULL ) return( 0 ); DWORD dwErrorFlags;
COMSTAT ComStat;
ClearCommError( m_hIDComDev, &dwErrorFlags, &ComStat ); return( (int) ComStat.cbInQue );
}
int CSerial::ReadData( void *buffer, int limit ) {
if( !m_bOpened || m_hIDComDev == NULL ) return( 0 ); BOOL bReadStatus;
DWORD dwBytesRead, dwErrorFlags; COMSTAT ComStat;
ClearCommError( m_hIDComDev, &dwErrorFlags, &ComStat ); if( !ComStat.cbInQue ) return( 0 );
dwBytesRead = (DWORD) ComStat.cbInQue;
if( limit < (int) dwBytesRead ) dwBytesRead = (DWORD) limit;
bReadStatus = ReadFile( m_hIDComDev, buffer, dwBytesRead, &dwBytesRead, &m_OverlappedRead );
if( !bReadStatus ){
if( GetLastError() == ERROR_IO_PENDING ){
WaitForSingleObject( m_OverlappedRead.hEvent, 2000 ); return( (int) dwBytesRead );
}
return( 0 ); }
return( (int) dwBytesRead ); }
int CSerial::ReadCommand(char *lpBuffer) {
time_t t0, t0a, t1, t1a; int szBuffer; int nBytesRead;
int ret, flag; char lpBuffer_aux[100]; lpBuffer[0]='\0'; lpBuffer_aux[0]='\0'; t0a = time(NULL); do { t0 = time(NULL); do { szBuffer=ReadDataWaiting(); t1 = time(NULL);
} while ((szBuffer==0) && ((t1-t0) < 2)); if (szBuffer!=0)
{
nBytesRead = ReadData(lpBuffer_aux, szBuffer); lpBuffer_aux[szBuffer]='\0'; strcat(lpBuffer,lpBuffer_aux); } szBuffer = strlen(lpBuffer); if (szBuffer > 0) { if (lpBuffer[szBuffer-1] == 10) { flag = 0; } else { flag = 1; } } else { flag = 1; } t1a = time(NULL); } while (flag && ((t1a-t0a) < 2) ); if (szBuffer > 2) { lpBuffer[szBuffer-2] = '\0'; //lpBuffer[szBuffer-2] = '\0'; ret = szBuffer - 2; } else ret = -1; return (ret); }
int CSerial::WriteCommand(char *lpBuffer, char *lpBuffer_ret) {
int ret;
char lpBuffer_trans[50]; int len = strlen(lpBuffer);
strcpy(lpBuffer_trans, lpBuffer); lpBuffer_trans[len] = 13; lpBuffer_trans[len+1] = 10; lpBuffer_trans[len+2] = '\0'; SendData(lpBuffer_trans, strlen(lpBuffer_trans)); ret = ReadCommand(lpBuffer_ret); if (ret != -1) { if (lpBuffer_ret[0] == (lpBuffer_trans[0])) //+32)) ret = 1; else ret = -1; } return(ret); } 114
ANEXO II
Implementação da classe Khepera em C++ Arquivo: khepera.cpp
ARQUIVO KHEPERA.CPP
// khepera.cpp
// by ANTONIO HERONALDO DE SOUSA E FABRICIO NOVELETTO // FACULDADE DE ENGENHARIA DE JOINVILLE
// SETEMBRO 2002 #include "stdafx.h" #include "khepera.h" #include <math.h> IMPLEMENT_SERIAL(CKhepera, CObject, 0); CKhepera::CKhepera() { m_prox = PROX_SATURATION; m_axis = AXIS_DISTANCE; m_encoder = ENCODER_RESOLUTION; m_light = LIGHT_SATURATION; } CKhepera::~CKhepera() { }
BOOL CKhepera::Open( int nPort, int nBaud ) {
return( serial.Open(nPort,nBaud)); }
BOOL CKhepera::Close( void ) {
return(serial.Close()); }
// ******************************************* // Move robô para frente
// *******************************************
void CKhepera::MoveForward(int Motor1, int Motor2) {
char lpBuffer_ret[50]; char buf[50];
char charMotor1[10], charMotor2[10]; _itoa(Motor1,charMotor1,10); _itoa(Motor2,charMotor2,10); strcpy(buf, "D,"); strcat(buf, charMotor1); strcat(buf, ","); strcat(buf, charMotor2); serial.WriteCommand(buf,lpBuffer_ret); } 116
// ******************************************* // Move robô para traz
// *******************************************
void CKhepera::MoveBackward(int Motor1, int Motor2) {
char lpBuffer_ret[50]; char buf[50];
char charMotor1[10], charMotor2[10]; _itoa(Motor1,charMotor1,10); _itoa(Motor2,charMotor2,10); strcpy(buf, "D,"); strcat(buf, charMotor1); strcat(buf, ","); strcat(buf, charMotor2); serial.WriteCommand(buf,lpBuffer_ret); } // ************************************************* // Para motores do robô
// ************************************************* void CKhepera::StopMotors() { char lpBuffer_ret[50]; lpBuffer_ret[0]='\0'; serial.WriteCommand("D,0,0",lpBuffer_ret); } // ************************************************* // Leitura dos contadores dos motores // *************************************************
void CKhepera::ReadMotorPosition(int* count_motors) { char lpBuffer_ret[50]; serial.WriteCommand("H",lpBuffer_ret); CString strCountMotors(lpBuffer_ret); CString Count_Motors_Str[10]; CString s1; CString s2(""); int z = 0; int x = strCountMotors.GetLength(); for( int i = 2; i < x; ++i )
{ s1 = strCountMotors.GetAt(i); if (s1 == ",") { z++; Count_Motors_Str[z] = s2; s2 = ""; } else s2 = s2 + s1; } 117
Count_Motors_Str[2] = s2; for( int ii = 1; ii < 3; ++ii ) {
count_motors[ii-1] = atoi(Count_Motors_Str[ii]); }
}
// ************************************************* // Leitura dos sensores de Proximidade // ************************************************* void CKhepera::ReadProxSensors(int* val_sensor) { char lpBuffer_ret[50]; serial.WriteCommand("N",lpBuffer_ret); CString strSensors(lpBuffer_ret); CString Val_Sensor_Str[10]; CString s1; CString s2(""); int z = 0; int x = strSensors.GetLength(); for( int i = 2; i < x; ++i )
{ s1 = strSensors.GetAt(i); if (s1 == ",") { z++; Val_Sensor_Str[z] = s2; s2 = ""; } else s2 = s2 + s1; } Val_Sensor_Str[8] = s2;
for( int ii = 1; ii < 9; ++ii ) // ???????? {
val_sensor[ii-1] = atoi(Val_Sensor_Str[ii]); }
}
// ************************************************* // Leitura dos sensores de luminosidade // ************************************************* void CKhepera::ReadLightSensors(int* val_sensor) { char lpBuffer_ret[50]; serial.WriteCommand("O",lpBuffer_ret); CString strSensors(lpBuffer_ret); CString Val_Sensor_Str[10]; CString s1; CString s2(""); int z = 0; int x = strSensors.GetLength(); for( int i = 2; i < x; ++i )
{ s1 = strSensors.GetAt(i); if (s1 == ",") { z++; Val_Sensor_Str[z] = s2; s2 = ""; } else s2 = s2 + s1; } Val_Sensor_Str[8] = s2;
for( int ii = 1; ii < 9; ++ii ) // ???????? {
val_sensor[ii-1] = atoi(Val_Sensor_Str[ii]); }
}
// ************************************************* // Gira robô à esquerda
// ************************************************* void CKhepera::TurnLeft(double degrad, int speed) {
int count_motors[10];
MoveForward(-speed,speed); ReadMotorPosition(count_motors);
int CountStop = (count_motors[1] + (int) (m_axis * degrad)); do { ReadMotorPosition(count_motors); } while(count_motors[1]<CountStop); StopMotors(); } 119
// ************************************************* // Gira robô à direita
// *************************************************
void CKhepera::TurnRight(double degrad, int speed) {
int count_motors[10];
MoveForward(speed,-speed); ReadMotorPosition(count_motors);
int CountStop = (count_motors[0] + (int)(m_axis * degrad)); do { ReadMotorPosition(count_motors); } while(count_motors[0]<CountStop); StopMotors(); } // ************************************************************ // Move robô por dada distância com dada velocidade // ************************************************************ void CKhepera::MoveLine(int distance, int angle, int speed) { int count_motors[10]; int val_sensor[10]; TurnRight(angle); MoveForward(speed,speed); ReadMotorPosition(count_motors);
int CountStop = (count_motors[0] + (int)(12.5 * distance)); do
{
ReadMotorPosition(count_motors); ReadProxSensors(val_sensor);
// Se existir obstaculo PARE!
// --- if (val_sensor[1]>m_prox || val_sensor[2]>m_prox || val_sensor[3]>m_prox || val_sensor[4]>m_prox) { StopMotors(); } else MoveForward(speed,speed); // --- } while (count_motors[0]<CountStop); StopMotors(); } 120
// ************************************************* // Move robô até um dado ponto P // *************************************************
void CKhepera::MoveToPoint(int Xfinal, int Yfinal, int* coordXY) {
int Contador, CountStop; int x_ref, y_ref;
int count_motors[2]; int val_sensor[8];
int flagStop = 0;
double eixoX, eixoY, distance, angleDeg, angleRad; double pi = 3.1415926535;
int x_atual = 0; int y_atual = 0;
do {
eixoX = (double) Xfinal - x_atual; eixoY = (double) Yfinal - y_atual;
distance = sqrt(eixoX*eixoX + eixoY*eixoY); angleRad = atan(eixoY / eixoX);
angleDeg = (angleRad*180.0 / pi); if (eixoX > 0) { TurnRight(90 - angleDeg); } else { TurnLeft(90 - angleDeg); } ResetMotors(); MoveForward(5,5);
CountStop = (int)(12.5 * distance); do
{
ReadMotorPosition(count_motors); ReadProxSensors(val_sensor); Contador = (int)(count_motors[0]*m_encoder);
x_ref = (int)(Contador * cos(angleRad)); y_ref = (int)(Contador * sin(angleRad)); }
while ( (count_motors[0]<CountStop) &&
(val_sensor[0]<m_prox) && (val_sensor[1]<m_prox) && (val_sensor[2]<m_prox) &&
(val_sensor[3]<m_prox) && (val_sensor[4]<m_prox) && (val_sensor[5]<m_prox));
if (x_atual > Xfinal)
x_atual = x_atual - x_ref; else
x_atual = x_atual + x_ref; if (y_atual > Yfinal)
y_atual = y_atual - y_ref; else
y_atual = y_atual + y_ref; if (count_motors[0]>=CountStop) StopMotors();
else {
int sentido_giro = -1;
int left_sensors = val_sensor[0]+val_sensor[1]+val_sensor[2]; int right_sensors = val_sensor[3]+val_sensor[4]+val_sensor[5]; if (left_sensors > right_sensors)
{
sentido_giro = 1; // gira para direita TurnRight(90);
} else {
sentido_giro = 0; // gira para esquerda TurnLeft(90); } int sair = 0; ResetMotors(); MoveForward(2,2); while (!sair) { ReadProxSensors(val_sensor); if ((sentido_giro == 1) && (val_sensor[0] < 1))
sair = 1;
if ((sentido_giro == 0) && (val_sensor[5] < 1)) sair = 1;
}
MoveLine(60,0,2); // anda o diametro do robo ReadMotorPosition(count_motors); Contador = (int)(count_motors[0]*m_encoder);
x_ref = (int)(Contador * sin(angleRad)); y_ref = (int)(Contador * cos(angleRad));
if (sentido_giro == 1) {
x_atual = x_atual + x_ref; y_atual = y_atual - y_ref;
TurnLeft(180 - angleDeg + 3);//ERRO = 3 }
else {
x_atual = x_atual - x_ref; y_atual = y_atual + y_ref; TurnRight(angleDeg);
} } }
while ( ((x_atual <= (0.9*Xfinal)) || (x_atual >= (1.1*Xfinal))) || ((y_atual <= (0.9*Yfinal)) || (y_atual >= (1.1*Yfinal))) ); StopMotors();
}
// ************************************************* // Zera os contadores dos motores // ************************************************* void CKhepera::ResetMotors() { char lpBuffer_ret[50]; serial.WriteCommand("G,0,0",lpBuffer_ret); } // ************************************************* // Código para Desvio de Obstáculo // ************************************************* void CKhepera::ObstacleSkipper(int val_angle) { int val_sensor[10]; do { ReadProxSensors(val_sensor); }
while ( val_sensor[0]<m_prox && val_sensor[1]<m_prox && val_sensor[2]<m_prox && val_sensor[3]<m_prox && val_sensor[4]<m_prox &&
val_sensor[5]<m_prox); StopMotors();
int leftSensors = val_sensor[0] + val_sensor[1] + val_sensor[2]; int rightSensors = val_sensor[3] + val_sensor[4] + val_sensor[5]; if (leftSensors < rightSensors) { TurnLeft(val_angle); } else { TurnRight(val_angle); } } 123
// ************************************************* // Código para encontrar luz
// ************************************************* int CKhepera::LightFinder(int val_sat) //int CKhepera::LightFinder() { int val_light; char lpBuffer_ret[50]; if (val_sat == -1) val_sat = m_light; serial.WriteCommand("O",lpBuffer_ret); CString strSensors(lpBuffer_ret); CString Val_Sensor_Str[10]; CString s1; CString s2(""); int z = 0; int x = strSensors.GetLength(); int val_sensor[8];
for( int i = 2; i < x; ++i ) { s1 = strSensors.GetAt(i); if (s1 == ",") { z++; Val_Sensor_Str[z] = s2; s2 = ""; } else s2 = s2 + s1; } Val_Sensor_Str[8] = s2;
for( int ii = 1; ii < 9; ++ii ) // ???????? {
val_sensor[ii-1] = atoi(Val_Sensor_Str[ii]); }
// Verifica qual sensor tem o menor valor (mais luz) int lightSensor=512;
int numSensor=0; for( int iii = 0; iii < 8; ++iii ) { if (val_sensor[iii] < lightSensor) { numSensor = iii; lightSensor = val_sensor[iii]; } } 124
if (lightSensor <= val_sat) val_light=1; else val_light=0; switch(numSensor) { case 0: TurnLeft(90); break; case 1: TurnLeft(45); break; case 2: TurnLeft(10); break; case 3: TurnRight(10); break; case 4: TurnRight(45); break; case 5: TurnRight(90); break; case 6: TurnRight(170); break; case 7: TurnLeft(170); break; } return (val_light); }
void CKhepera::SetValues(float a, float e, int l, int p) { m_axis = a; m_encoder = e; m_light = l; m_prox = p; }
void CKhepera::GetValues(float &a, float &e, int &l, int &p) { a = m_axis; e = m_encoder; l = m_light; p = m_prox; } 125
void CKhepera::Serialize(CArchive& ar) { if (ar.IsStoring()) { ar << (WORD) m_light; ar << (WORD) m_prox; ar << m_axis; ar << m_encoder; } else { WORD wTemp;
ar >> wTemp; m_light = (int)wTemp; ar >> wTemp; m_prox = (int)wTemp; ar >> m_axis;
ar >> m_encoder; }
}