void roomba_t::setup(int BRC_pin)
{
// Bring baud rate control (BRC) pin low
// This wakes up the Open Interface so it listens for serial
pinMode(BRC_pin,OUTPUT);
digitalWrite(BRC_pin,HIGH); // BRC seems to be edge-triggered
roomba_delay(ROOMBA_SYNC_TIME);
digitalWrite(BRC_pin,LOW);
for (int attempt=0;attempt<10;attempt++)
{
sensor_packet_m.mode=0;
roomba_delay(ROOMBA_SYNC_TIME);
start();
set_mode(roomba_t::FULL);
set_led_clean(255,0xff); // orange == 128
led_update();
set_receive_sensors(true);
for (int wait=0;wait<20;wait++) {
update();
if (sensor_packet_m.mode!=0) { // success!
digitalWrite(BRC_pin,HIGH);
return;
}
roomba_delay(100);
}
// Well, that didn't work. Try reset command.
reset();
}
digitalWrite(BRC_pin,HIGH);
}
Roomba* roomba_init( const char* portpath )
{
int fd = roomba_init_serialport( portpath, B57600 );
if( fd == -1 ) return NULL;
uint8_t cmd[1];
cmd[0] = 128; // START
int n = write(fd, cmd, 1);
if( n!=1 ) {
perror("open_port: Unable to write to port ");
return NULL;
}
roomba_delay(COMMANDPAUSE_MILLIS);
cmd[0] = 130; // CONTROL
n = write(fd, cmd, 1);
if( n!=1 ) {
perror("open_port: Unable to write to port ");
return NULL;
}
roomba_delay(COMMANDPAUSE_MILLIS);
Roomba* roomba = calloc( 1, sizeof(Roomba) );
roomba->fd = fd;
strcpy(roomba->portpath, portpath);
roomba->velocity = DEFAULT_VELOCITY;
return roomba;
}
void roomba_t::set_mode(const mode_t& mode)
{
uint8_t id=ROOMBA_ID_PASSIVE_MODE;
if(mode==SAFE)
id=ROOMBA_ID_SAFE_MODE;
else if(mode==FULL)
id=ROOMBA_ID_FULL_MODE;
serial_m->write(&id,1);
roomba_delay(ROOMBA_SYNC_TIME);
}
int main(int argc, char *argv[])
{
char* serialport;
speed_t baud = B57600;
fprintf(stderr, argv[0]);
if ((!strncmp((argv[0]+strlen(argv[0])-13), "simpletest500",13))) {
fprintf(stderr,"roomba 500 series\n");
baud = B115200; // running as simpletest500 sets baud rate to 115200
}
if( argc>1 && strcmp(argv[1],"-p" )==0 ) {
serialport = argv[2];
} else {
usage();
}
roombadebug = 1;
Roomba* roomba = roomba_init( serialport, baud );
roomba_forward( roomba );
roomba_delay(1000);
roomba_backward( roomba );
roomba_delay(1000);
roomba_spinleft( roomba );
roomba_delay(1000);
roomba_spinright( roomba );
roomba_delay(1000);
roomba_stop( roomba );
roomba_close( roomba );
roomba_free( roomba );
return 0;
}
void roomba_t::reset()
{
uint8_t id=ROOMBA_ID_RESET;
serial_m->write(&id,1);
roomba_delay(ROOMBA_RESET_TIME);
}
void roomba_t::stop()
{
uint8_t id=ROOMBA_ID_STOP;
serial_m->write(&id,1);
roomba_delay(ROOMBA_SYNC_TIME);
}
