private void UpdatePumpMotor(ElmoWhistleMotor motor, double setPoint, ref ElmoWhistleMotor.Modes requestedMode, ref double requestedSetPoint) { if (null == motor.FaultReason) { bool modeChange = false; ElmoWhistleMotor.Modes neededMode = ElmoWhistleMotor.Modes.undefined; double neededValue = 0; if (0 == setPoint) { if (0 == requestedSetPoint) { neededMode = ElmoWhistleMotor.Modes.off; neededValue = 0; } else { neededMode = ElmoWhistleMotor.Modes.velocity; neededValue = 0; } } else { neededMode = ElmoWhistleMotor.Modes.velocity; neededValue = setPoint; } if (neededMode != requestedMode) { motor.SetMode(neededMode); requestedMode = neededMode; modeChange = true; Tracer.WriteMedium(TraceGroup.TBUS, null, "{0} {1}", motor.Name, requestedMode.ToString()); } if (ElmoWhistleMotor.Modes.velocity == requestedMode) { if ((neededValue != requestedSetPoint) || (false != modeChange)) { motor.SetVelocity((int)neededValue); requestedSetPoint = neededValue; Tracer.WriteMedium(TraceGroup.TBUS, null, "{0} velocity {1} {2}", motor.Name, neededValue, requestedSetPoint); } } } }
private bool UpdateFeederMotor(ElmoWhistleMotor motor, FeederMotorStatus status, FeederMotorParameters parameters) { bool scheduled = false; if (null == motor.FaultReason) { bool modeChange = false; ElmoWhistleMotor.ControlModes neededControlMode = ElmoWhistleMotor.ControlModes.singleLoopPosition; ElmoWhistleMotor.Modes neededMode = ElmoWhistleMotor.Modes.undefined; double neededValue = 0; double inversionValue = (MotorDirections.Normal == parameters.Direction) ? 1 : -1; inversionValue *= (false == parameters.PositionInversion) ? 1 : -1; if (MotorStates.Disabled == parameters.State) { neededControlMode = ElmoWhistleMotor.ControlModes.singleLoopPosition; if (0 == status.requestedVelocity) { neededMode = ElmoWhistleMotor.Modes.off; neededValue = 0; } else { neededMode = ElmoWhistleMotor.Modes.velocity; neededValue = 0; } } else if (MotorStates.Enabled == parameters.State) { if (FeederModes.off == this.feederModeSetPoint) { neededControlMode = ElmoWhistleMotor.ControlModes.singleLoopPosition; if (0 == status.requestedVelocity) { neededMode = ElmoWhistleMotor.Modes.off; neededValue = 0; } else { neededMode = ElmoWhistleMotor.Modes.velocity; neededValue = 0; } } else if (FeederModes.move == this.feederModeSetPoint) { neededControlMode = ElmoWhistleMotor.ControlModes.singleLoopPosition; bool velocityMode = true; if (((false != parameters.PositivePusher) && (this.feederVelocitySetPoint < 0)) || ((false == parameters.PositivePusher) && (this.feederVelocitySetPoint > 0))) { velocityMode = false; } if (false != velocityMode) { neededMode = ElmoWhistleMotor.Modes.velocity; neededValue = this.feederVelocitySetPoint; } else { double neededCurrent = ParameterAccessor.Instance.FeederCurrentPer1kRPM.OperationalValue * this.feederVelocitySetPoint * ParameterAccessor.Instance.FeederVelocityToRpm / 1000; neededMode = ElmoWhistleMotor.Modes.current; neededValue = neededCurrent; } } else if (FeederModes.locked == this.feederModeSetPoint) { neededControlMode = ElmoWhistleMotor.ControlModes.microStepper; neededMode = ElmoWhistleMotor.Modes.current; neededValue = ParameterAccessor.Instance.FeederLockCurrent.OperationalValue; inversionValue = 1.0; } } else if (MotorStates.Locked == parameters.State) { neededControlMode = ElmoWhistleMotor.ControlModes.microStepper; neededMode = ElmoWhistleMotor.Modes.current; neededValue = ParameterAccessor.Instance.FeederLockCurrent.OperationalValue; inversionValue = 1.0; } if ((neededControlMode != status.requestedControlMode) || (neededMode != status.requestedMode)) { bool requestedZero = false; bool atZero = false; if ((ElmoWhistleMotor.Modes.undefined == status.requestedMode) || (ElmoWhistleMotor.Modes.off == status.requestedMode) || ((ElmoWhistleMotor.Modes.velocity == status.requestedMode) && (0 == status.requestedVelocity)) || ((ElmoWhistleMotor.Modes.current == status.requestedMode) && (0 == status.requestedCurrent))) { requestedZero = true; } if ((ElmoWhistleMotor.Modes.undefined == status.requestedMode) || (ElmoWhistleMotor.Modes.off == status.requestedMode) || ((ElmoWhistleMotor.Modes.velocity == status.requestedMode) && (0 == motor.RPM)) || ((ElmoWhistleMotor.Modes.current == status.requestedMode) && (0 == motor.Torque))) { atZero = true; } if ((false == requestedZero) || (false == atZero)) { neededValue = 0; } else { motor.SetControlMode(neededControlMode); status.requestedControlMode = neededControlMode; if (ElmoWhistleMotor.ControlModes.singleLoopPosition == status.requestedControlMode) { motor.SetMode(neededMode); status.requestedMode = neededMode; } else { status.requestedMode = neededMode; } Tracer.WriteMedium(TraceGroup.TBUS, null, "{0} control={1}, mode={2}", motor.Name, status.requestedControlMode, status.requestedMode); modeChange = true; } } if (((neededControlMode == status.requestedControlMode) && (neededMode == status.requestedMode)) || (0 == neededValue)) { if (ElmoWhistleMotor.ControlModes.singleLoopPosition == status.requestedControlMode) { if (ElmoWhistleMotor.Modes.velocity == status.requestedMode) { if ((neededValue != status.requestedVelocity) || (false != modeChange) || (false != this.evaluateFeederParameters)) { int velocityRpm = (int)(inversionValue * neededValue * ParameterAccessor.Instance.FeederVelocityToRpm); motor.ScheduleVelocity(velocityRpm); scheduled = true; status.requestedVelocity = neededValue; Tracer.WriteMedium(TraceGroup.TBUS, null, "{0} velocity {1:0.00} {2}", motor.Name, neededValue, velocityRpm); } } else if (ElmoWhistleMotor.Modes.current == status.requestedMode) { if ((neededValue != status.requestedCurrent) || (false != modeChange) || (false != this.evaluateFeederParameters)) { float torqueCurrent = (float)(inversionValue * neededValue); motor.ScheduleTorque(torqueCurrent); scheduled = true; status.requestedCurrent = neededValue; Tracer.WriteMedium(TraceGroup.TBUS, null, "{0} current {1:0.000} {2:0.000}", motor.Name, neededValue, torqueCurrent); } } else if (ElmoWhistleMotor.Modes.off == status.requestedMode) { if (false != modeChange) { motor.ScheduleVelocity(0); scheduled = true; status.requestedVelocity = 0; Tracer.WriteMedium(TraceGroup.RBUS, null, "{0} velocity=0 rpm=0", motor.Name, 0, 0); } } } else if (ElmoWhistleMotor.ControlModes.microStepper == status.requestedControlMode) { if ((neededValue != status.requestedCurrent) || (false != modeChange)) { float torqueCurrent = (float)neededValue; motor.SetStepperCurrent(torqueCurrent); status.requestedCurrent = neededValue; Tracer.WriteMedium(TraceGroup.TBUS, null, "{0} lock current {1}", motor.Name, neededValue); } } } } return (scheduled); }
private void StartGuideMotor(ElmoWhistleMotor motor) { if (null == motor.FaultReason) { motor.OnInputChange = new ElmoWhistleMotor.InputChangeHandler(this.ElmoMotorInputChangeHandler); motor.SetConsumerHeartbeat((UInt16)ParameterAccessor.Instance.TruckBus.ConsumerHeartbeatRate, (byte)ParameterAccessor.Instance.TruckBus.ControllerBusId); motor.SetProducerHeartbeat((UInt16)ParameterAccessor.Instance.TruckBus.ProducerHeartbeatRate); motor.Start(); motor.SetMode(ElmoWhistleMotor.Modes.velocity); Tracer.WriteMedium(TraceGroup.TBUS, null, "{0} started", motor.Name); } }