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