public MainForm() { this.InitializeComponent(); this.StatusLabel.Text = ""; this.device = new Device("device", 0); this.downloadDevice = new Device("download device", 0); this.digitalIo = new PeakDigitalIo("digital IO", 0); this.digitalIo.OnInputChange = new PeakDigitalIo.InputChangeHandler(this.DigitalIoChangeHandler); this.analogIo = new PeakAnalogIo("analog IO", 0); this.encoder = new KublerRotaryEncoder("encoder", 0); this.motor = new ElmoWhistleMotor("motor", 0); this.motor.OnInputChange = new ElmoWhistleMotor.InputChangeHandler(this.MotorInputChangeHandler); this.camera = new UlcRoboticsCamera("camera", 0); this.gps = new UlcRoboticsGps("gps", 0); this.nicbotBody = new UlcRoboticsNicbotBody("nicbot body", 0); this.nicbotWheel = new UlcRoboticsNicbotWheel("nicbot wheel", 0); this.rs232 = new UlcRoboticsRs232("rs232", 0); this.rs232.OnSerialReceive = new UlcRoboticsRs232.SerialReceiveHandler(this.Rs232ReceiveHandler); this.rs232RxQueue = new Queue(); this.busInterface = new BusInterface(); this.busInterface.AddDevice(this.device); this.busInterface.AddDevice(this.downloadDevice); this.busInterface.AddDevice(this.digitalIo); this.busInterface.AddDevice(this.analogIo); this.busInterface.AddDevice(this.encoder); this.busInterface.AddDevice(this.motor); this.busInterface.AddDevice(this.camera); this.busInterface.AddDevice(this.gps); this.busInterface.AddDevice(this.nicbotBody); this.busInterface.AddDevice(this.nicbotWheel); this.busInterface.AddDevice(this.rs232); Array interfaces = Enum.GetValues(typeof(BusInterfaces)); this.BusInterfaceComboBox.Items.Clear(); for (IEnumerator i = interfaces.GetEnumerator(); i.MoveNext(); ) { this.BusInterfaceComboBox.Items.Add(i.Current); } this.BusInterfaceComboBox.SelectedIndex = 0; this.BaudComboBox.Items.Clear(); this.BaudComboBox.Items.Add("10000"); this.BaudComboBox.Items.Add("20000"); this.BaudComboBox.Items.Add("50000"); this.BaudComboBox.Items.Add("100000"); this.BaudComboBox.Items.Add("125000"); this.BaudComboBox.Items.Add("250000"); this.BaudComboBox.Items.Add("500000"); this.BaudComboBox.Items.Add("1000000"); this.BaudComboBox.SelectedIndex = 5; this.DigitalIoBaudComboBox.Items.Clear(); this.DigitalIoBaudComboBox.Items.Add("1000000"); this.DigitalIoBaudComboBox.Items.Add("800000"); this.DigitalIoBaudComboBox.Items.Add("500000"); this.DigitalIoBaudComboBox.Items.Add("250000"); this.DigitalIoBaudComboBox.Items.Add("125000"); this.DigitalIoBaudComboBox.Items.Add("50000"); this.DigitalIoBaudComboBox.SelectedIndex = 5; this.AnalogIoBaudComboBox.Items.Clear(); this.AnalogIoBaudComboBox.Items.Add("1000000"); this.AnalogIoBaudComboBox.Items.Add("800000"); this.AnalogIoBaudComboBox.Items.Add("500000"); this.AnalogIoBaudComboBox.Items.Add("250000"); this.AnalogIoBaudComboBox.Items.Add("125000"); this.AnalogIoBaudComboBox.Items.Add("50000"); this.AnalogIoBaudComboBox.SelectedIndex = 5; this.EncoderBaudComboBox.Items.Clear(); this.EncoderBaudComboBox.Items.Add("10000"); this.EncoderBaudComboBox.Items.Add("20000"); this.EncoderBaudComboBox.Items.Add("50000"); this.EncoderBaudComboBox.Items.Add("100000"); this.EncoderBaudComboBox.Items.Add("125000"); this.EncoderBaudComboBox.Items.Add("250000"); this.EncoderBaudComboBox.Items.Add("500000"); this.EncoderBaudComboBox.Items.Add("1000000"); this.EncoderBaudComboBox.SelectedIndex = 6; this.MotorModeComboBox.SelectedIndex = 0; this.CameraBaudComboBox.Items.Clear(); this.CameraBaudComboBox.Items.Add("10000"); this.CameraBaudComboBox.Items.Add("20000"); this.CameraBaudComboBox.Items.Add("50000"); this.CameraBaudComboBox.Items.Add("125000"); this.CameraBaudComboBox.Items.Add("250000"); this.CameraBaudComboBox.Items.Add("500000"); this.CameraBaudComboBox.Items.Add("1000000"); this.CameraBaudComboBox.SelectedIndex = 5; this.GpsBaudComboBox.Items.Clear(); this.GpsBaudComboBox.Items.Add("10000"); this.GpsBaudComboBox.Items.Add("20000"); this.GpsBaudComboBox.Items.Add("50000"); this.GpsBaudComboBox.Items.Add("125000"); this.GpsBaudComboBox.Items.Add("250000"); this.GpsBaudComboBox.Items.Add("500000"); this.GpsBaudComboBox.Items.Add("1000000"); this.GpsBaudComboBox.SelectedIndex = 5; this.NicbotBodyBaudComboBox.Items.Clear(); this.NicbotBodyBaudComboBox.Items.Add("10000"); this.NicbotBodyBaudComboBox.Items.Add("20000"); this.NicbotBodyBaudComboBox.Items.Add("50000"); this.NicbotBodyBaudComboBox.Items.Add("100000"); this.NicbotBodyBaudComboBox.Items.Add("125000"); this.NicbotBodyBaudComboBox.Items.Add("250000"); this.NicbotBodyBaudComboBox.Items.Add("500000"); this.NicbotBodyBaudComboBox.Items.Add("1000000"); this.NicbotBodyBaudComboBox.SelectedIndex = 5; this.NicbotWheelModeComboBox.SelectedIndex = 0; this.NicbotWheelBaudComboBox.Items.Clear(); this.NicbotWheelBaudComboBox.Items.Add("10000"); this.NicbotWheelBaudComboBox.Items.Add("20000"); this.NicbotWheelBaudComboBox.Items.Add("50000"); this.NicbotWheelBaudComboBox.Items.Add("100000"); this.NicbotWheelBaudComboBox.Items.Add("125000"); this.NicbotWheelBaudComboBox.Items.Add("250000"); this.NicbotWheelBaudComboBox.Items.Add("500000"); this.NicbotWheelBaudComboBox.Items.Add("1000000"); this.NicbotWheelBaudComboBox.SelectedIndex = 5; this.Rs232BaudComboBox.Items.Clear(); this.Rs232BaudComboBox.Items.Add("10000"); this.Rs232BaudComboBox.Items.Add("20000"); this.Rs232BaudComboBox.Items.Add("50000"); this.Rs232BaudComboBox.Items.Add("125000"); this.Rs232BaudComboBox.Items.Add("250000"); this.Rs232BaudComboBox.Items.Add("500000"); this.Rs232BaudComboBox.Items.Add("1000000"); this.Rs232BaudComboBox.SelectedIndex = 5; this.NicbotAutoDrillOriginCheckBox.Checked = false; this.NicbotAutoDrillContinuousModeRadioButton.Checked = true; this.NicbotAutoDrillDistanceRetractionRadioButton.Checked = true; this.traceQueue = new Queue(); QueuedTraceListener queuedTraceListener = new QueuedTraceListener(this.traceQueue); Trace.Listeners.Add(queuedTraceListener); Tracer.MaskString = "FFFFFFFF"; this.ActivityButton.Text = "Start"; this.active = false; this.DownloadProgressLabel.Text = ""; this.DownloadActivityButton.Text = "Start"; this.downloadActive = false; this.HeartbeatActivityButton.Text = "Start"; this.heartbeatActive = false; this.GpsDegreesRadioButton.Checked = true; }
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 StartRobotWheel(UlcRoboticsNicbotWheel motor) { motor.SetConsumerHeartbeat((UInt16)ParameterAccessor.Instance.RobotBus.ConsumerHeartbeatRate, (byte)ParameterAccessor.Instance.RobotBus.ControllerBusId); motor.SetProducerHeartbeat((UInt16)ParameterAccessor.Instance.RobotBus.ProducerHeartbeatRate); motor.Configure(); motor.Start(); }
private void EvaluateMovementMotorValue(MovementForwardControls cumulativeForwardControl, UlcRoboticsNicbotWheel motor, MovementMotorParameters parameters, ref double total, ref int count) { if ((null == motor.FaultReason) && (MotorStates.Enabled == parameters.State) && (MovementModes.move == this.movementMode)) { if (MovementForwardControls.velocity == cumulativeForwardControl) { MovementForwardControls motorForwordControl = ParameterAccessor.Instance.GetMovementForwardControl(parameters, this.movementForwardMode); if (motorForwordControl == cumulativeForwardControl) { double motorVelocity = motor.RPM; motorVelocity /= ParameterAccessor.Instance.MovementMotorVelocityToRpm; motorVelocity *= (MotorDirections.Normal == parameters.Direction) ? 1 : -1; total += motorVelocity; count++; } else { double velocityEqualivant = motor.Torque * 1000 / ParameterAccessor.Instance.MovementMotorCurrentPer1kRPM.OperationalValue / ParameterAccessor.Instance.MovementMotorVelocityToRpm; total += velocityEqualivant; count++; } } 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 total += motor.Torque; count++; } } }
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]; }