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 Initialize()
{
this.busReceiveQueue = new Queue();
this.deviceResetQueue = new Queue();
this.reelMotor = new ElmoWhistleMotor("reel motor", (byte)ParameterAccessor.Instance.TruckBus.ReelMotorBusId);
this.reelDigitalIo = new PeakDigitalIo("reel digital IO", (byte)ParameterAccessor.Instance.TruckBus.ReelDigitalBusId);
this.reelAnalogIo = new PeakAnalogIo("reel analog IO", (byte)ParameterAccessor.Instance.TruckBus.ReelAnalogBusId);
this.reelEncoder = new KublerRotaryEncoder("reel encoder", (byte)ParameterAccessor.Instance.TruckBus.ReelEncoderBusId);
this.feederTopFrontMotor = new ElmoWhistleMotor("feeder tf-motor", (byte)ParameterAccessor.Instance.TruckBus.FeederTopFrontMotorBusId);
this.feederTopRearMotor = new ElmoWhistleMotor("feeder tr-motor", (byte)ParameterAccessor.Instance.TruckBus.FeederTopRearMotorBusId);
this.feederBottomFrontMotor = new ElmoWhistleMotor("feeder bf-motor", (byte)ParameterAccessor.Instance.TruckBus.FeederBottomFrontMotorBusId);
this.feederBottomRearMotor = new ElmoWhistleMotor("feeder br-motor", (byte)ParameterAccessor.Instance.TruckBus.FeederBottomRearMotorBusId);
this.feederEncoder = new KublerRotaryEncoder("feeder encoder", (byte)ParameterAccessor.Instance.TruckBus.FeederEncoderBusId);
this.guideLeftMotor = new ElmoWhistleMotor("guide l-motor", (byte)ParameterAccessor.Instance.TruckBus.GuideLeftMotorBusId);
this.guideRightMotor = new ElmoWhistleMotor("guide r-motor", (byte)ParameterAccessor.Instance.TruckBus.GuideRightMotorBusId);
this.launchDigitalIo = new PeakDigitalIo("launch digital IO", (byte)ParameterAccessor.Instance.TruckBus.LaunchDigitalIoBusId);
this.launchAnalogIo = new PeakAnalogIo("launch analog IO", (byte)ParameterAccessor.Instance.TruckBus.LaunchAnalogIoBusId);
this.gps = new UlcRoboticsGps("gps", (byte)ParameterAccessor.Instance.TruckBus.GpsBusId);
if (RobotApplications.repair == ParameterAccessor.Instance.RobotApplication)
{
this.frontPumpMotor = new ElmoWhistleMotor("front pump motor", (byte)ParameterAccessor.Instance.TruckBus.FrontPumpBusId);
this.frontScaleRs232 = new UlcRoboticsRs232("front scale rs232", (byte)ParameterAccessor.Instance.TruckBus.FrontScaleRs232BusId);
this.frontScaleRs232.OnSerialReceive = new UlcRoboticsRs232.SerialReceiveHandler(FgDigitalScale.Front.Receive);
this.rearPumpMotor = new ElmoWhistleMotor("rear pump motor", (byte)ParameterAccessor.Instance.TruckBus.RearPumpBusId);
this.rearScaleRs232 = new UlcRoboticsRs232("rear scale rs232", (byte)ParameterAccessor.Instance.TruckBus.RearScaleRs232BusId);
this.rearScaleRs232.OnSerialReceive = new UlcRoboticsRs232.SerialReceiveHandler(FgDigitalScale.Rear.Receive);
}
this.deviceList = new ArrayList();
this.deviceList.Add(this.reelMotor);
this.deviceList.Add(this.reelEncoder);
this.deviceList.Add(this.feederTopFrontMotor);
this.deviceList.Add(this.feederTopRearMotor);
this.deviceList.Add(this.feederBottomFrontMotor);
this.deviceList.Add(this.feederBottomRearMotor);
this.deviceList.Add(this.feederEncoder);
this.deviceList.Add(this.guideLeftMotor);
this.deviceList.Add(this.guideRightMotor);
this.deviceList.Add(this.reelDigitalIo);
this.deviceList.Add(this.launchDigitalIo);
this.deviceList.Add(this.reelAnalogIo);
this.deviceList.Add(this.launchAnalogIo);
this.deviceList.Add(this.gps);
if (RobotApplications.repair == ParameterAccessor.Instance.RobotApplication)
{
this.deviceList.Add(this.frontPumpMotor);
this.deviceList.Add(this.frontScaleRs232);
this.deviceList.Add(this.rearPumpMotor);
this.deviceList.Add(this.rearScaleRs232);
}
foreach (Device device in this.deviceList)
{
device.OnReceiveTrace = new Device.ReceiveTraceHandler(this.DeviceTraceReceive);
device.OnTransmitTrace = new Device.TransmitTraceHandler(this.DeviceTraceTransmit);
device.OnTransmit = new Device.TransmitHandler(this.DeviceTransmit);
device.OnCommError = new Device.CommErrorHandler(this.DeviceError);
}
this.reelDigitalIo.OnInputChange = new PeakDigitalIo.InputChangeHandler(this.DigitalIoInputChangeHandler);
this.launchDigitalIo.OnInputChange = new PeakDigitalIo.InputChangeHandler(this.DigitalIoInputChangeHandler);
this.feederTopFrontStatus = new FeederMotorStatus();
this.feederTopRearStatus = new FeederMotorStatus();
this.feederBottomFrontStatus = new FeederMotorStatus();
this.feederBottomRearStatus = new FeederMotorStatus();
this.guideLeftStatus = new GuideMotorStatus();
this.guideRightStatus = new GuideMotorStatus();
}
public bool EvaluateFeederMotorLockMode(FeederMotorStatus status)
{
bool result = false;
if (ElmoWhistleMotor.ControlModes.microStepper == status.requestedControlMode)
{
result = true;
}
return (result);
}
private void EvaluateFeederMotorLockCurrent(ElmoWhistleMotor motor, FeederMotorStatus status, ref double total, ref int count)
{
if ((null == motor.FaultReason) &&
(ElmoWhistleMotor.ControlModes.microStepper == status.requestedControlMode))
{
total += motor.Torque;
count++;
}
}
private void EvaluateFeederMotorVelocity(ElmoWhistleMotor motor, FeederMotorStatus status, FeederMotorParameters parameters, ref double total, ref int count)
{
if ((null == motor.FaultReason) &&
(MotorStates.Enabled == parameters.State) &&
(ElmoWhistleMotor.ControlModes.singleLoopPosition == status.requestedControlMode))
{
double motorContribution = motor.RPM;
if (status.requestedMode == ElmoWhistleMotor.Modes.current)
{
motorContribution = motor.Torque * 1000 / ParameterAccessor.Instance.FeederCurrentPer1kRPM.OperationalValue;
}
int settingInversionValue = (MotorDirections.Normal == parameters.Direction) ? 1 : -1;
int positionInversionValue = (false == parameters.PositionInversion) ? 1 : -1;
total += (motorContribution * settingInversionValue * positionInversionValue) / ParameterAccessor.Instance.FeederVelocityToRpm;
count++;
}
}
