AREXPORT bool ArUrg::disconnect(void)
{
if (!isConnected())
return true;
ArLog::log(ArLog::Normal, "%s: Disconnecting", getName());
bool ret;
char buf[2048];
sprintf(buf, "S%06d7654321", atoi(getStartingBaudChoice()));
ret = writeLine(buf);
laserDisconnectNormally();
return ret;
}
AREXPORT bool ArUrg_2_0::blockingConnect(void)
{
if (!getRunning())
runAsync();
myConnMutex.lock();
if (myConn == NULL)
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because there is no connection defined",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
// if we have a starting baud and are a serial port, then change the
// baud rate... not by default this will set it to 0 baud which'll
// cause the serial stuff not to touch it
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN &&
!myConn->openSimple())
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because the connection was not open and could not open it",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserPullUnsetParamsFromRobot();
laserCheckParams();
ArUtil::sleep(100);
bool connected = false;
if (internalConnect())
connected = true;
if (connected)
{
lockDevice();
myIsConnected = true;
myTryingToConnect = false;
unlockDevice();
ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
laserConnect();
return true;
}
else
{
failedToConnect();
return false;
}
}
AREXPORT bool ArUrg::blockingConnect(void)
{
if (!getRunning())
runAsync();
myConnMutex.lock();
if (myConn == NULL)
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because there is no connection defined",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN &&
!myConn->openSimple())
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because the connection was not open and could not open it",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserPullUnsetParamsFromRobot();
laserCheckParams();
setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped());
ArUtil::sleep(100);
bool connected = false;
if (internalConnect())
connected = true;
if (connected)
{
lockDevice();
myIsConnected = true;
myTryingToConnect = false;
unlockDevice();
ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
laserConnect();
return true;
}
else
{
failedToConnect();
return false;
}
}
AREXPORT bool ArSZSeries::blockingConnect(void) {
if (!getRunning())
runAsync();
myConnMutex.lock();
if (myConn == NULL) {
ArLog::log(ArLog::Terse,
"%s: Could not connect because there is no connection defined",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
// myPrevSensorIntTime = myConn->getTimeRead(0);
// PS 9/9/11 - moved this here to fix issue with setting baud in mt400.p
laserPullUnsetParamsFromRobot();
laserCheckParams();
// PS 9/9/11 - add setting baud
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN
&& !myConn->openSimple()) {
ArLog::log(
ArLog::Terse,
"%s: Could not connect because the connection was not open and could not open it",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
// PS - set logging level and laser type in packet receiver class
myReceiver.setmyInfoLogLevel(myInfoLogLevel);
myReceiver.setmyName(getName());
myReceiver.setDeviceConnection(myConn);
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
// PS 9/9/11 - moved up top
//laserPullUnsetParamsFromRobot();
//laserCheckParams();
int size = ArMath::roundInt((270/.3) + 1);
ArLog::log(myInfoLogLevel,
"%s::blockingConnect() Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArTime timeDone;
if (myPowerControlled)
{
if (!timeDone.addMSec(60 * 1000))
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect() error adding msecs (60 * 1000)",
getName());
}
}
else
{
if (!timeDone.addMSec(30 * 1000))
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect() error adding msecs (30 * 1000)",
getName());
}
}
ArSZSeriesPacket *packet;
ArSZSeriesPacket sendPacket;
#if 0
sendPacket.empty();
sendPacket.uByteToBuf(0xA0); // stop continous sending
sendPacket.uByteToBuf(0x00);
sendPacket.uByteToBuf(0x1D);
sendPacket.uByteToBuf(0x7E);
sendPacket.finalizePacket();
if ((myConn->write(sendPacket.getBuf(), sendPacket.getLength())) == -1)
{
ArLog::log(ArLog::Terse,
"%s::blockingConnect() Could not send Stop Continuous mode to laser", getName());
failedToConnect();
return false;
}
#endif
// Build the Start Continuous sending packet and set it
// once we get a response, then we are connected, note
// the response needs to be a real reading
sendPacket.empty();
// command id = 0x91
//sendPacket.uByteToBuf(145);
sendPacket.uByteToBuf(0x91);
// note communication ID default is 0
// this value is set via the SZ Configurator
// ???? not sure what to do if it fails
// and put in CRC - from manual, this is
// specific to the communication ID = 0
sendPacket.uByteToBuf(0);
sendPacket.uByteToBuf(43);
sendPacket.uByteToBuf(218);
unsigned short crc = myReceiver.CRC16((unsigned char *)sendPacket.getBuf(), 2);
#if 0
// other communications IDs and CRC
// communication ID =1
sendPacket.uByteToBuf(1);
sendPacket.uByteToBuf(59);
sendPacket.uByteToBuf(251);
// communication ID =2
sendPacket.uByteToBuf(2);
sendPacket.uByteToBuf(11);
sendPacket.uByteToBuf(152);
// communication ID =3
sendPacket.uByteToBuf(3);
sendPacket.uByteToBuf(27);
sendPacket.uByteToBuf(185);
#endif
sendPacket.finalizePacket();
IFDEBUG(
int i;
char x[100000];
printf("buffer with len = %d: ",sendPacket.getLength());
for (i = 0;i < sendPacket.getLength();i++)
{
printf("0x%x ",sendPacket.getBuf()[i] & 0xff);
//sprintf(&x[i], "%2x", (char *)sendPacket.getBuf()[i]);
}
printf("\n");
//ArLog::log(ArLog::Terse,
// "%s::blockingConnect() write Buffer = %s", getName(), x);
); // end IFDEBUG
