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]; }