int hokuyo_startContinuous(HokuyoURG* urg, int startStep, int endStep, int clusterCount){
if (! urg->isInitialized)
return -1;
if (urg->isContinuous)
return -1;
// switch on the laser
fprintf(stderr, "Switching on the laser emitter... ");
int status=hokuyo_readStatus(urg, HK_BEAM);
if (! status){
fprintf(stderr, "Ok\n");
} else {
fprintf(stderr, "Error. Unable to control the laser, status is %d\n", status);
return -1;
}
char command[1024];
sprintf (command, "\nMD%04d%04d%02d000\n", startStep, endStep, clusterCount);
status=hokuyo_readStatus(urg, command);
if (status==99 || status==0){
fprintf(stderr, "Continuous mode started with command %s\n", command);
urg->isContinuous=1;
return 1;
}
fprintf(stderr, "Error. Unable to set the continuous mode, status=%02d\n", status);
return -1;
}
int hokuyo_init(HokuyoLaser* hokuyoLaser){
if (hokuyoLaser->fd<=0){
return -1;
}
#ifdef HOKUYO_ALWAYS_IN_SCIP20
fprintf(stderr, "\nAssuming laser is already in SCIP2.0 mode!\n");
fprintf(stderr, " (if the dirver hangs, either your Hokuyo has the old\n");
fprintf(stderr, " firmware or you have to disable -DHOKUYO_ALWAYS_IN_SCIP20\n");
fprintf(stderr, " in the Makefile of the laser driver)\n");
hokuyoLaser->isContinuous=0;
hokuyoLaser->isProtocol2=1;
#else
// stop the device anyhow
fprintf(stderr, "Stopping the device... ");
write_command(hokuyoLaser,HK_QUIT);
write_command(hokuyoLaser,HK_QUIT);
write_command(hokuyoLaser,HK_QUIT);
fprintf(stderr, "done\n");
hokuyoLaser->isContinuous=0;
// The UTM-30LX functions only in SCIP2.0 Mode, as do some URG's
fprintf(stderr, "Switching to enhanced mode (SCIP2.0)... ");
fprintf(stderr, " (if the dirver hangs you probably configured your Hokuyo\n");
fprintf(stderr, " so that SCIP is alwas on. IN this case, enable the Option\n");
fprintf(stderr, " CFLAGS += -DHOKUYO_ALWAYS_IN_SCIP20\n");
fprintf(stderr, " in the Makefile of the laser driver)\n");
int status=hokuyo_readStatus(hokuyoLaser, HK_SCIP,1);
if (status==0){
fprintf(stderr, "\nSwitching to SCIP 2.0 was successful!\n");
hokuyoLaser->isProtocol2=1;
} else {
fprintf(stderr, "Error. Unable to switch to SCIP2.0 Mode, please upgrade the firmware of your device.\n");
return -1;
}
#endif
fprintf(stderr, "Device initialized successfully\n");
hokuyoLaser->isInitialized=1;
hokuyo_readStatus(hokuyoLaser, "\nVV\n", 1);
hokuyo_readStatus(hokuyoLaser, "\nPP\n", 1);
hokuyo_readStatus(hokuyoLaser, "\nII\n", 1);
return 1;
}
int hokuyo_reset(HokuyoURG* urg){
if (! urg->isInitialized)
return -1;
int status=hokuyo_readStatus(urg, HK_RESET);
if (status==0){
fprintf(stderr, "Ok\n");
urg->isContinuous=0;
} else {
fprintf(stderr, "Error. Unable to reset laser\n");
return -1;
}
return 1;
}
int hokuyo_reset(HokuyoLaser* hokuyoLaser){
if (! hokuyoLaser->isInitialized)
return -1;
int status=hokuyo_readStatus(hokuyoLaser, HK_RESET, 1);
if (status==0){
fprintf(stderr, "Ok\n");
hokuyoLaser->isContinuous=0;
} else {
fprintf(stderr, "Error. Unable to reset laser\n");
return -1;
}
return 1;
}
