AREXPORT void ArLaser::laserSetAbsoluteMaxRange(unsigned int absoluteMaxRange)
{
ArLog::log(myInfoLogLevel, "%s: Setting absolute max range to %u",
getName(), absoluteMaxRange);
myAbsoluteMaxRange = absoluteMaxRange;
setMaxRange(getMaxRange());
}
bool Datalink::setSlotMaxRange(const Basic::Distance* const msg)
{
bool ok = false;
if(msg != 0) {
double rng = Basic::NauticalMiles::convertStatic(*msg);
ok = setMaxRange(rng);
}
return ok;
}
void particle::setRanges(float x, float y, float z, float r){
float maxX = x + r;
float maxY = y + r;
float maxZ = z + r;
float minX = x - r;
float minY = y - r;
float minZ = z - r;
setMaxRange(maxX, maxY, maxZ);
setMinRange(minX, minY, minZ);
}
// setSlotMaxRange() -- sets the maxRange slot
bool Otw::setSlotMaxRange(const Basic::Number* const msg)
{
bool ok = false;
if (msg != 0) {
// We have a simple number, which should be meters!
ok = setMaxRange(msg->getReal());
}
if (!ok) {
// error -- invalid range
std::cerr << "Otw::setSlotMaxRange: invalid maximum range" << std::endl;
}
return ok;
}
// setSlotMaxRange() -- sets the maxRange slot
bool Otw::setSlotMaxRange(const Basic::Distance* const msg)
{
bool ok = false;
if (msg != 0) {
// We have a distance which we can convert to meters
LCreal rng = Basic::Meters::convertStatic(*msg);
ok = setMaxRange( rng );
}
if (!ok) {
// error -- invalid range
std::cerr << "Otw::setSlotMaxRange: invalid maximum range" << std::endl;
}
return ok;
}
AREXPORT ArLaserFilter::ArLaserFilter(
ArLaser *laser, const char *name) :
ArLaser(laser->getLaserNumber(),
name != NULL && name[0] != '\0' ? name : laser->getName(),
laser->getAbsoluteMaxRange(),
laser->isLocationDependent(),
false),
myProcessCB(this, &ArLaserFilter::processReadings)
{
myLaser = laser;
if (name == NULL || name[0] == '\0')
{
std::string filteredName;
filteredName = "filtered_";
filteredName += laser->getName();
laserSetName(filteredName.c_str());
}
myRawReadings = new std::list<ArSensorReading *>;
char buf[1024];
sprintf(buf, "%sProcessCB", getName());
myProcessCB.setName(buf);
myAngleToCheck = 1;
myAnyFactor = -1;
myAllFactor = -1;
myMaxRange = -1;
setCurrentDrawingData(
new ArDrawingData(*(myLaser->getCurrentDrawingData())),
true);
setCumulativeDrawingData(
new ArDrawingData(*(myLaser->getCumulativeDrawingData())),
true);
// laser parameters
setInfoLogLevel(myLaser->getInfoLogLevel());
setConnectionTimeoutSeconds(myLaser->getConnectionTimeoutSeconds());
setCumulativeCleanDist(myLaser->getCumulativeCleanDist());
setCumulativeCleanInterval(myLaser->getCumulativeCleanInterval());
setCumulativeCleanOffset(myLaser->getCumulativeCleanOffset());
setSensorPosition(myLaser->getSensorPosition());
laserSetAbsoluteMaxRange(myLaser->getAbsoluteMaxRange());
setMaxRange(myLaser->getMaxRange());
// base range device parameters
setMaxSecondsToKeepCurrent(myLaser->getMaxSecondsToKeepCurrent());
setMinDistBetweenCurrent(getMinDistBetweenCurrent());
setMaxSecondsToKeepCumulative(myLaser->getMaxSecondsToKeepCumulative());
setMaxDistToKeepCumulative(myLaser->getMaxDistToKeepCumulative());
setMinDistBetweenCumulative(myLaser->getMinDistBetweenCumulative());
setMaxInsertDistCumulative(myLaser->getMaxInsertDistCumulative());
setCurrentDrawingData(myLaser->getCurrentDrawingData(), false);
setCumulativeDrawingData(myLaser->getCumulativeDrawingData(), false);
// now all the specific laser settings (this should already be taken
// care of when this is created, but the code existed for the
// simulated laser so I put it here too)
if (myLaser->canSetDegrees())
laserAllowSetDegrees(
myLaser->getStartDegrees(), myLaser->getStartDegreesMin(),
myLaser->getStartDegreesMax(), myLaser->getEndDegrees(),
myLaser->getEndDegreesMin(), myLaser->getEndDegreesMax());
if (myLaser->canChooseDegrees())
laserAllowDegreesChoices(myLaser->getDegreesChoice(),
myLaser->getDegreesChoicesMap());
if (myLaser->canSetIncrement())
laserAllowSetIncrement(myLaser->getIncrement(),
myLaser->getIncrementMin(),
myLaser->getIncrementMax());
if (myLaser->canChooseIncrement())
laserAllowIncrementChoices(myLaser->getIncrementChoice(),
myLaser->getIncrementChoicesMap());
if (myLaser->canChooseUnits())
laserAllowUnitsChoices(myLaser->getUnitsChoice(),
myLaser->getUnitsChoices());
if (myLaser->canChooseReflectorBits())
laserAllowReflectorBitsChoices(myLaser->getReflectorBitsChoice(),
myLaser->getReflectorBitsChoices());
if (canSetPowerControlled())
laserAllowSetPowerControlled(myLaser->getPowerControlled());
if (myLaser->canChooseStartingBaud())
laserAllowStartingBaudChoices(myLaser->getStartingBaudChoice(),
myLaser->getStartingBaudChoices());
if (myLaser->canChooseAutoBaud())
laserAllowAutoBaudChoices(myLaser->getAutoBaudChoice(),
myLaser->getAutoBaudChoices());
laserSetDefaultTcpPort(myLaser->getDefaultTcpPort());
laserSetDefaultPortType(myLaser->getDefaultPortType());
}
AREXPORT bool ArLaser::laserPullUnsetParamsFromRobot(void)
{
if (myRobot == NULL)
{
ArLog::log(ArLog::Normal, "%s: Trying to connect, but have no robot, continuing under the assumption this is intentional", getName());
return true;
}
const ArRobotParams *params = myRobot->getRobotParams();
if (params == NULL)
{
ArLog::log(ArLog::Terse,
"%s: Robot has no params, cannot pull unset params from robot",
getName());
return false;
}
const char *paramStr;
char *endPtr;
double paramDouble;
int paramInt;
bool paramBool;
paramBool = params->getLaserFlipped(getLaserNumber());
if (!myFlippedSet)
{
if (paramBool)
{
ArLog::log(myInfoLogLevel,
"%s: Setting flipped to true from robot params",
getName());
setFlipped(true);
}
else if (!paramBool)
{
ArLog::log(myInfoLogLevel,
"%s: Setting flipped to false from robot params",
getName());
setFlipped(false);
}
}
paramInt = params->getLaserMaxRange(getLaserNumber());
if (!myMaxRangeSet)
{
if(paramInt < 0)
{
ArLog::log(ArLog::Terse, "%s: LaserMaxRange in robot param file was negative but shouldn't be (it was '%d'), failing", getName(), paramInt);
return false;
}
if (paramInt > 0)
{
ArLog::log(myInfoLogLevel,
"%s: Setting max range to %d from robot params",
getName(), paramInt);
setMaxRange(paramInt);
}
}
paramInt = params->getLaserCumulativeBufferSize(getLaserNumber());
if (!myCumulativeBufferSizeSet)
{
if(paramInt < 0)
{
ArLog::log(ArLog::Terse, "%s: LaserCumulativeBufferSize in robot param file was negative but shouldn't be (it was '%d'), failing", getName(), paramInt);
return false;
}
if (paramInt > 0)
{
ArLog::log(myInfoLogLevel,
"%s: Setting cumulative buffer size to %d from robot params",
getName(), paramInt);
setCumulativeBufferSize(paramInt);
}
}
paramStr = params->getLaserStartDegrees(getLaserNumber());
if (canSetDegrees() && !myStartDegreesSet &&
paramStr != NULL && paramStr[0] != '\0')
{
paramDouble = strtod(paramStr, &endPtr);
if(endPtr == paramStr)
{
ArLog::log(ArLog::Terse, "%s: LaserStartDegrees in robot param file was not a double (it was '%s'), failing", getName(), paramStr);
return false;
}
ArLog::log(myInfoLogLevel,
"%s: Setting start degrees to %g from robot params",
getName(), paramDouble);
setStartDegrees(paramDouble);
}
paramStr = params->getLaserEndDegrees(getLaserNumber());
if (canSetDegrees() && !myEndDegreesSet &&
paramStr != NULL && paramStr[0] != '\0')
{
paramDouble = strtod(paramStr, &endPtr);
if(endPtr == paramStr)
{
ArLog::log(ArLog::Terse,
"%s: LaserEndDegrees in robot param file was not a double (it was '%s'), failing",
getName(), paramStr);
return false;
}
ArLog::log(myInfoLogLevel,
"%s: Setting end degrees to %g from robot params",
getName(), paramDouble);
setEndDegrees(paramDouble);
}
paramStr = params->getLaserDegreesChoice(getLaserNumber());
if (canChooseDegrees() && !myDegreesChoiceSet &&
paramStr != NULL && paramStr[0] != '\0')
{
ArLog::log(myInfoLogLevel,
"%s: Setting degrees choice to %s from robot params",
getName(), paramStr);
chooseDegrees(paramStr);
}
paramStr = params->getLaserIncrement(getLaserNumber());
if (canSetDegrees() && !myIncrementSet &&
paramStr != NULL && paramStr[0] != '\0')
{
paramDouble = strtod(paramStr, &endPtr);
if(endPtr == paramStr)
{
ArLog::log(ArLog::Terse,
"%s: LaserIncrement in robot param file was not a double (it was '%s'), failing",
getName(), paramStr);
return false;
}
ArLog::log(myInfoLogLevel,
"%s: Setting increment to %g from robot params",
getName(), paramDouble);
setIncrement(paramDouble);
}
paramStr = params->getLaserIncrementChoice(getLaserNumber());
if (canChooseIncrement() && !myIncrementChoiceSet &&
paramStr != NULL && paramStr[0] != '\0')
{
ArLog::log(myInfoLogLevel,
"%s: Setting increment choice to %s from robot params",
getName(), paramStr);
chooseIncrement(paramStr);
}
paramStr = params->getLaserUnitsChoice(getLaserNumber());
if (canChooseUnits() && !myUnitsChoiceSet &&
paramStr != NULL && paramStr[0] != '\0')
{
ArLog::log(myInfoLogLevel,
"%s: Setting units choice to %s from robot params",
getName(), paramStr);
chooseUnits(paramStr);
}
paramStr = params->getLaserReflectorBitsChoice(getLaserNumber());
if (canChooseReflectorBits() && !myReflectorBitsChoiceSet &&
paramStr != NULL && paramStr[0] != '\0')
{
ArLog::log(myInfoLogLevel,
"%s: Setting reflectorBits choice to %s from robot params",
getName(), paramStr);
chooseReflectorBits(paramStr);
}
paramBool = params->getLaserPowerControlled(getLaserNumber());
if (canSetPowerControlled() && !myPowerControlledSet)
{
if (paramBool)
{
ArLog::log(myInfoLogLevel,
"%s: Setting powerControlled to true from robot params",
getName());
setPowerControlled(true);
}
else if (!paramBool)
{
ArLog::log(myInfoLogLevel,
"%s: Setting powerControlled to false from robot params",
getName());
setPowerControlled(false);
}
}
paramStr = params->getLaserStartingBaudChoice(getLaserNumber());
if (canChooseStartingBaud() && !myStartingBaudChoiceSet &&
paramStr != NULL && paramStr[0] != '\0')
{
ArLog::log(myInfoLogLevel,
"%s: Setting startingBaud choice to %s from robot params",
getName(), paramStr);
chooseStartingBaud(paramStr);
}
paramStr = params->getLaserAutoBaudChoice(getLaserNumber());
if (canChooseAutoBaud() && !myAutoBaudChoiceSet &&
paramStr != NULL && paramStr[0] != '\0')
{
ArLog::log(myInfoLogLevel,
"%s: Setting autoBaud choice to %s from robot params",
getName(), paramStr);
chooseAutoBaud(paramStr);
}
if (!addIgnoreReadings(params->getLaserIgnore(getLaserNumber())))
return false;
setSensorPosition(params->getLaserX(getLaserNumber()),
params->getLaserY(getLaserNumber()),
params->getLaserTh(getLaserNumber()),
params->getLaserZ(getLaserNumber()));
return true;
}
/*!
* Description: General funtion for menus which set values
* (Such as Set Max Range). This calls the
* appropriate function, and transmits user messages.
*/
static void setValue(int input)
{
char string[20] = " set to \0";
char stringLcd[20] = {0};
Direction dir;
// Sends a new line
sendNewLine(1);
// Handle inpropper input values
if (input < m_currentMenu.minVal || input > m_currentMenu.maxVal)
{
errOutOfRange(m_currentMenu.minVal, m_currentMenu.maxVal);
sendNewLine(1);
return;
}
// Handle Different cases
// @TODO handle current values
// @TODO Integrate
switch (m_currentMenu.menuID)
{
case AZ_GOTO:
dir.azimuth = input;
dir.elevation = getDir().elevation;
move(dir);
sendROM(angleStr);
break;
case EL_GOTO:
dir.azimuth = getDir().azimuth;
dir.elevation = input;
move(dir);
sendROM(angleStr);
break;
case AZ_MIN:
setMinAzimuthAngle((char) input);
AzGoto.minVal = input;
sendROM(angleStr);
break;
case AZ_MAX:
setMaxAzimuthAngle((char) input);
AzGoto.maxVal = input;
sendROM(angleStr);
break;
case EL_MIN:
setMinElevationAngle((char) input);
ElGoto.minVal = input;
sendROM(angleStr);
break;
case EL_MAX:
setMaxElevationAngle((char) input);
ElGoto.maxVal = input;
sendROM(angleStr);
break;
case RANGE_MIN:
setMinRange(input);
sendROM(mmStr);
break;
case RANGE_MAX:
setMaxRange(input);
sendROM(mmStr);
break;
case US_SAMPLE_RATE:
setUsSampleRate(input);
sendROM(sampleRate);
break;
case US_SAMPLE_AVG:
setNumSamples(input);
sendROM(numPerSample);
break;
}
// Transmit the final part of the sentence
transmit(string);
transmit(intToAscii(input));
sendNewLine(1);
//lcdWriteString(strcpypgm2ram(stringLcd, "OK!"), 2);
}
bool ArUrg_2_0::internalConnect(void)
{
bool ret = true;
char buf[1024];
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
bool alreadyAtAutobaud = false;
// empty the buffer...
/*
sendCommandAndRecvStatus(
"RS", "reset",
buf, sizeof(buf), 1000);
readLine(buf, sizeof(buf), 1, true, false);
sendCommandAndRecvStatus(
"SCIP2.0", "SCIP 2.0 request",
buf, sizeof(buf), 1000);
*/
writeLine("RS");
ArUtil::sleep(100);
writeLine("SCIP2.0");
ArUtil::sleep(100);
ArTime startedFlushing;
while (readLine(buf, sizeof(buf), 1, true, false) ||
startedFlushing.mSecSince() < 1000);
buf[0] = '\0';
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
// if we didn't get it and have an autobaud, try it at what the autobaud rate is
if (serConn != NULL && atoi(getAutoBaudChoice()) > 0)
{
alreadyAtAutobaud = true;
serConn->setBaud(atoi(getAutoBaudChoice()));
ArUtil::sleep(100);
writeLine("RS");
ArUtil::sleep(100);
writeLine("SCIP2.0");
ArUtil::sleep(100);
startedFlushing.setToNow();
while (readLine(buf, sizeof(buf), 1, true, false) ||
startedFlushing.mSecSince() < 1000);
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request after falling back to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after falling back to autobaudchoice",
getName());
return false;
}
}
// if we don't have a serial port or no autobaud then we can't
// change the baud, so just fail
else
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response (%s)",
getName(), buf);
return false;
}
}
// if we want to autobaud, then give it a whirl
if (!alreadyAtAutobaud && serConn != NULL && atoi(getAutoBaudChoice()) > 0)
{
// empty the buffer from the last version request
while (readLine(buf, sizeof(buf), 100, true, false));
// now change the baud...
sprintf(buf, "SS%06d", atoi(getAutoBaudChoice()));
if (!writeLine(buf))
return false;
ArUtil::sleep(100);
//serConn->setBaud(115200);
serConn->setBaud(atoi(getAutoBaudChoice()));
// wait a second for the baud to change...
ArUtil::sleep(100);
// empty the buffer from the baud change
while (readLine(buf, sizeof(buf), 100, true, false));
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request after switching to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after switching to autobaudchoice",
getName());
return false;
}
ArLog::log(ArLog::Verbose, "%s: Switched to %s baud rate",
getName(), getAutoBaudChoice());
}
while (readLine(buf, sizeof(buf), 10000, false, true))
{
if (strlen(buf) == 0)
break;
if (strncasecmp(buf, "VEND:", strlen("VEND:")) == 0)
myVendor = &buf[5];
else if (strncasecmp(buf, "PROD:", strlen("PROD:")) == 0)
myProduct = &buf[5];
else if (strncasecmp(buf, "FIRM:", strlen("FIRM:")) == 0)
myFirmwareVersion = &buf[5];
else if (strncasecmp(buf, "PROT:", strlen("PROT:")) == 0)
myProtocolVersion = &buf[5];
else if (strncasecmp(buf, "SERI:", strlen("SERI:")) == 0)
mySerialNumber = &buf[5];
else if (strncasecmp(buf, "STAT:", strlen("STAT:")) == 0)
myStat = &buf[5];
}
if (myVendor.empty() || myProduct.empty() || myFirmwareVersion.empty() ||
myProtocolVersion.empty() || mySerialNumber.empty())
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Missing information in version response",
getName());
return false;
}
if (!(ret = sendCommandAndRecvStatus(
"PP", "parameter info request",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad response to parameter info request",
getName());
return false;
}
while (readLine(buf, sizeof(buf), 10000, false, true))
{
if (strlen(buf) == 0)
break;
if (strncasecmp(buf, "MODL:", strlen("MODL:")) == 0)
myModel = &buf[5];
else if (strncasecmp(buf, "DMIN:", strlen("DMIN:")) == 0)
myDMin = atoi(&buf[5]);
else if (strncasecmp(buf, "DMAX:", strlen("DMAX:")) == 0)
myDMax = atoi(&buf[5]);
else if (strncasecmp(buf, "ARES:", strlen("ARES:")) == 0)
myARes = atoi(&buf[5]);
else if (strncasecmp(buf, "AMIN:", strlen("AMIN:")) == 0)
myAMin = atoi(&buf[5]);
else if (strncasecmp(buf, "AMAX:", strlen("AMAX:")) == 0)
myAMax = atoi(&buf[5]);
else if (strncasecmp(buf, "AFRT:", strlen("AFRT:")) == 0)
myAFront = atoi(&buf[5]);
else if (strncasecmp(buf, "SCAN:", strlen("SCAN:")) == 0)
myScan = atoi(&buf[5]);
}
if (myModel.empty() || myDMin == 0 || myDMax == 0 || myARes == 0 ||
myAMin == 0 || myAMax == 0 || myAFront == 0 || myScan == 0)
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Missing information in parameter info response",
getName());
return false;
}
myStepSize = 360.0 / myARes;
myStepFirst = myAFront * myStepSize;
if (myMaxRange > myDMax)
setMaxRange(myDMax);
//log();
setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped());
//myLogMore = true;
// myLogMore = false;
ArUtil::sleep(100);
//printf("myRequestString %s\n", myRequestString);
if (!(ret = sendCommandAndRecvStatus(
myRequestString, "request distance reading",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on distance reading response (%s)",
getName(), buf);
return false;
}
//myLogMore = false;
ArTime started;
started.setToNow();
while (started.secSince() < 10 &&
readLine(buf, sizeof(buf), 10000, true, false))
{
if (strlen(buf) == 0)
return true;
}
ArLog::log(ArLog::Normal, "%s::blockingConnect: Did not get distance reading back",
getName());
return false;
}
void WeaponObjectImplementation::updateCraftingValues(CraftingValues* values, bool firstUpdate) {
/*
* Incoming Values: Ranges:
* mindamage Differs between weapons
* maxdamage
* attackspeed
* woundchance
* roundsused
* hitpoints
* zerorangemod
* maxrange
* maxrangemod
* midrange
* midrangemod
* charges
* attackhealthcost
* attackactioncost
* attackmindcost
*/
float value = 0.f;
setMinDamage(MAX(values->getCurrentValue("mindamage"), 0));
setMaxDamage(MAX(values->getCurrentValue("maxdamage"), 0));
setAttackSpeed(values->getCurrentValue("attackspeed"));
setHealthAttackCost((int)values->getCurrentValue("attackhealthcost"));
setActionAttackCost((int)values->getCurrentValue("attackactioncost"));
setMindAttackCost((int)values->getCurrentValue("attackmindcost"));
if (isJediWeapon()) {
setForceCost((int)values->getCurrentValue("forcecost"));
setBladeColor(31);
}
value = values->getCurrentValue("woundchance");
if(value != CraftingValues::VALUENOTFOUND)
setWoundsRatio(value);
//value = craftingValues->getCurrentValue("roundsused");
//if(value != DraftSchematicValuesImplementation::VALUENOTFOUND)
//_this.getReferenceUnsafeStaticCast()->set_______(value);
value = values->getCurrentValue("zerorangemod");
if(value != CraftingValues::VALUENOTFOUND)
setPointBlankAccuracy((int)value);
value = values->getCurrentValue("maxrange");
if(value != CraftingValues::VALUENOTFOUND)
setMaxRange((int)value);
value = values->getCurrentValue("maxrangemod");
if(value != CraftingValues::VALUENOTFOUND)
setMaxRangeAccuracy((int)value);
value = values->getCurrentValue("midrange");
if(value != CraftingValues::VALUENOTFOUND)
setIdealRange((int)value);
value = values->getCurrentValue("midrangemod");
if(value != CraftingValues::VALUENOTFOUND)
setIdealAccuracy((int)value);
//value = craftingValues->getCurrentValue("charges");
//if(value != CraftingValues::VALUENOTFOUND)
// setUsesRemaining((int)value);
value = values->getCurrentValue("hitpoints");
if(value != CraftingValues::VALUENOTFOUND)
setMaxCondition((int)value);
setConditionDamage(0);
}
LUltrasonic::LUltrasonic(unsigned int echoPin, unsigned int triggerPin, unsigned long maximumRange)
{
setTriggerPin(triggerPin);
setEchoPin(echoPin);
setMaxRange(maximumRange);
}
void SpinBoxRangeSliderQt::setMinMaxRange(const int minRange, const int maxRange) {
blockSignals(true);
setMinRange(minRange);
setMaxRange(maxRange);
blockSignals(false);
}
