/// <summary> /// Sets the desired position for the inner and outer axes. /// </summary> /// <param name="command">The motion command, containing the positions</param> internal abstract void SetPosition(MotionCommand command);
/// <summary> /// Sets the desired velocity for the inner and outer axes. /// </summary> /// <param name="command">The motion command, containing the velocities</param> internal abstract void SetVelocity(MotionCommand command);
/// <summary>
/// Initializes the set of motion commands for the predefined positions (Hang, Mayday, Loading, etc).
/// </summary>
private static void InitializeMotionStates()
{
logger.Debug("Enter: InitializeMotionStates()");
lastCommand = new MotionCommand();
backCommand = new MotionCommand();
if (mount == MountType.Back)
{
backCommand.outerPosition = 180.0;
backCommand.innerPosition = 180.0;
}
else
{
backCommand.outerPosition = 90.0;
backCommand.innerPosition = 0.0;
}
backCommand.outerVelocity = NORMAL_VELOCITY;
backCommand.innerVelocity = NORMAL_VELOCITY;
backCommand.outerAcceleration = NORMAL_ACCELERATION;
backCommand.innerAcceleration = NORMAL_ACCELERATION;
frontCommand = new MotionCommand();
if (mount == MountType.Back)
{
frontCommand.outerPosition = 0;
frontCommand.innerPosition = 0;
}
else
{
frontCommand.outerPosition = 90.0;
frontCommand.innerPosition = 180.0;
}
frontCommand.outerVelocity = NORMAL_VELOCITY;
frontCommand.innerVelocity = NORMAL_VELOCITY;
frontCommand.outerAcceleration = NORMAL_ACCELERATION;
frontCommand.innerAcceleration = NORMAL_ACCELERATION;
hangCommand = new MotionCommand {
outerPosition = 90.0,
innerPosition = 90.0,
outerVelocity = NORMAL_VELOCITY,
innerVelocity = NORMAL_VELOCITY,
outerAcceleration = NORMAL_ACCELERATION,
innerAcceleration = NORMAL_ACCELERATION
};
loadingCommand = new MotionCommand();
if (mount == MountType.Back)
{
loadingCommand.outerPosition = 5.0;
loadingCommand.innerPosition = 0.0;
}
else
{
loadingCommand.outerPosition = 95.0;
loadingCommand.innerPosition = 180.0;
}
loadingCommand.outerVelocity = NORMAL_VELOCITY;
loadingCommand.innerVelocity = NORMAL_VELOCITY;
loadingCommand.outerAcceleration = NORMAL_ACCELERATION;
loadingCommand.innerAcceleration = NORMAL_ACCELERATION;
maydayCommand = new MotionCommand();
if (mount == MountType.Back)
{
maydayCommand.outerPosition = 340.0;
maydayCommand.innerPosition = 0.0;
}
else
{
maydayCommand.outerPosition = 60.0;
maydayCommand.innerPosition = 180.0;
}
maydayCommand.outerVelocity = MAYDAY_VELOCITY;
maydayCommand.innerVelocity = MAYDAY_VELOCITY;
maydayCommand.outerAcceleration = MAYDAY_ACCELERATION;
maydayCommand.innerAcceleration = MAYDAY_ACCELERATION;
uprightCommand = new MotionCommand();
if (mount == MountType.Back)
{
uprightCommand.outerPosition = 0.0;
uprightCommand.innerPosition = 0.0;
}
else
{
uprightCommand.outerPosition = 90.0;
uprightCommand.innerPosition = 180.0;
}
uprightCommand.outerVelocity = NORMAL_VELOCITY;
uprightCommand.innerVelocity = NORMAL_VELOCITY;
uprightCommand.outerAcceleration = NORMAL_ACCELERATION;
uprightCommand.innerAcceleration = NORMAL_ACCELERATION;
}
/// <summary> /// Sets the desired acceleration for the inner and outer axes. /// </summary> /// <param name="command">The motion command, containing the accelerations</param> internal abstract void SetAcceleration(MotionCommand command);
/// <summary>
/// Sets the desired velocity for the inner and outer axes.
/// </summary>
/// <param name="command">The motion command, containing the velocities</param>
internal override void SetVelocity(MotionCommand command)
{
targetMotion.innerVelocity = command.innerVelocity;
targetMotion.outerVelocity = command.outerVelocity;
}
/// <summary>
/// Sets the desired position for the inner and outer axes.
/// </summary>
/// <param name="command">The motion command, containing the positions</param>
internal override void SetPosition(MotionCommand command)
{
targetMotion.innerPosition = command.innerPosition;
targetMotion.outerPosition = command.outerPosition;
}
/// <summary>
/// Sets the desired acceleration for the inner and outer axes.
/// </summary>
/// <param name="command">The motion command, containing the accelerations</param>
internal override void SetAcceleration(MotionCommand command)
{
targetMotion.innerAcceleration = command.innerAcceleration;
targetMotion.outerAcceleration = command.outerAcceleration;
}
public MotionData(MotionCommand c, long t)
{
command = c;
milliSecondsElapsed = t;
}
/// <summary>
/// Updates the internal state of the motion controller
/// </summary>
/// <param name="interval"></param>
private void _updateHulkState()
{
long interval = stopwatch.ElapsedMilliseconds - lastHulkStatusUpdate;
lastHulkStatusUpdate = lastHulkStatusUpdate + interval;
if (
(Hulk.SystemStatus == Hulk.Status.Idling) ||
(Hulk.SystemStatus == Hulk.Status.Homing) ||
(Hulk.SystemStatus == Hulk.Status.Initializing) ||
(Hulk.SystemStatus == Hulk.Status.NeedsHoming) ||
(Hulk.SystemStatus == Hulk.Status.Offline) ||
(Hulk.SystemStatus == Hulk.Status.Stopping)
)
{
return;
}
// figure out what the HULK current motion is
MotionCommand cm = currentMotion;
if (delayInMs != 0)
{
lock (lockObject)
{
if ((commandBuffer.Count == 0))
{
logger.Warn("_updateHulkState() empty commandbuffer detected, while the delayInMs is " + delayInMs.ToString() + ". Using currentMotion to determine newMotion.");
}
else
{
cm = commandBuffer[commandBuffer.Count - 1].command;
}
}
}
MotionCommand newMotion = new MotionCommand(cm);
switch (currentCommandType)
{
case CommandType.Velocity:
// velocity command; just apply the velocity
newMotion.innerVelocity = targetMotion.innerVelocity;
newMotion.outerVelocity = targetMotion.outerVelocity;
newMotion.innerPosition += newMotion.innerVelocity * interval * 1.0f / 1000;
newMotion.outerPosition += newMotion.outerVelocity * interval * 1.0f / 1000;
break;
case CommandType.ModulusPosition:
{
// modulus position command; figure out what the new position should be
// what is the current difference in position
double deltaPosInner = targetMotion.innerPosition - newMotion.innerPosition;
double deltaPosOuter = targetMotion.outerPosition - newMotion.outerPosition;
// what should the velocity be
double deltaVelInner = Math.Sign(deltaPosInner) * targetMotion.innerVelocity * interval * 1.0f / 1000;
double deltaVelOuter = Math.Sign(deltaPosOuter) * targetMotion.outerVelocity * interval * 1.0f / 1000;
// apply velocity
if (Math.Abs(deltaPosInner) > Math.Abs(deltaVelInner))
{
newMotion.innerPosition += deltaVelInner;
newMotion.innerVelocity = targetMotion.innerVelocity;
}
else
{
newMotion.innerPosition = targetMotion.innerPosition;
targetMotion.innerVelocity = 0;
newMotion.innerVelocity = 0;
}
// apply velocity
if (Math.Abs(deltaPosOuter) > Math.Abs(deltaVelOuter))
{
newMotion.outerPosition += deltaVelOuter;
newMotion.outerVelocity = targetMotion.outerVelocity;
}
else
{
newMotion.outerPosition = targetMotion.outerPosition;
targetMotion.outerVelocity = 0;
newMotion.outerVelocity = 0;
}
}
break;
}
_processCommand(newMotion);
}
