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