private void SetFeederMotorState(FeederMotorParameters parameters, int selectedOption)
{
if (1 == selectedOption)
{
parameters.State = MotorStates.Enabled;
}
else if (2 == selectedOption)
{
parameters.State = MotorStates.Disabled;
}
else
{
parameters.State = MotorStates.Locked;
}
}
private void SetFeederMotorDirection(FeederMotorParameters parameters, bool selectedOption)
{
if (false != selectedOption)
{
parameters.Direction = MotorDirections.Normal;
}
else
{
parameters.Direction = MotorDirections.Inverse;
}
}
private void WriteFeederMotorParameters(XmlWriter writer, FeederMotorParameters feederMotorParameters)
{
writer.WriteStartElement("FeederMotor");
this.WriteElement(writer, "Location", feederMotorParameters.Location);
this.WriteElement(writer, "State", feederMotorParameters.State.ToString());
this.WriteElement(writer, "Direction", feederMotorParameters.Direction.ToString());
this.WriteElement(writer, "PositivePusher", feederMotorParameters.PositivePusher);
this.WriteElement(writer, "PositionInversion", feederMotorParameters.PositionInversion);
writer.WriteEndElement();
}
private void SetFeederButtons(ValueCycleButton stateButton, ValueToggleButton directionButton, FeederMotorParameters parameters)
{
int stateOption = 0;
if (MotorStates.Enabled == parameters.State)
{
stateOption = 1;
}
else if (MotorStates.Disabled == parameters.State)
{
stateOption = 2;
}
else if (MotorStates.Locked == parameters.State)
{
stateOption = 3;
}
stateButton.SelectedOption = stateOption;
bool directionOption = false;
if (MotorDirections.Normal == parameters.Direction)
{
directionOption = true;
}
else
{
directionOption = false;
}
directionButton.OptionASelected = directionOption;
}
private void ReadData(string filePath)
{
try
{
using (XmlReader reader = XmlReader.Create(filePath))
{
bool result = true;
#region Drill Read
{
for (; result;)
{
result = reader.Read();
if (reader.IsStartElement())
{
switch (reader.Name)
{
case "TruckBus":
{
TruckBusParameters truckBusParameters = this.ReadTruckBusParameters(reader);
if (null != truckBusParameters)
{
this.TruckBus = truckBusParameters;
}
break;
}
case "RobotBus":
{
RobotBusParameters robotBusParameters = this.ReadRobotBusParameters(reader);
if (null != robotBusParameters)
{
this.RobotBus = robotBusParameters;
}
break;
}
case "IpEndpoint":
{
IpEndpointParameters ipEndpointParameters = this.ReadIpEndpoint(reader);
if (null != ipEndpointParameters)
{
switch (ipEndpointParameters.Name)
{
case "Trace":
{
this.Trace = ipEndpointParameters;
break;
}
case "LocationServer":
{
this.LocationServer = ipEndpointParameters;
break;
}
case "ThicknessSensor":
{
this.ThicknessSensor = ipEndpointParameters;
break;
}
case "StressSensor":
{
this.StressSensor = ipEndpointParameters;
break;
}
}
}
break;
}
case "DigitalScale":
{
DigitalScaleParameters digitalScaleParameters = this.ReadDigitalScaleParameters(reader);
if (null != digitalScaleParameters)
{
switch (digitalScaleParameters.Location)
{
case "FrontScale":
{
this.FrontScale = digitalScaleParameters;
break;
}
case "RearScale":
{
this.RearScale = digitalScaleParameters;
break;
}
}
}
break;
}
case "Caution":
{
CautionParameter cautionParameter = this.ReadCautionParameters(reader);
if (null != cautionParameter)
{
switch (cautionParameter.Name)
{
case "NitrogenPressureCaution":
{
this.NitrogenPressureCaution = cautionParameter;
break;
}
case "FeederCurrentCaution":
{
this.FeederCurrentCaution = cautionParameter;
break;
}
case "MovementCurrentCaution":
{
this.MovementCurrentCaution = cautionParameter;
break;
}
case "MovementTemperatureCaution":
{
this.MovementTemperatureCaution = cautionParameter;
break;
}
}
}
break;
}
case "Value":
{
ValueParameter valueParameter = this.ReadValueParameters(reader);
if (null != valueParameter)
{
switch (valueParameter.Name)
{
case "ThicknessConversionUnit":
{
this.ThicknessConversionUnit = valueParameter;
break;
}
case "StressConversionUnit":
{
this.StressConversionUnit = valueParameter;
break;
}
case "NitrogenPressureConversionUnit":
{
this.NitrogenPressureConversionUnit = valueParameter;
break;
}
case "RobotTotalCurrentConversionUnit":
{
this.RobotTotalCurrentConversionUnit = valueParameter;
break;
}
case "LaunchTotalCurrentConversionUnit":
{
this.LaunchTotalCurrentConversionUnit = valueParameter;
break;
}
case "GuideExtensionSpeed":
{
this.GuideExtensionSpeed = valueParameter;
break;
}
case "GuideRetractionSpeed":
{
this.GuideRetractionSpeed = valueParameter;
break;
}
case "ReelDistance":
{
this.ReelDistance = valueParameter;
break;
}
case "ReelDistanceScale":
{
this.ReelDistanceScale = valueParameter;
break;
}
case "ReelReverseCurrent":
{
this.ReelReverseCurrent = valueParameter;
break;
}
case "ReelLockCurrent":
{
this.ReelLockCurrent = valueParameter;
break;
}
case "ReelCalibrationDistance":
{
this.ReelCalibrationDistance = valueParameter;
break;
}
case "ReelManualCurrent":
{
this.ReelManualCurrent = valueParameter;
break;
}
case "ReelReverseSpeed":
{
this.ReelReverseSpeed = valueParameter;
break;
}
case "ReelManualSpeed":
{
this.ReelManualSpeed = valueParameter;
break;
}
case "FeederTrackingCalibration":
{
this.FeederTrackingCalibration = valueParameter;
break;
}
case "FeederMaxSpeed":
{
this.FeederMaxSpeed = valueParameter;
break;
}
case "FeederLowSpeedScale":
{
this.FeederLowSpeedScale = valueParameter;
break;
}
case "FeederLockCurrent":
{
this.FeederLockCurrent = valueParameter;
break;
}
case "FeederCurrentPer1kRPM":
{
this.FeederCurrentPer1kRPM = valueParameter;
break;
}
case "FeederManualSpeed":
{
this.FeederManualSpeed = valueParameter;
break;
}
case "MovementMotorLockCurrent":
{
this.MovementMotorLockCurrent = valueParameter;
break;
}
case "MovementMotorMaxCurrent":
{
this.MovementMotorMaxCurrent = valueParameter;
break;
}
case "MovementMotorMaxSpeed":
{
this.MovementMotorMaxSpeed = valueParameter;
break;
}
case "MovementMotorCurrentPer1kRPM":
{
this.MovementMotorCurrentPer1kRPM = valueParameter;
break;
}
case "MovementMotorLowSpeedScale":
{
this.MovementMotorLowSpeedScale = valueParameter;
break;
}
case "MovementMotorManualJogDistance":
{
this.MovementMotorManualJogDistance = valueParameter;
break;
}
case "MovementMotorManualMoveSpeed":
{
this.MovementMotorManualMoveSpeed = valueParameter;
break;
}
}
}
break;
}
case "RobotApplication":
{
this.RobotApplication = ReadRobotApplication(reader);
break;
}
case "EnableRobotBus":
{
this.EnableRobotBus = this.ReadBool(reader);
break;
}
case "JoystickDeadband":
{
this.JoystickDeadband = this.ReadInt(reader);
break;
}
case "Latitude":
{
this.Latitude = this.ReadDouble(reader);
break;
}
case "Longitude":
{
this.Longitude = this.ReadDouble(reader);
break;
}
case "GuideMomentaryButtonAction":
{
this.ReadElement(reader, ref this.GuideMomentaryButtonAction);
break;
}
case "ReelMotionMode":
{
this.ReelMotionMode = this.ReadMotorForwardControl(reader);
break;
}
case "ReelVelocityToRpm":
{
this.ReelVelocityToRpm = this.ReadDouble(reader);
break;
}
case "FeederAutomaticTracking":
{
this.ReadElement(reader, ref this.FeederAutomaticTracking);
break;
}
case "FeederVelocityToRpm":
{
this.FeederVelocityToRpm = this.ReadDouble(reader);
break;
}
case "FeederMotor":
{
FeederMotorParameters feederMotorParameters = this.ReadFeederMotorParameters(reader);
if (null != feederMotorParameters)
{
switch (feederMotorParameters.Location)
{
case "TopFront":
{
this.TopFrontFeederMotor = feederMotorParameters;
break;
}
case "TopRear":
{
this.TopRearFeederMotor = feederMotorParameters;
break;
}
case "BottomFront":
{
this.BottomFrontFeederMotor = feederMotorParameters;
break;
}
case "BottomRear":
{
this.BottomRearFeederMotor = feederMotorParameters;
break;
}
}
}
break;
}
case "MovementMotor":
{
MovementMotorParameters movementMotorParameters = this.ReadMovementMotorParameters(reader);
if (null != movementMotorParameters)
{
switch (movementMotorParameters.Location)
{
case "TopFront":
{
this.TopFrontMovementMotor = movementMotorParameters;
break;
}
case "TopRear":
{
this.TopRearMovementMotor = movementMotorParameters;
break;
}
case "BottomFront":
{
this.BottomFrontMovementMotor = movementMotorParameters;
break;
}
case "BottomRear":
{
this.BottomRearMovementMotor = movementMotorParameters;
break;
}
}
}
break;
}
case "MovementMotorVelocityToRpm":
{
this.MovementMotorVelocityToRpm = this.ReadDouble(reader);
break;
}
case "FrontToolSelected":
{
this.FrontToolSelected = this.ReadBool(reader);
break;
}
case "Drill":
{
DrillParameters drillParameters = this.ReadDrillParameters(reader);
if (null != drillParameters)
{
switch (drillParameters.Location)
{
case "Front":
{
this.FrontDrill = drillParameters;
break;
}
case "Rear":
{
this.RearDrill = drillParameters;
break;
}
}
}
break;
}
case "Pump":
{
PumpParameters pumpParameters = this.ReadPumpParameters(reader);
if (null != pumpParameters)
{
switch (pumpParameters.Location)
{
case "Front":
{
this.FrontPump = pumpParameters;
break;
}
case "Rear":
{
this.RearPump = pumpParameters;
break;
}
}
}
break;
}
case "Osd":
{
OsdParameters osdParameters = this.ReadOsdParameters(reader);
if (null != osdParameters)
{
this.Osd = osdParameters;
}
break;
}
case "Lights":
{
this.ReadLightParameters(reader);
break;
}
}
}
else
{
#if false
switch (reader.Name)
{
case "Drills":
{
break;
}
case "Drill":
{
break;
}
}
#endif
}
}
}
#endregion
reader.Close();
reader.Dispose();
}
}
catch { }
}
private void AssignDefaults(RobotApplications robotApplication = RobotApplications.repair)
{
this.RobotApplication = robotApplication;
this.TruckBus = new TruckBusParameters();
this.TruckBus.BusInterface = BusInterfaces.PCIA;
this.TruckBus.BitRate = 50000;
this.TruckBus.ConsumerHeartbeatRate = 3000;
this.TruckBus.ProducerHeartbeatRate = 1000;
this.TruckBus.ControllerBusId = 80;
this.TruckBus.ReelMotorBusId = 70;
this.TruckBus.ReelDigitalBusId = 17;
this.TruckBus.ReelAnalogBusId = 19;
this.TruckBus.ReelEncoderBusId = 21;
this.TruckBus.FeederTopFrontMotorBusId = 64;
this.TruckBus.FeederTopRearMotorBusId = 65;
this.TruckBus.FeederBottomFrontMotorBusId = 66;
this.TruckBus.FeederBottomRearMotorBusId = 67;
this.TruckBus.FeederEncoderBusId = 22;
this.TruckBus.GuideLeftMotorBusId = 68;
this.TruckBus.GuideRightMotorBusId = 69;
this.TruckBus.LaunchDigitalIoBusId = 16;
this.TruckBus.LaunchAnalogIoBusId = 18;
this.TruckBus.GpsBusId = 20;
this.TruckBus.FrontPumpBusId = 71;
this.TruckBus.FrontScaleRs232BusId = 96;
this.TruckBus.RearPumpBusId = 72;
this.TruckBus.RearScaleRs232BusId = 97;
this.TruckBus.ControllerTraceMask = 0;
this.TruckBus.ReelMotorTraceMask = 1;
this.TruckBus.ReelDigitalTraceMask = 1;
this.TruckBus.ReelAnalogTraceMask = 1;
this.TruckBus.ReelEncoderTraceMask = 1;
this.TruckBus.FeederTopFrontMotorTraceMask = 1;
this.TruckBus.FeederTopRearMotorTraceMask = 1;
this.TruckBus.FeederBottomFrontMotorTraceMask = 1;
this.TruckBus.FeederBottomRearMotorTraceMask = 1;
this.TruckBus.FeederEncoderTraceMask = 1;
this.TruckBus.GuideLeftMotorTraceMask = 1;
this.TruckBus.GuideRightMotorTraceMask = 1;
this.TruckBus.LaunchDigitalIoTraceMask = 1;
this.TruckBus.LaunchAnalogIoTraceMask = 1;
this.TruckBus.GpsTraceMask = 1;
this.TruckBus.FrontPumpTraceMask = 1;
this.TruckBus.FrontScaleRs232TraceMask = 1;
this.TruckBus.RearPumpTraceMask = 1;
this.TruckBus.RearScaleRs232TraceMask = 1;
this.JoystickDeadband = 5000;
this.Latitude = double.NaN;
this.Longitude = double.NaN;
this.RobotBus = new RobotBusParameters();
this.RobotBus.BusInterface = BusInterfaces.PCIB;
this.RobotBus.BitRate = 50000;
this.RobotBus.ConsumerHeartbeatRate = 3000;
this.RobotBus.ProducerHeartbeatRate = 1000;
this.RobotBus.ControllerBusId = 80;
this.RobotBus.RobotBodyBusId = 32;
this.RobotBus.RobotTopFrontWheelBusId = 49;
this.RobotBus.RobotTopRearWheelBusId = 50;
this.RobotBus.RobotBottomFrontWheelBusId = 51;
this.RobotBus.RobotBottomRearWheelBusId = 52;
this.RobotBus.ControllerTraceMask = 0;
this.RobotBus.RobotBodyTraceMask = 1;
this.RobotBus.RobotTopFrontWheelTraceMask = 1;
this.RobotBus.RobotTopRearWheelTraceMask = 1;
this.RobotBus.RobotBottomFrontWheelTraceMask = 1;
this.RobotBus.RobotBottomRearWheelTraceMask = 1;
this.EnableRobotBus = true;
this.FrontScale = new DigitalScaleParameters("FrontScale", 1, 9600);
this.RearScale = new DigitalScaleParameters("RearScale", 2, 9600);
this.Trace = new IpEndpointParameters("Trace", "127.0.0.1", 10000);
this.LocationServer = new IpEndpointParameters("LocationServer", "0.0.0.0", 5050);
this.ThicknessSensor = new IpEndpointParameters("ThicknessSensor", "192.168.1.101", 0);
this.ThicknessConversionUnit = new ValueParameter("ThicknessConversionUnit", "mm", 3, 0, 100, 1, 1, 1);
this.StressSensor = new IpEndpointParameters("StressSensor", "192.168.1.102", 0);
this.StressConversionUnit = new ValueParameter("StressConversionUnit", "MPa", 3, 0, 100, 1, 1, 1);
this.NitrogenPressureConversionUnit = new ValueParameter("NitrogenPressureConversionUnit", "PSI", 3, 0, 1000, 1, 100, 100);
this.NitrogenPressureCaution = new CautionParameter("NitrogenPressureCaution", 450, 425, 375, 350);
this.RobotTotalCurrentConversionUnit = new ValueParameter("RobotTotalCurrentConversionUnit", "A", 2, 0, 1000, 1, 100, 100);
this.LaunchTotalCurrentConversionUnit = new ValueParameter("LaunchTotalCurrentConversionUnit", "A", 2, 0, 1000, 1, 100, 100);
this.GuideExtensionSpeed = new ValueParameter("GuideExtensionSpeed", "RPM", 0, 1, 4500, 1, 1500, 1500);
this.GuideRetractionSpeed = new ValueParameter("GuideRetractionSpeed", "RPM", 0, 1, 4500, 1, 1500, 1500);
this.GuideMomentaryButtonAction = true;
this.ReelDistance = new ValueParameter("ReelDistance", "m", 0, 0, 0, 0, 0, 0);
this.ReelDistanceScale = new ValueParameter("ReelDistanceScale", "m", 6, 0, 1000, 0, 1, 1);
this.ReelMotionMode = MovementForwardControls.current;
this.ReelReverseCurrent = new ValueParameter("ReelReverseCurrent", "A", 1, 0.0, 6.5, 0.1, 5.0, 5.0);
this.ReelLockCurrent = new ValueParameter("ReelLockCurrent", "A", 1, 0.0, 6.5, 0.1, 2.0, 2.0);
this.ReelCalibrationDistance = new ValueParameter("ReelCalibrationDistance", "m", 0, 1, 100, 1, 1, 1);
this.ReelManualCurrent = new ValueParameter("ReelManualCurrent", "A", 1, 0.0, 6.5, 0.1, 5.0, 5.0);
this.ReelReverseSpeed = new ValueParameter("ReelReverseSpeed", "m/MIN", 1, 0.0, 40.0, 0.1, 20.0, 20.0);
this.ReelManualSpeed = new ValueParameter("ReelManualSpeed", "m/MIN", 1, 0.0, 40.0, 0.1, 20, 20);
this.ReelVelocityToRpm = (1000 / 40.0); // 40.0 m/MIN at 1000 RPM
this.FeederAutomaticTracking = false;
this.FeederVelocityToRpm = (4000 / 12.05992); // 12.05992 m/MIN at 4000 RPM
this.FeederTrackingCalibration = new ValueParameter("FeederTrackingCalibration", "%", 1, -100, 100, 0.1, 50, 50);
this.FeederMaxSpeed = new ValueParameter("FeederMaxSpeed", "m/MIN", 2, 0, 12.05992, 1, 12, 12);
this.FeederLowSpeedScale = new ValueParameter("FeederLowSpeedScale", "%", 0, 1, 100, 1, 30, 30);
this.FeederLockCurrent = new ValueParameter("FeederLockCurrent", "A", 1, 0.0, 6.5, 0.1, 2.0, 2.0);
this.FeederCurrentPer1kRPM = new ValueParameter("FeederCurrentPer1kRPM", "A", 1, 0.1, 3.0, 0.1, 0.8, 0.8);
this.FeederManualSpeed = new ValueParameter("FeederManualSpeed", "m/MIN", 2, 0, 12.05992, 1, 7, 7);
this.FeederCurrentCaution = new CautionParameter("FeederCurrentCaution", 6.0, 5.0, 0.0, 0.0);
this.TopFrontFeederMotor = new FeederMotorParameters();
this.SetFeederMotorDefaults(ref this.TopFrontFeederMotor, "TopFront", false, false);
this.TopRearFeederMotor = new FeederMotorParameters();
this.SetFeederMotorDefaults(ref this.TopRearFeederMotor, "TopRear", false, false);
this.BottomFrontFeederMotor = new FeederMotorParameters();
this.SetFeederMotorDefaults(ref this.BottomFrontFeederMotor, "BottomFront", true, true);
this.BottomRearFeederMotor = new FeederMotorParameters();
this.SetFeederMotorDefaults(ref this.BottomRearFeederMotor, "BottomRear", true, true);
this.MovementMotorLockCurrent = new ValueParameter("MovementMotorLockCurrent", "A", 1, 0.1, 3.0, 0.1, 1.0, 1.0);
this.MovementMotorMaxCurrent = new ValueParameter("MovementMotorMaxCurrent", "A", 1, 0.1, 3.0, 0.1, 3.0, 3.0);
this.MovementMotorMaxSpeed = new ValueParameter("MovementMotorMaxSpeed", "m/MIN", 2, 0, 10, 1, 3.5, 3.5);
this.MovementMotorCurrentPer1kRPM = new ValueParameter("MovementMotorCurrentPer1kRPM", "A", 1, 0.1, 3.0, 0.1, 1.0, 1.0);
this.MovementMotorLowSpeedScale = new ValueParameter("MovementMotorLowSpeedScale", "%", 0, 1, 100, 1, 30, 30);
this.MovementMotorManualJogDistance = new ValueParameter("MovementMotorManualJogDistance", "mm", 0, 1, 100, 1, 1, 1);
this.MovementMotorManualMoveSpeed = new ValueParameter("MovementMotorManualMoveSpeed", "m/MIN", 2, 0, 10, 1, 2, 2);
this.MovementMotorVelocityToRpm = (40000.0/19.04); // 19.04m/MIN at 40000 RPM, (0.476mm/(motor revolution)
this.TopFrontMovementMotor = new MovementMotorParameters();
this.SetMovementMotorDefaults(ref this.TopFrontMovementMotor, "TopFront", MovementForwardControls.velocity, MovementForwardControls.velocity, MovementForwardControls.velocity, MovementForwardControls.current);
this.TopRearMovementMotor = new MovementMotorParameters();
this.SetMovementMotorDefaults(ref this.TopRearMovementMotor, "TopRear", MovementForwardControls.velocity, MovementForwardControls.velocity, MovementForwardControls.velocity, MovementForwardControls.current);
this.BottomFrontMovementMotor = new MovementMotorParameters();
this.SetMovementMotorDefaults(ref this.BottomFrontMovementMotor, "BottomFront", MovementForwardControls.velocity, MovementForwardControls.velocity, MovementForwardControls.current, MovementForwardControls.current);
this.BottomRearMovementMotor = new MovementMotorParameters();
this.SetMovementMotorDefaults(ref this.BottomRearMovementMotor, "BottomRear", MovementForwardControls.velocity, MovementForwardControls.velocity, MovementForwardControls.current, MovementForwardControls.current);
this.MovementCurrentCaution = new CautionParameter("MovementCurrentCaution", 15.0, 10.0, 0.0, 0.0);
this.MovementTemperatureCaution = new CautionParameter("MovementTemperatureCaution", 130.0, 100.0, 0.0, 0.0);
this.FrontToolSelected = true;
this.FrontDrill = new DrillParameters();
this.SetDrillDefaults(ref this.FrontDrill, "Front");
this.RearDrill = new DrillParameters();
this.SetDrillDefaults(ref this.RearDrill, "Rear");
this.FrontPump = new PumpParameters();
this.SetPumpDefaults(ref this.FrontPump);
this.RearPump = new PumpParameters();
this.SetPumpDefaults(ref this.RearPump);
this.Osd = new OsdParameters();
this.SetOsdDefaults(ref this.Osd);
this.robotLightLevels = new ValueParameter[12];
for (int i = 0; i < this.robotLightLevels.Length; i++)
{
ValueParameter value = new ValueParameter();
this.SetLightDefaults(ref value);
value.Name = "RobotLightLevel_" + (i + 1).ToString();
this.robotLightLevels[i] = value;
}
this.launchLightLevels = new ValueParameter[4];
for (int i = 0; i < this.launchLightLevels.Length; i++)
{
ValueParameter value = new ValueParameter();
this.SetLightDefaults(ref value);
value.Name = "LaunchLightLevel_" + (i + 1).ToString();
this.launchLightLevels[i] = value;
}
}
private FeederMotorParameters ReadFeederMotorParameters(XmlReader reader)
{
FeederMotorParameters result = new FeederMotorParameters();
this.SetFeederMotorDefaults(ref result, "", false, false);
FeederMotorParameters temp = new FeederMotorParameters();
bool readResult = true;
for (; readResult; )
{
readResult = reader.Read();
if (reader.IsStartElement())
{
switch (reader.Name)
{
case "Location":
{
temp.Location = this.ReadString(reader);
break;
}
case "State":
{
temp.State = this.ReadMotorState(reader);
break;
}
case "Direction":
{
temp.Direction = this.ReadMotorDirection(reader);
break;
}
case "PositivePusher":
{
temp.PositivePusher = this.ReadBool(reader);
break;
}
case "PositionInversion":
{
temp.PositionInversion = this.ReadBool(reader);
break;
}
}
}
else
{
if ("FeederMotor" == reader.Name)
{
result = temp;
break;
}
}
}
return (result);
}
private void SetFeederMotorDefaults(ref FeederMotorParameters feederMotorParameters, string location, bool positivePusher, bool positionInversion)
{
feederMotorParameters.Location = location;
feederMotorParameters.State = MotorStates.Enabled;
feederMotorParameters.Direction = MotorDirections.Normal;
feederMotorParameters.PositivePusher = positivePusher;
feederMotorParameters.PositionInversion = positionInversion;
}
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 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++;
}
}
public void Set(FeederMotorParameters parameters)
{
this.Location = parameters.Location;
this.State = parameters.State;
this.Direction = parameters.Direction;
}
