public CameraTrackAndDollyYoVariablesHolder(YoVariableHolder holder) {
if (holder != null) {
if (holder.hasUniqueVariable("q_x")) {
setTrackXVar((DoubleYoVariable) holder.getVariable("q_x"));
setDollyXVar((DoubleYoVariable) holder.getVariable("q_x"));
}
if (holder.hasUniqueVariable("q_y")) {
setTrackYVar((DoubleYoVariable) holder.getVariable("q_y"));
setDollyYVar((DoubleYoVariable) holder.getVariable("q_y"));
}
if (holder.hasUniqueVariable("q_z")) {
setTrackZVar((DoubleYoVariable) holder.getVariable("q_z"));
setDollyZVar((DoubleYoVariable) holder.getVariable("q_z"));
}
}
}
public void mapBooleanVariableToComponent(
YoVariableHolder holder,
SaitekX52Mapping mapping,
String variableName,
boolean toggle,
boolean invert) {
BooleanYoVariable yoVariable = (BooleanYoVariable) holder.getVariable(variableName);
if (yoVariable != null) {
mapBooleanVariableToComponent(mapping, yoVariable, toggle, invert);
} else {
PrintTools.warn(this, "Variable " + variableName + " could not be found!");
}
}
public void mapDoubleVariableToComponent(
YoVariableHolder holder,
SaitekX52Mapping mapping,
String variableName,
double minValue,
double maxValue,
double deadZone,
boolean invert) {
DoubleYoVariable yoVariable = (DoubleYoVariable) holder.getVariable(variableName);
if (yoVariable != null) {
mapDoubleVariableToComponent(mapping, yoVariable, minValue, maxValue, deadZone, invert);
} else {
PrintTools.warn(this, "Variable " + variableName + " could not be found!");
}
}
public void run() {
while (true) {
final DoubleYoVariable headingDotConstant =
(DoubleYoVariable) registry.getVariable("headingDotConstant");
final DoubleYoVariable velocityXConstant =
(DoubleYoVariable) registry.getVariable("velocityXConstant");
BooleanYoVariable multisenseControlsSpeedAndHeading =
(BooleanYoVariable) registry.getVariable("isMultisenseControllingSpeedAndHeading");
DoubleYoVariable desiredHeadingDot =
(DoubleYoVariable)
registry.getVariable("RateBasedDesiredHeadingControlModule", "desiredHeadingDot");
DoubleYoVariable desiredVelocityX =
(DoubleYoVariable)
registry.getVariable("ManualDesiredVelocityControlModule", "desiredVelocityX");
DoubleYoVariable multisenseHeading =
(DoubleYoVariable) registry.getVariable("multisenseHeading");
DoubleYoVariable multisenseMagnitude =
(DoubleYoVariable) registry.getVariable("multisenseMagnitude");
if (multisenseControlsSpeedAndHeading == null
|| desiredHeadingDot == null
|| desiredVelocityX == null
|| multisenseHeading == null
|| multisenseMagnitude == null) {
continue;
}
if (multisenseControlsSpeedAndHeading.getBooleanValue()) {
if (Math.abs(multisenseHeading.getDoubleValue()) > EPSILON) {
currentHeadingDot =
multisenseHeading.getDoubleValue() * headingDotConstant.getDoubleValue();
if (currentHeadingDot < minHeadingDot) currentHeadingDot = minHeadingDot;
if (currentHeadingDot > maxHeadingDot) currentHeadingDot = maxHeadingDot;
} else {
currentHeadingDot = 0.0;
}
double newVelocity =
multisenseMagnitude.getDoubleValue() * velocityXConstant.getDoubleValue();
// --(!) stop at half a meter from centroid of obj
if (multisenseMagnitude.getValueAsDouble() > 0.5) {
// -- This precaution may not be crucial, but
// -- ramp up velocity for large jumps
double deltaVelocity = newVelocity - currentVelocityX;
if (Math.abs(deltaVelocity) > .1)
currentVelocityX += deltaVelocity / Math.abs(deltaVelocity) * 0.05;
else currentVelocityX = newVelocity;
if (currentVelocityX < minVeclocity) currentVelocityX = minVeclocity;
if (currentVelocityX > maxVelocity) currentVelocityX = maxVelocity;
} else {
currentVelocityX = 0.0;
}
desiredHeadingDot.set(currentHeadingDot);
desiredVelocityX.set(currentVelocityX);
}
try {
Thread.sleep(100); // 10Hz
} catch (InterruptedException e) {
e.printStackTrace();
}
}
}