/**
* Author: Rajeev Alur, January 1999
* Discussed with Radu Grosu, Yerang Hur, Vijay Kumar, and Insup Lee
* Updated by Yerang Hur, 1999, 2000
* Updated by Oleg Sokolsky and Valentina Sokolskaya, 2000
* --
* changes for Charon 0.60, plus type checking changes
* 092600, Valentina Sokolskaya
*/
//macro diffStop diff diffStop {d(myPos.x) = 0.0; d(myPos.y) = 0.0}
//macro updateFreq 20.0
//macro updateCost 2.0
extern Estimate Robot.updateEstimate(Position, Estimate);
extern Estimate Robot.computeGoal(Position, Position, Estimate, Estimate);
extern real java.lang.Math.sin (real);
extern real java.lang.Math.cos (real);
extern Position iPos1;
extern Position iPos2;
extern Position target1;
extern Position target2;
extern Estimate ob1;
extern Estimate ob2;
/*********************/
/* mode moving */
/*********************/
mode movingM(real v, real phi, real k) {
entry fromUPt;
exit toUPt;
read analog real timer;
readWrite analog real omega;
readWrite analog Position myPos;
readWrite analog real theta;
diff diffSteer { d(myPos.x) == v*java.lang.Math.cos(theta);
d(myPos.y) == v*java.lang.Math.sin(theta);
d(theta) == omega }
alge algeOmega { omega == k * (theta - phi)}
inv invFreq { 0.0 <= timer && timer <= 20.0 }
} // end of mode moving
/*********************/
/* mode updating */
/*********************/
mode updatingM () {
entry fromMPt;
exit toMPt;
read analog real timer;
read Estimate rO1, rO2;
readWrite analog Position myPos;
write Estimate estO1, estO2;
diff diffStop {d(myPos.x) == 0.0; d(myPos.y) == 0.0}
alge algeUpdate { estO1 == Robot.updateEstimate(myPos, rO1);
estO2 == Robot.updateEstimate(myPos, rO2) }
inv invCost { timer <= 2.0 }
} // end of mode updating
/*********************/
/* mode awayTarget */
/*********************/
mode awayTargetM(Position target, real v, real phi, real initTheta,
real initOmega, real k) {
exit toAtPt;
read analog real timer;
readWrite analog Position myPos;
read channel of Estimate inLink1, inLink2;
write channel of Estimate outLink1, outLink2;
write Estimate currentGoal;
private analog real theta;
private analog real omega;
private Estimate estO1, estO2, rO1, rO2;
diff diffPlan { d(myPos.x) == v*java.lang.Math.cos(phi);
d(myPos.y) == v*java.lang.Math.sin(phi) }
inv invAway { myPos != target }
mode moving = movingM (v, phi, k)
mode updating = updatingM()
trans from default to moving
when true do{ theta = initTheta; omega = initOmega }
trans freqTimeout from moving.toUPt to updating.fromMPt
when (timer % 20.0 == 0) do {}
trans updateTimeout from updating.toMPt to moving.fromUPt
when (timer % 2.0 == 0) do {
receive (inLink1, rO1); receive (inLink2, rO2);
send(outLink1,estO1); send(outLink2, estO2);
currentGoal= Robot.computeGoal(myPos, target, estO1, estO2) }
} // end of mode awayTarget
/*********************/
/* mode atTarget */
/*********************/
mode atTargetM(Position target) {
entry fromAwPt;
readWrite analog Position myPos;
diff diffStop {d(myPos.x) == 0.0; d(myPos.y) == 0.0}
inv invAt { myPos == target }
} // end of atTarget
/*********************/
/* mode top */
/*********************/
mode topM(Position initPos, Estimate o1, Estimate o2, Position target,
real v, real phi, real initTheta, real initOmega, real k) {
read channel of Estimate inLink1, inLink2;
write channel of Estimate outLink1, outLink2;
write analog Position myPos;
private analog real timer;
private Estimate estO1, estO2;
private Estimate currentGoal;
diff diffTime {d(timer) == 1.0}
mode awayTarget = awayTargetM(target, v, phi, initTheta, initOmega, k)
mode atTarget = atTargetM(target)
trans toawayTarget from default to awayTarget when true do {
// send(outLink1, EMPTY); send(outLink2, EMPTY);
myPos = initPos; timer = 0.0;
estO1 = Robot.updateEstimate(myPos, o1);
estO2 = Robot.updateEstimate(myPos, o2);
currentGoal = Robot.computeGoal(myPos, target, estO1, estO2) }
trans arrival from awayTarget.toAtPt to atTarget.fromAwPt
when (myPos == target) do {}
} // end of mode top
/*****************************************************************/
/* Robot Navigation Example */
/* Two robots - robot1 and robot2 navigating to reach the goals*/
/*****************************************************************/
agent robot (Position initPos, Estimate o1, Estimate o2,
Position target, real v, real phi, real initTheta,
real initOmega, real k) {
read channel of Estimate inLink1, inLink2;
write channel of Estimate outLink1, outLink2;
write analog Position myPos;
mode top = topM (initPos, o1, o2, target, v, phi, initTheta, initOmega, k)
}
agent system() {
write analog Position myPos1, myPos2;
private channel of Estimate r21Link1, r21Link2, r12Link1, r12Link2;
agent robot1 = robot (iPos1, ob1, ob2, target1, 10.0, 1.0, 1.0, 1.0, 0.5)
[inLink1, inLink2, outLink1, outLink2, myPos :=
r21Link1, r21Link2, r12Link1, r12Link2, myPos1]
agent robot2 = robot (iPos2, ob1, ob2, target2, 10.0, 1.1, 1.0, 1.0, 0.5)
[inLink1, inLink2, outLink1, outLink2, myPos :=
r12Link1, r12Link2, r21Link1, r21Link2, myPos2]
}
|