/// <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); }