int main(int argc, char **argv)
{
std::string str;
int ret;
int successes = 0, failures = 0;
int action;
bool exitOnFailure = true;
ArSerialConnection con;
ArRobot robot;
//ArLog::init(ArLog::StdOut, ArLog::Verbose);
srand(time(NULL));
robot.runAsync(false);
// if (!exitOnFailure)
// ArLog::init(ArLog::None, ArLog::Terse);
//else
//ArLog::init(ArLog::None);
while (1)
{
if (con.getStatus() != ArDeviceConnection::STATUS_OPEN &&
(ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
++failures;
if (exitOnFailure)
{
printf("Failed\n");
exit(0);
}
else
{
ArUtil::sleep(200);
robot.unlock();
continue;
}
}
robot.lock();
robot.setDeviceConnection(&con);
robot.unlock();
ArUtil::sleep((rand() % 5) * 100);
if (robot.asyncConnect())
{
robot.waitForConnectOrConnFail();
robot.lock();
if (!robot.isConnected())
{
if (exitOnFailure)
{
printf("Failed after %d tries.\n", successes);
exit(0);
}
printf("Failed to connect successfully");
++failures;
}
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::SOUNDTOG, 0);
//robot.comInt(ArCommands::PLAYLIST, 0);
robot.comInt(ArCommands::ENCODER, 1);
ArUtil::sleep(((rand() % 20) + 3) * 100);
++successes;
// okay, now try to leave it in a messed up state
action = rand() % 8;
robot.dropConnection();
switch (action) {
case 0:
printf("Discon 0 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
break;
case 1:
printf("Discon 1 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
ArUtil::sleep(100);
robot.com(1);
break;
case 2:
printf("Discon 2 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
ArUtil::sleep(100);
robot.com(1);
ArUtil::sleep(100);
robot.com(2);
break;
case 3:
printf("Discon 10 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(10);
break;
case 4:
printf("Discon ");
robot.disconnect();
break;
default:
printf("Leave ");
break;
}
robot.unlock();
}
else
{
if (exitOnFailure)
{
printf("Failed after %d tries.\n", successes);
exit(0);
}
printf("Failed to start connect ");
++failures;
}
if ((rand() % 2) == 0)
{
printf(" ! RadioDisconnect ! ");
con.write("|||\15", strlen("!!!\15"));
ArUtil::sleep(100);
con.write("WMD\15", strlen("WMD\15"));
ArUtil::sleep(200);
}
if ((rand() % 2) == 0)
{
printf(" ! ClosePort !\n");
con.close();
}
else
printf("\n");
printf("#### %d successes %d failures, %% %.2f success\n", successes, failures,
(float)successes/(float)(successes+failures)*100);
ArUtil::sleep((rand() % 2)* 1000);
}
return 0;
}
int main(int argc, char **argv)
{
int ret;
std::string str;
ArSerialConnection con;
ArSickPacket sick;
ArSickPacket *packet;
ArSickPacketReceiver receiver(&con);
ArTime start;
unsigned int value;
int numReadings;
ArTime lastReading;
ArTime packetTime;
start.setToNow();
// open the connection, if it fails, exit
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
start.setToNow();
printf("Waiting for laser to power on\n");
sick.empty();
sick.uByteToBuf(0x10);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
while (start.secSince() < 70 &&
((packet = receiver.receivePacket(100)) == NULL
|| (packet->getID() != 0x90)));
if (packet != NULL)
printf("Laser powered on\n");
else
exit(1);
printf("Changing baud\n");
sick.empty();
sick.byteToBuf(0x20);
sick.byteToBuf(0x40);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
ArUtil::sleep(10);
if (!con.setBaud(38400))
{
printf("Could not set baud, exiting\n");
}
/*packet = receiver.receivePacket(100);
if (packet != NULL)
packet->log();
*/
sick.empty();
sick.uByteToBuf(0x3B);
sick.uByte2ToBuf(180);
sick.uByte2ToBuf(100);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
packet = receiver.receivePacket(100);
if (packet != NULL)
packet->log();
sick.empty();
sick.byteToBuf(0x20);
sick.byteToBuf(0x24);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
packet = receiver.receivePacket(100);
if (packet != NULL)
packet->log();
printf("Starting to report back from port, it took %ld ms to get here:\n",
start.mSecSince());
start.setToNow();
while (start.secSince() < 6)
{
packetTime.setToNow();
packet = receiver.receivePacket();
if (packet != NULL)
printf("####### %ld ms was how long the packet took\n", packetTime.mSecSince());
if (packet != NULL)
{
if (packet->getLength() < 10)
packet->log();
else if (packet->getID() == 0x90)
{
char strBuf[512];
packet->log();
//printf("%x\n", packet->bufToUByte());
packet->bufToStr(strBuf, 512);
printf("0x%x %s\n", packet->getID(), strBuf);
sick.empty();
sick.uByteToBuf(0x3B);
sick.uByte2ToBuf(180);
sick.uByte2ToBuf(100);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
packet = receiver.receivePacket(100);
sick.empty();
sick.uByteToBuf(0x20);
sick.uByteToBuf(0x24);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
}
else
{
value = packet->bufToUByte2();
numReadings = value & 0x3ff;
printf("%ld ms after last reading.\n", lastReading.mSecSince());
/*
printf("Reading number %d, complete %d, unit: %d %d:\n", value & 0x3ff, !(bool)(value & ArUtil::BIT13), (bool)(value & ArUtil::BIT14), (bool)(value & ArUtil::BIT15));
for (i = 0; i < numReadings; i++)
{
value = packet->bufToUByte2();
if (value & ArUtil::BIT13)
printf("D");
printf("%d ", value & 0x1fff);
}
printf("\n");
*/
lastReading.setToNow();
}
}
else
{
//printf("No packet\n");
}
}
}