#
# The Servo class inherits from RevoluteJoint, which is a joint that can 
# continuously rotate in a plane.  RevoluteJoints can be used to model motors
# for wheels.
#

RevoluteJoint : Servo {

  + variables:
      target (double).   # The target angle for the servo
      epsilon (double).  # The largest acceptable difference between target
                         # and actual angle

  + to init:
      target = 0.        # Initialize instance variables
      epsilon = 0.05.    


  #
  # The next two lines of code define a method named set-target-angle.
  # It takes a single argument named angle which is of type double.
  # Given a Servo object named, for example, servo, the following line
  # of code sets the target angle to -1 radians:
  #
  #   servo set-target-angle to -1
  #
  # Note that in the method declaration we have "to angle (double)".  That
  # says that there is an argument referred to inside the method as angle
  # which is of type double, and the value specified for that argument when
  # the method is called must be preceded by the word to.

  + to set-target-angle to angle (double):
      target = angle.


  # 
  # This method determines whether the servo's current position is close
  # enough to the target angle.  If it is, the method returns 1, otherwise it
  # returns 0.
  # 

  + to at-target-angle :
      difference (double).

      difference = target - (self get-joint-angle).
      if (max(difference, -difference) > epsilon) :
	return 0.
      else
        return 1.


  #
  # Every class can have an iterate method that defines how instances of that
  # class behave.  When breve runs a simulation, it repeatedly loops over all 
  # objects that have been created and calls their iterate method.
  #

  + to iterate:

      # Test to see if servo is at target angle already
      if ((self at-target-angle) == 0) : {       

        # If it isn't, we need to move joint
        if (target > (self get-joint-angle)) :
          self set-joint-velocity to 1.
        else
          self set-joint-velocity to -1.

      } else

        # If it is, don't move the joint
	self set-joint-velocity to 0.
}