macro diffStop diffEqn diffStop {d(myPos.x) = 0.0;  d(myPos.y) = 0.0}
macro updateFreq 1000.0
macro updateCost 100.0
macro spd        25.0

/*****************************************************************/
/*                Robot Navigation Example                       */
/*   Two robots - robot1 and robot2 navigating to reach the goals*/
/*****************************************************************/
agent Robot (position initPos, real speed, position o1, position o2) {
  input  channel of estimate inLink1, inLink2; 
  output channel of estimate outLink1, outLink2;

  extern estimate updateEstimate(position, estimate);
  extern estimate computeGoal(position, position, estimate, estimate);
  extern real computeSlope(position, position);

  /*********************/
  /*    mode main     */
  /*********************/
  mode main() {
    diff analog position myPos;
    diff analog real timer;
    estimate estO1, estO2;
    position currentGoal; 
       
    diffEqn diffTime {d(timer) = 1.0}

    // Initialization
    // all controlled variables - output variables and private variables 
    // are required to be initialized
    trans initTrans from main to awayTarget when true do {
      outLink1 = EMPTY; outLink2 = EMPTY;
      myPos = initPos; 
      timer = 0.0;
      estO1 = updateEstimate(myPos, o1); estO2 = updateEstimate(myPos, o2);
      currentGoal = computeGoal(myPos, target, estO1, estO2)
    }

    /*********************/
    /*    mode atTarget  */
    /*********************/
    mode atTarget() {
      inv invAt { myPos == target }
      diffStop
    }

    /*********************/
    /*   mode awayTarget */
    /*********************/
    init mode awayTarget() {
      analog real direction; 

      inv invAway { myPos != target }
      algeEqn compDir {direction = computeSlope(myPos, target)}

      /*********************/
      /*   mode moving     */
      /*********************/
      init mode moving() {
        inv invFreq { timer <= updateFreq }
        diffEqn diffMove {
			   d(myPos.x) = speed*cos(direction); 
   			   d(myPos.y) = speed*sin(direction)
	}
      }  

      /*********************/
      /*   mode updating   */
      /*********************/
      mode updating () {
        inv invTUCost { timer <= updateCost }
        diffStop
      } 

      trans  freqTimeout from moving to updating 
      when (timer== updateFreq) do {}
      trans updateTimeout from updating to moving 
      when (timer== updateCost) do {
        timer = 0;
        estO1 = updateEstimate(myPos, o1); 
        estO2 = updateEstimate(myPos, o2);
        send (outLink1, estO1); send (outLink1, estO2);
        currentGoal= computeGoal(myPos, target, estO1, estO2)
      }
    }
    trans arrived from awayTarget to atTarget 
    when (myPos == target) do {}
  }
}

robot1 = Robot (iPos, spd, o1, o2) [inLink1, inLink2, outLink1, outLink2 = 
			   r21Link1, r21Link2, r12Link1, r12Link2];
robot2 = Robot (iPos, spd, o1, o2) [inLink1, inLink2, outLink1, outLink2 = 
			   r12Link1, r12Link2, r21Link1, r21Link2];
sys = robot1 || robot2 ;