private bool UpdateRobotWheel(UlcRoboticsNicbotWheel motor, MovementMotorStatus status, MovementMotorParameters parameters)
{
bool scheduled = false;
if (null == motor.FaultReason)
{
bool modeChange = false;
UlcRoboticsNicbotWheel.Modes neededMode = UlcRoboticsNicbotWheel.Modes.undefined;
double neededValue = 0;
if (MotorStates.Disabled == parameters.State)
{
if (0 == status.requestedVelocity)
{
neededMode = UlcRoboticsNicbotWheel.Modes.off;
neededValue = 0;
}
else
{
neededMode = UlcRoboticsNicbotWheel.Modes.velocity;
neededValue = 0;
}
}
else if (MotorStates.Enabled == parameters.State)
{
if (0 != this.movementManualVelocity)
{
neededMode = UlcRoboticsNicbotWheel.Modes.velocity;
neededValue = this.movementManualVelocity;
}
else if (MovementModes.off == this.movementMode)
{
if (0 == status.requestedVelocity)
{
neededMode = UlcRoboticsNicbotWheel.Modes.off;
neededValue = 0;
}
else
{
neededMode = UlcRoboticsNicbotWheel.Modes.velocity;
neededValue = 0;
}
}
else if (MovementModes.move == this.movementMode)
{
MovementForwardControls cumulativeForwardControl = this.GetMovementForwardControl(); // applicable to all motors: velocity or current
double setPoint;
if (false == this.movementTriggered)
{
setPoint = 0;
neededMode = UlcRoboticsNicbotWheel.Modes.velocity;
}
else if (MovementForwardControls.velocity == cumulativeForwardControl)
{
MovementForwardControls motorForwordControl = ParameterAccessor.Instance.GetMovementForwardControl(parameters, this.movementForwardMode);
if (motorForwordControl == cumulativeForwardControl)
{
setPoint = this.movementRequest * ParameterAccessor.Instance.MovementMotorMaxSpeed.OperationalValue;
neededMode = UlcRoboticsNicbotWheel.Modes.velocity;
}
else
{
double velocitySetPoint = this.movementRequest * ParameterAccessor.Instance.MovementMotorMaxSpeed.OperationalValue;
setPoint = velocitySetPoint * ParameterAccessor.Instance.MovementMotorVelocityToRpm * ParameterAccessor.Instance.MovementMotorCurrentPer1kRPM.OperationalValue / 1000;
neededMode = UlcRoboticsNicbotWheel.Modes.current;
}
}
else
{
// NOTE: if any motor is set for velocity then cumulative mode is velocity
// NOTE: current cumulative mode implies all motors are set for current
setPoint = this.movementRequest * ParameterAccessor.Instance.MovementMotorMaxCurrent.OperationalValue;
neededMode = UlcRoboticsNicbotWheel.Modes.current;
}
#if false
bool offSetpointRequested = false;
if ((UlcRoboticsNicbotWheel.Modes.off == status.requestedMode) ||
(UlcRoboticsNicbotWheel.Modes.undefined == status.requestedMode) ||
((UlcRoboticsNicbotWheel.Modes.velocity == status.requestedMode) && (0 == status.requestedVelocity)) ||
((UlcRoboticsNicbotWheel.Modes.current == status.requestedMode) && (0 == status.requestedCurrent)))
{
offSetpointRequested = true;
}
if ((0 == setPoint) && (false != offSetpointRequested))
{
neededMode = UlcRoboticsNicbotWheel.Modes.off;
neededValue = 0;
}
else
{
neededValue = setPoint;
}
#endif
neededValue = setPoint;
}
else if (MovementModes.locked == this.movementMode)
{
neededMode = UlcRoboticsNicbotWheel.Modes.current;
neededValue = ParameterAccessor.Instance.MovementMotorLockCurrent.OperationalValue;
}
}
else if (MotorStates.Locked == parameters.State)
{
neededMode = UlcRoboticsNicbotWheel.Modes.current;
neededValue = ParameterAccessor.Instance.MovementMotorLockCurrent.OperationalValue;
}
if (neededMode != status.requestedMode)
{
motor.SetMode(neededMode);
status.requestedMode = neededMode;
modeChange = true;
Tracer.WriteMedium(TraceGroup.RBUS, null, "{0} mode={1}", motor.Name, neededMode.ToString());
}
if (UlcRoboticsNicbotWheel.Modes.velocity == status.requestedMode)
{
if ((neededValue != status.requestedVelocity) || (false != modeChange))
{
int settingInversionValue = (MotorDirections.Normal == parameters.Direction) ? 1 : -1;
int velocityRpm = (int)(settingInversionValue * neededValue * ParameterAccessor.Instance.MovementMotorVelocityToRpm);
motor.ScheduleVelocity(velocityRpm);
scheduled = true;
status.requestedVelocity = neededValue;
Tracer.WriteMedium(TraceGroup.RBUS, null, "{0} velocity={1:0.00} rpm={2}", motor.Name, neededValue, velocityRpm);
}
}
else if (UlcRoboticsNicbotWheel.Modes.current == status.requestedMode)
{
if ((neededValue != status.requestedCurrent) || (false != modeChange))
{
int settingInversionValue = (MotorDirections.Normal == parameters.Direction) ? 1 : -1;
float torqueCurrent = (float)(settingInversionValue * neededValue);
motor.ScheduleTorque(torqueCurrent);
scheduled = true;
status.requestedCurrent = neededValue;
Tracer.WriteMedium(TraceGroup.RBUS, null, "{0} current {1}", motor.Name, torqueCurrent);
}
}
else if (UlcRoboticsNicbotWheel.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);
}
}
}
return (scheduled);
}
private void Initialize()
{
this.busReceiveQueue = new Queue();
this.deviceResetQueue = new Queue();
this.robotBody = new UlcRoboticsNicbotBody("nicbot body", (byte)ParameterAccessor.Instance.RobotBus.RobotBodyBusId);
this.robotTopFrontWheel = new UlcRoboticsNicbotWheel("nicbot tf-wheel", (byte)ParameterAccessor.Instance.RobotBus.RobotTopFrontWheelBusId);
this.robotTopRearWheel = new UlcRoboticsNicbotWheel("nicbot tr-wheel", (byte)ParameterAccessor.Instance.RobotBus.RobotTopRearWheelBusId);
this.robotBottomFrontWheel = new UlcRoboticsNicbotWheel("nicbot bf-wheel", (byte)ParameterAccessor.Instance.RobotBus.RobotBottomFrontWheelBusId);
this.robotBottomRearWheel = new UlcRoboticsNicbotWheel("nicbot br-wheel", (byte)ParameterAccessor.Instance.RobotBus.RobotBottomRearWheelBusId);
this.deviceList = new ArrayList();
this.deviceList.Add(this.robotBody);
this.deviceList.Add(this.robotTopFrontWheel);
this.deviceList.Add(this.robotTopRearWheel);
this.deviceList.Add(this.robotBottomFrontWheel);
this.deviceList.Add(this.robotBottomRearWheel);
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.movementTopFrontWheelStatus = new MovementMotorStatus();
this.movementTopRearWheelStatus = new MovementMotorStatus();
this.movementBottomFrontWheelStatus = new MovementMotorStatus();
this.movementBottomRearWheelStatus = new MovementMotorStatus();
this.cameraLightLevelSetPoints = new int[12];
this.cameraLightLevelRequests = new int[12];
}
