@KnowledgeExchange
@PeriodicScheduling(50)
public static void map(
@In("coord.leaderGas") Double lGas,
@In("coord.leaderBrake") Double lBrake,
@Out("coord.lPos") OutWrapper<Double> lPos,
@Out("coord.lSpeed") OutWrapper<Double> lSpeed,
@Out("coord.creationTime") OutWrapper<Double> creationTime,
@Out("member.eLeaderGas") OutWrapper<Double> eLeaderGas,
@Out("member.eLeaderBrake") OutWrapper<Double> eLeaderBrake,
@In("member.eLPos") Double eLPos,
@In("member.eLeaderSpeed") Double eLeaderSpeed,
@In("member.eLastTime") Double eLastTime) {
eLeaderGas.value = lGas;
eLeaderBrake.value = lBrake;
lPos.value = eLPos;
lSpeed.value = eLeaderSpeed;
creationTime.value = eLastTime;
}
@Process
@PeriodicScheduling((int) TIMEPERIOD)
public static void computeTarget(
@In("hPos") Double hPos,
@In("hSpeed") Double hSpeed,
@In("hRangeDistance") Double hRangeDistance,
@In("hFFPos") Double hFFPos,
@In("hFFSpeed") Double hFFSpeed,
@In("hFFCreationTime") Double hFFCreationTime,
@Out("hGas") OutWrapper<Double> hGas,
@Out("hBrake") OutWrapper<Double> hBrake,
@InOut("hMove") OutWrapper<Boolean> hMove,
@InOut("hMoveByOrder") OutWrapper<Boolean> hMoveByOrder,
@InOut("hFFConnected") OutWrapper<Boolean> hFFConnected,
@InOut("hFFLost") OutWrapper<Boolean> hFFLost,
@InOut("hFFTargetPos") OutWrapper<Double> hFFTargetPos,
@InOut("hFFTargetSpeed") OutWrapper<Double> hFFTargetSpeed) {
System.out.println(
" - RescueHelicopter : pos "
+ hPos
+ ", speed "
+ hSpeed
+ "...In the RescueHelicopter : - firefighter : pos "
+ hFFPos
+ " , speed "
+ hFFSpeed
+ " , creation time "
+ hFFCreationTime);
if (hFFCreationTime != 0.0) {
double inaccuracy = -1;
computeBeliefBoundaries(hFFPos, hFFSpeed, hFFTargetPos.value, hFFCreationTime);
if (hFFTargetPos.value != 0.0)
inaccuracy = Math.max(Math.abs(hFFPos - hFFPosMin), Math.abs(hFFPosMax - hFFPos));
if (inaccuracy <= THRESHOLD) {
hMove.value = false;
hFFLost.value = false;
} else if (inaccuracy > THRESHOLD) {
hFFConnected.value = false;
hMove.value = true;
hFFLost.value = true;
}
if (!hFFConnected.value && inaccuracy >= 0 && hFFLost.value) {
if (hMoveByOrder.value || hMove.value) {
if (hFFPosMin > hPos) {
hFFTargetPos.value = hFFPosMax;
hFFTargetSpeed.value = hFFSpeedMax;
} else if (hPos > hFFPosMax) {
hFFTargetPos.value = hPos;
hFFTargetSpeed.value = DESIRED_SPEED;
} else {
hFFTargetPos.value = hFFPosMax;
hFFTargetSpeed.value = hFFSpeedMax;
}
System.err.println(
"H2_Move..... from : "
+ hPos
+ " to "
+ hFFTargetPos.value
+ ", Min pos: "
+ hFFPosMin
+ " Max pos:"
+ hFFPosMax);
} else if (!hMove.value) {
hFFTargetPos.value = hPos;
hFFTargetSpeed.value = DESIRED_SPEED;
System.err.println(
"H2_%%%%%%%%%% OffloadHelicopter moves toward FireFighter %%%%%%%%%%%%");
} else if (!hMove.value) {
hFFTargetPos.value = hPos;
hFFTargetSpeed.value = DESIRED_SPEED;
System.err.println("H2_OffloadHelicopter order RescueHelicopter to move ......");
}
} else {
hFFTargetPos.value = hPos;
hFFTargetSpeed.value = DESIRED_SPEED;
}
} else {
hFFTargetPos.value = hPos;
hFFTargetSpeed.value = DESIRED_SPEED;
}
Pedal p = speedControl(hPos, hSpeed, hFFTargetPos.value, hFFTargetSpeed.value);
hGas.value = p.gas;
hBrake.value = p.brake;
}