private void UpdateRobotBody()
{
bool evaluateSolenoidSetPoint = false;
UInt16 solenoidSetPoint = 0;
if (this.videoASetPoint != this.videoARequested)
{
this.robotBody.SetVideoASelect((byte)this.videoASetPoint);
this.videoARequested = this.videoASetPoint;
}
if (this.videoBSetPoint != this.videoBRequested)
{
this.robotBody.SetVideoBSelect((byte)this.videoBSetPoint);
this.videoBRequested = this.videoBSetPoint;
}
for (int i = 0; i < this.cameraLightLevelSetPoints.Length; i++)
{
if (this.cameraLightLevelSetPoints[i] != this.cameraLightLevelRequests[i])
{
int level = (255 * this.cameraLightLevelSetPoints[i]) / 100;
this.robotBody.SetCameraLightLevelt((byte)(i + 1), (byte)level);
this.cameraLightLevelRequests[i] = this.cameraLightLevelSetPoints[i];
}
}
this.movementWheelModeActual = this.movementWheelModeChangeRequest;
lock (this)
{
if (0 == this.robotSolenoidSetPointChangeCounter)
{
evaluateSolenoidSetPoint = true;
solenoidSetPoint = this.robotSolenoidSetPoint;
}
}
if ((false != evaluateSolenoidSetPoint) &&
(solenoidSetPoint != this.robotSolenoidRequested))
{
this.robotBody.SetSolenoids(solenoidSetPoint);
this.robotSolenoidRequested = solenoidSetPoint;
}
if (RobotApplications.repair == ParameterAccessor.Instance.RobotApplication)
{
if (this.drillFrontRotationSetPoint != this.drillFrontRotationRequested)
{
this.robotBody.SetFrontDrillSpeed(this.drillFrontRotationSetPoint);
this.drillFrontRotationRequested = this.drillFrontRotationSetPoint;
}
if (this.drillRearRotationSetPoint != this.drillRearRotationRequested)
{
this.robotBody.SetRearDrillSpeed(this.drillRearRotationSetPoint);
this.drillRearRotationRequested = this.drillRearRotationSetPoint;
}
if (false != this.drillFrontConfigurationNeeded)
{
this.drillFrontIndexSpeedRequested = 0;
this.robotBody.SetDrillServoAcceleration((UInt32)ParameterAccessor.Instance.FrontDrill.Acceleration.OperationalValue);
this.robotBody.SetDrillServoErrorLimit((UInt16)ParameterAccessor.Instance.FrontDrill.ErrorLimit.OperationalValue);
this.robotBody.SetDrillServoProportionalControlConstant((UInt32)ParameterAccessor.Instance.FrontDrill.ProportionalControlConstant.OperationalValue);
this.robotBody.SetDrillServoIntegralControlConstant((UInt32)ParameterAccessor.Instance.FrontDrill.IntegralControlConstant.OperationalValue);
this.robotBody.SetDrillServoDerivativeControlConstant((UInt32)ParameterAccessor.Instance.FrontDrill.DerivativeControlConstant.OperationalValue);
this.drillFrontConfigurationNeeded = false;
}
if (false != this.drillRearConfigurationNeeded)
{
this.drillRearIndexSpeedRequested = 0;
this.robotBody.SetDrillServoAcceleration((UInt32)ParameterAccessor.Instance.RearDrill.Acceleration.OperationalValue);
this.robotBody.SetDrillServoErrorLimit((UInt16)ParameterAccessor.Instance.RearDrill.ErrorLimit.OperationalValue);
this.robotBody.SetDrillServoProportionalControlConstant((UInt32)ParameterAccessor.Instance.RearDrill.ProportionalControlConstant.OperationalValue);
this.robotBody.SetDrillServoIntegralControlConstant((UInt32)ParameterAccessor.Instance.RearDrill.IntegralControlConstant.OperationalValue);
this.robotBody.SetDrillServoDerivativeControlConstant((UInt32)ParameterAccessor.Instance.RearDrill.DerivativeControlConstant.OperationalValue);
this.drillRearConfigurationNeeded = false;
}
if (false != this.drillFrontRetractToLimit)
{
this.robotBody.SetFrontDrillRetractToLimit();
this.drillFrontRetractToLimit = false;
}
if (false != this.drillRearRetractToLimit)
{
this.robotBody.SetRearDrillRetractToLimit();
this.drillRearRetractToLimit = false;
}
if (false != this.drillFrontStop)
{
this.robotBody.StopFrontDrill();
this.drillFrontStop = false;
}
if (false != this.drillRearStop)
{
this.robotBody.StopRearDrill();
this.drillRearStop = false;
}
if (this.drillFrontIndexSetPoint != this.drillFrontIndexRequested)
{
double indexerSpeed = 0;
if (double.IsNaN(this.drillFrontOriginOffset) == false)
{
if (this.drillFrontIndexSetPoint > this.drillFrontOriginOffset)
{
indexerSpeed = ParameterAccessor.Instance.FrontDrill.CuttingSpeed.OperationalValue;
}
else
{
indexerSpeed = ParameterAccessor.Instance.FrontDrill.TravelSpeed.OperationalValue;
}
}
else
{
indexerSpeed = ParameterAccessor.Instance.FrontDrill.SearchSpeed.OperationalValue;
}
if (indexerSpeed != this.drillFrontIndexSpeedRequested)
{
UInt32 velocityCount = (UInt32)(indexerSpeed * ParameterAccessor.Instance.FrontDrill.SpeedToVelocityCount);
this.robotBody.SetDrillServoTravelVelocity(velocityCount);
this.drillFrontIndexSpeedRequested = indexerSpeed;
}
this.robotBody.SetFrontDrillIndex(this.drillFrontIndexSetPoint);
this.drillFrontIndexRequested = this.drillFrontIndexSetPoint;
}
if (this.drillRearIndexSetPoint != this.drillRearIndexRequested)
{
double indexerSpeed = 0;
if (double.IsNaN(this.drillRearOriginOffset) == false)
{
if (this.drillRearOriginOffset > this.drillRearIndexSetPoint)
{
indexerSpeed = ParameterAccessor.Instance.RearDrill.CuttingSpeed.OperationalValue;
}
else
{
indexerSpeed = ParameterAccessor.Instance.RearDrill.TravelSpeed.OperationalValue;
}
}
else
{
indexerSpeed = ParameterAccessor.Instance.RearDrill.SearchSpeed.OperationalValue;
}
if (indexerSpeed != this.drillRearIndexSpeedRequested)
{
UInt32 velocityCount = (UInt32)(indexerSpeed * ParameterAccessor.Instance.RearDrill.SpeedToVelocityCount);
this.robotBody.SetDrillServoTravelVelocity(velocityCount);
this.drillRearIndexSpeedRequested = indexerSpeed;
}
this.robotBody.SetRearDrillIndex(this.drillRearIndexSetPoint);
this.drillRearIndexRequested = this.drillRearIndexSetPoint;
}
if (this.drillFrontLaserSetPoint != this.drillFrontLaserRequested)
{
this.robotBody.SetFrontLaser(this.drillFrontLaserSetPoint);
this.drillFrontLaserRequested = this.drillFrontLaserSetPoint;
}
if (this.drillRearLaserSetPoint != this.drillRearLaserRequested)
{
this.robotBody.SetRearLaser(this.drillRearLaserSetPoint);
this.drillRearLaserRequested = this.drillRearLaserSetPoint;
}
}
if (RobotApplications.inspect == ParameterAccessor.Instance.RobotApplication)
{
}
}
private void StartRobotBody()
{
this.robotBody.SetConsumerHeartbeat((UInt16)ParameterAccessor.Instance.RobotBus.ConsumerHeartbeatRate, (byte)ParameterAccessor.Instance.RobotBus.ControllerBusId);
this.robotBody.SetProducerHeartbeat((UInt16)ParameterAccessor.Instance.RobotBus.ProducerHeartbeatRate);
UlcRoboticsNicbotBody.Modes robotMode = UlcRoboticsNicbotBody.Modes.repair;
if (RobotApplications.inspect == ParameterAccessor.Instance.RobotApplication)
{
robotMode = UlcRoboticsNicbotBody.Modes.inspect;
}
this.robotBody.Configure(robotMode);
this.robotBody.Start();
this.robotSolenoidSetPoint = this.robotBody.GetSolenoidCache();
this.robotSolenoidRequested = this.robotSolenoidSetPoint;
MovementWheelModes startWheelMode = MovementWheelModes.neither;
bool lastAxial = this.robotBody.LastAxial;
bool lastCircumferential = this.robotBody.LastCircumferential;
if (false != lastAxial)
{
startWheelMode = MovementWheelModes.axial;
}
else if (false != lastCircumferential)
{
startWheelMode = MovementWheelModes.circumferential;
}
this.movementWheelModeChangeRequest = startWheelMode;
this.movementWheelModeActual = startWheelMode;
Thread.Sleep(50);
if (RobotApplications.repair == ParameterAccessor.Instance.RobotApplication)
{
this.drillFrontConfigurationNeeded = true;
this.drillRearConfigurationNeeded = true;
}
}
public MovementWheelModes GetMovementWheelMode()
{
MovementWheelModes result = MovementWheelModes.neither;
if (MovementWheelModes.neither != this.movementWheelModeActual)
{
result = this.movementWheelModeActual;
}
else if (MovementWheelModes.neither != this.movementWheelModeChangeRequest)
{
result = this.movementWheelModeChangeRequest;
}
#if false
MovementWheelModes startWheelMode = MovementWheelModes.neither;
bool lastAxial = this.robotBody.LastAxial;
bool lastCircumferential = this.robotBody.LastCircumferential;
if (false != lastAxial)
{
startWheelMode = MovementWheelModes.axial;
}
else if (false != lastCircumferential)
{
startWheelMode = MovementWheelModes.circumferential;
}
this.movementWheelModeChangeRequest = startWheelMode;
this.movementWheelModeActual = startWheelMode;
this.movementWheelModeActual
GetMovementWheelMode
MovementWheelModes result = MovementWheelModes.neither;
bool lastAxial = this.robotBody.LastAxial;
bool lastCircumferential = this.robotBody.LastCircumferential;
if (false != lastAxial)
{
result = MovementWheelModes.axial;
}
else if (false != lastCircumferential)
{
result = MovementWheelModes.circumferential;
}
#endif
return (result);
#if false
bool wheelAxial = this.GetSolenoidActive(Solenoids.wheelAxial);
bool wheelCircumferential = this.GetSolenoidActive(Solenoids.wheelCircumferential);
MovementWheelModes result = MovementWheelModes.neither;
if ((false == wheelAxial) && (false == wheelCircumferential))
{
result = MovementWheelModes.neither;
}
else if ((false == wheelAxial) && (false != wheelCircumferential))
{
result = MovementWheelModes.circumferential;
}
else if ((false != wheelAxial) && (false == wheelCircumferential))
{
result = MovementWheelModes.axial;
}
else
{
result = MovementWheelModes.both;
}
return (result);
#endif
}
public void SetMovementForwardMode(MovementForwardModes mode)
{
if (mode == MovementForwardModes.circumferential)
{
this.movementWheelModeChangeRequest = MovementWheelModes.circumferential;
}
else
{
this.movementWheelModeChangeRequest = MovementWheelModes.axial;
}
this.movementForwardMode = mode;
}
private void InitializeValues()
{
this.busReady = false;
this.busStatus = null;
this.busReceiveQueue.Clear();
this.deviceResetQueue.Clear();
this.robotBody.NodeId = (byte)ParameterAccessor.Instance.RobotBus.RobotBodyBusId;
this.robotTopFrontWheel.NodeId = (byte)ParameterAccessor.Instance.RobotBus.RobotTopFrontWheelBusId;
this.robotTopRearWheel.NodeId = (byte)ParameterAccessor.Instance.RobotBus.RobotTopRearWheelBusId;
this.robotBottomFrontWheel.NodeId = (byte)ParameterAccessor.Instance.RobotBus.RobotBottomFrontWheelBusId;
this.robotBottomRearWheel.NodeId = (byte)ParameterAccessor.Instance.RobotBus.RobotBottomRearWheelBusId;
this.TraceMask = ParameterAccessor.Instance.RobotBus.ControllerTraceMask;
this.robotBody.TraceMask = ParameterAccessor.Instance.RobotBus.RobotBodyTraceMask;
this.robotTopFrontWheel.TraceMask = ParameterAccessor.Instance.RobotBus.RobotTopFrontWheelTraceMask;
this.robotTopRearWheel.TraceMask = ParameterAccessor.Instance.RobotBus.RobotTopRearWheelTraceMask;
this.robotBottomFrontWheel.TraceMask = ParameterAccessor.Instance.RobotBus.RobotBottomFrontWheelTraceMask;
this.robotBottomRearWheel.TraceMask = ParameterAccessor.Instance.RobotBus.RobotBottomRearWheelTraceMask;
this.InitializeRobotBody();
this.movementMode = MovementModes.off;
this.movementForwardMode = MovementForwardModes.normalAxial;
this.movementWheelModeChangeRequest = MovementWheelModes.neither;
this.movementWheelModeActual = MovementWheelModes.neither;
this.movementTopFrontWheelStatus.Initialize();
this.movementTopRearWheelStatus.Initialize();
this.movementBottomFrontWheelStatus.Initialize();
this.movementBottomRearWheelStatus.Initialize();
this.drillFrontRotationSetPoint = 0;
this.drillRearRotationSetPoint = 0;
this.drillFrontIndexSetPoint = 0;
this.drillRearIndexSetPoint = 0;
this.drillFrontLaserSetPoint = false;
this.drillRearLaserSetPoint = false;
this.cameraA = CameraLocations.robotFrontUpperBack;
this.cameraB = CameraLocations.robotLowerBack;
this.videoASetPoint = 0;
this.videoBSetPoint = 0;
for (int i = 0; i < this.cameraLightLevelSetPoints.Length; i++)
{
this.cameraLightLevelSetPoints[i] = 0;
}
}