javaclient2.extra
Class HeadingControl

java.lang.Object
  extended by javaclient2.extra.Controller
      extended by javaclient2.extra.PIDController
          extended by javaclient2.extra.HeadingControl

public class HeadingControl
extends PIDController

Heading control interface for Position, Position2D and Position3D Player interfaces. Uses methods from both player interfaces and PIDController.

Author:
Radu Bogdan Rusu & Marius Borodi

Field Summary
 
Fields inherited from class javaclient2.extra.PIDController
kd, ki, kp
 
Fields inherited from class javaclient2.extra.Controller
currE, eSum, goal, lastE
 
Constructor Summary
HeadingControl(AbstractPositionDevice pd)
          Constructor for HeadingControl.
HeadingControl(AbstractPositionDevice pd, int minC, int maxC)
          Constructor for HeadingControl.
HeadingControl(AbstractPositionDevice pd, int kp, int ki, int kd)
          Constructor for HeadingControl.
HeadingControl(AbstractPositionDevice pd, int minC, int maxC, int kp, int ki, int kd)
          Constructor for HeadingControl.
 
Method Summary
 float getCommand(float currentOutput)
          Calculate and return the controller's command for the controlled system.
 void setAllowedError(float err)
          Set the maximum allowed error between the final goal and the current position.
 boolean setDiffHeading(float angle)
          Rotate the robot on spot (differential heading) with a desired heading.
 boolean setHeading(float angle)
          Rotate the robot on spot (absolute heading) to the desired heading.
 void setMaximumCommand(float maxC)
          Set the maximum admissible command for the robot's motors.
 void setMinimumCommand(float minC)
          Set the minimum admissible command for the robot's motors.
 void stopRobot()
          Stop the robot from moving.
 
Methods inherited from class javaclient2.extra.PIDController
getKd, getKi, getKp, setKd, setKi, setKp
 
Methods inherited from class javaclient2.extra.Controller
deltaE, setGoal
 
Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
 

Constructor Detail

HeadingControl

public HeadingControl(AbstractPositionDevice pd)
Constructor for HeadingControl.

Parameters:
pd - a reference to a PlayerDevice interface (Position, Position2D or Position3D).

HeadingControl

public HeadingControl(AbstractPositionDevice pd,
                      int kp,
                      int ki,
                      int kd)
Constructor for HeadingControl.

Parameters:
pd - a reference to a PlayerDevice interface (Position, Position2D or Position3D).
kp - the proportional constant
ki - the integral constant
kd - the derivative constant

HeadingControl

public HeadingControl(AbstractPositionDevice pd,
                      int minC,
                      int maxC)
Constructor for HeadingControl.

Parameters:
pd - a reference to a PlayerDevice interface (Position, Position2D or Position3D).
minC - minimum admissible command for the robot's motors
maxC - maximum admissible command for the robot's motors

HeadingControl

public HeadingControl(AbstractPositionDevice pd,
                      int minC,
                      int maxC,
                      int kp,
                      int ki,
                      int kd)
Constructor for HeadingControl.

Parameters:
pd - a reference to a PlayerDevice interface (Position, Position2D or Position3D).
minC - minimum admissible command for the robot's motors
maxC - maximum admissible command for the robot's motors
kp - the proportional constant
ki - the integral constant
kd - the derivative constant
Method Detail

setMinimumCommand

public void setMinimumCommand(float minC)
Set the minimum admissible command for the robot's motors.

Parameters:
minC - minimum admissible command as an integer

setMaximumCommand

public void setMaximumCommand(float maxC)
Set the maximum admissible command for the robot's motors.

Parameters:
maxC - maximum admissible command as an integer

stopRobot

public void stopRobot()
Stop the robot from moving.


setAllowedError

public void setAllowedError(float err)
Set the maximum allowed error between the final goal and the current position. (default error is 0)

Parameters:
err - maximum allowed error as an integer

getCommand

public float getCommand(float currentOutput)
Calculate and return the controller's command for the controlled system.

Overrides:
getCommand in class PIDController
Parameters:
currentOutput - the current output of the system
Returns:
the new calculated command for the system

setDiffHeading

public boolean setDiffHeading(float angle)
Rotate the robot on spot (differential heading) with a desired heading.

Parameters:
angle - angle for rotation
Returns:
false in case the rotation was interrupted, true otherwise

setHeading

public boolean setHeading(float angle)
Rotate the robot on spot (absolute heading) to the desired heading.

Parameters:
angle - goal angle
Returns:
false in case the rotation was interrupted, true otherwise