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();
}
private void UpdateGuideMotor(ElmoWhistleMotor motor, GuideMotorStatus status)
{
if (null == motor.FaultReason)
{
double neededValue = 0;
if (GuideDirections.retract == status.direction)
{
if (status.RetractionLimit == false)
{
neededValue = ParameterAccessor.Instance.GuideRetractionSpeed.OperationalValue;
}
else
{
status.direction = GuideDirections.off;
neededValue = 0;
}
}
else if (GuideDirections.extend == status.direction)
{
if (status.ExtensionLimit == false)
{
neededValue = ParameterAccessor.Instance.GuideExtensionSpeed.OperationalValue * -1;
}
else
{
status.direction = GuideDirections.off;
neededValue = 0;
}
}
else
{
neededValue = 0;
}
if (status.requestedVelocity != neededValue)
{
int velocityRpm = (int)(neededValue);
motor.SetVelocity(velocityRpm);
status.requestedVelocity = neededValue;
Tracer.WriteMedium(TraceGroup.TBUS, null, "{0} velocity {1}", motor.Name, neededValue);
}
}
}
