koala.c

Go to the documentation of this file.

/*--------------------------------------------------------------------
 * koala.c - KoreBot Library - Koala Robot Support for KoreBot
 *--------------------------------------------------------------------
 * $Id: koala.c,v 1.1 2004/09/22 08:29:15 pbureau Exp $
 *--------------------------------------------------------------------
 * $Author: pbureau $
 * $Date: 2004/09/22 08:29:15 $
 * $Revision: 1.1 $
 *--------------------------------------------------------------------*/

#include "korebot.h"


char * koa_get_nextValue(char * current, char * value)
{
  while(*current != ',' && *current != '\n')
  {
    *value = *current;
    value++;
    current++;
  }

  if(*current == '\n')
    return NULL;
  else
  {
    *value = '\0';
    return current+1;
  }
}

/*--------------------------------------------------------------------*/
int koa_getOSVersion( knet_dev_t * dev , 
                      unsigned char *version , 
                      unsigned char *revision,
                      unsigned char *pversion,
                      unsigned char *prevision)
{
  char *p;
  char buf[16];
  int rc;

  if ((rc=knet_printf( dev , "B\r\n")) < 0 )
    return rc;

  if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
    return rc;

  /* bad acknowledge */
  if ( buf[0] != 'b' )
    return 0;

  p = &buf[2];

  if ( version != NULL ) {
    *version = atoi(p);
  }

  if ( revision != NULL ) {

    while (*p!='\0' && *p!='.' ) p++;
    if (*p == '.' ) {
      p++;
        *revision = atoi(p);
    }
  }

  if ( pversion != NULL ) {

    while (*p!='\0' && *p!=',' ) p++;
    if (*p == ',' ) {
      p++;
        *pversion = atoi(p);
    }
  }

  if ( prevision != NULL ) {

    while (*p!='\0' && *p!='.' ) p++;
    if (*p == '.' ) {
      p++;
        *prevision = atoi(p);
    }
  }

  return 0;
}

/*--------------------------------------------------------------------*/
int koa_setSpeed( knet_dev_t * dev , short int left , short int right )
{
  char buf[32];
  int rc;

  if ((rc=knet_printf( dev , "D,%d,%d\r\n" , left , right )) < 0 )
    return rc;

  if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
    return rc;

  /* bad acknowledge */
  if ( buf[0] != 'd' )
    return 0;

  return 1;
}

int koa_readProximity(knet_dev_t * dev, int * senstable)
{
  char buf[256];
  char val[8];
  char * scan;
  int rc;

  if ((rc=knet_printf( dev , "N\r\n" )) < 0 )
    return rc;

  if ((rc=knet_read_string( dev , buf , sizeof(buf)-1 , '\n' )) < 0 )
    return rc;

  /* bad acknowledge */
  if ( buf[0] != 'n' )
    return 0;

  scan = buf+2;
  while(scan) {
    scan       = koa_get_nextValue(scan,val);
    *senstable = atoi(val);
    senstable++;
  }
  
  return 1;
}

---