/// <summary>
/// Handles the Click event of the buttonMove control.
/// </summary>
/// <param name="sender">The source of the event.</param>
/// <param name="ea">The <see cref="System.EventArgs"/> instance containing the event data.</param>
private void ButtonMoveClick(object sender, EventArgs ea)
{
try
{
ProfilePositionMode ppm = _epos.Operation.ProfilePositionMode;
ppm.ActivateProfilePositionMode();
textBoxAOM.Text = _epos.Operation.OperationMode.GetOperationModeAsString();
textBoxPositionStart.Text = string.Format("{0}", _epos.Operation.MotionInfo.GetPositionIs());
ppm.MoveToPosition(Convert.ToInt32(textBoxTargetPosition.Text), radioButtonAbsoluteMove.Checked, true);
}
catch (DeviceException e)
{
StopRefresh();
ShowMessageBox(e.ErrorMessage, e.ErrorCode);
}
catch (Exception e)
{
StopRefresh();
MessageBox.Show(e.Message);
}
}
/// <summary>
/// Inicializace motoru
/// </summary>
/// <param name="connector">connector sběrnice</param>
/// <param name="stateObserver">posluchač stavu motoru</param>
/// <param name="nodeNumber">číslo node</param>
/// <param name="id">id motoru</param>
/// <param name="mode">defaultní nastavení módu</param>
/// <param name="reverse">příznak obrácení směru točení</param>
/// <param name="multiplier">násobitel otáček v případě, že je motor za převodovkou</param>
/// <param name="positionVeocity">rychlost motoru v otáčkách při pozicování</param>
/// <param name="positionAceleration">zrychlení motoru v otáčkách při pozicování</param>
/// <param name="positionDeceleration">zpomalení motoru v otáčkách při pozicování</param>
/// <param name="velocity">maximální rychlost motoru při rychlostním řízení</param>
/// <param name="aceleration">zrychlení motoru při rychlostním řízení</param>
/// <param name="deceleration">zpomalení motoru při rychlostním řízení</param>
public void inicialize(DeviceManager connector, IStateObserver stateObserver, Action motorErrorOccuredObserver, int nodeNumber, MotorId id, MotorMode mode, bool reverse, int multiplier, uint positionVelocity, uint positionAceleration, uint positionDeceleration, uint velocity, uint aceleration, uint deceleration)
{
try
{
this.mode = mode;
this.stateObserver = stateObserver;
this.motorErrorOccuredObserver = motorErrorOccuredObserver;
this.id = id;
this.multiplier = multiplier;
if (reverse)
{
rev = -1;
}
motor = connector.CreateDevice(Convert.ToUInt16(nodeNumber));
stateHandler = motor.Operation.MotionInfo;
sm = motor.Operation.StateMachine;
if (sm.GetFaultState())
{
sm.ClearFault();
}
sm.SetEnableState();
maxSpeed = (int)velocity;
velocityHandler = motor.Operation.ProfileVelocityMode;
velocityHandler.SetVelocityProfile(aceleration, deceleration);
positionHandler = motor.Operation.ProfilePositionMode;
positionHandler.SetPositionProfile(positionVelocity, positionAceleration, positionDeceleration);
homingHandler = motor.Operation.HomingMode;
changeMode(mode);
setStateObserver();
state = MotorState.enabled;
stateObserver.motorStateChanged(MotorState.enabled, "", id, 0, 0, 0, 0);
targetPosition = stateHandler.GetPositionIs();
targetAngle = getAngleFromPosition(targetPosition);
}
catch (DeviceException e)
{
sm = null;
disableStateObserver();
state = MotorState.error;
stateObserver.motorStateChanged(MotorState.error, String.Format("{0}\nError: {1}", e.ErrorMessage, errorDictionary.getComunicationErrorMessage(e.ErrorCode)), id, 0, 0, 0, 0);
}
}
/// <summary>
/// Handles the Click event of the buttonHalt control.
/// </summary>
/// <param name="sender">The source of the event.</param>
/// <param name="ea">The <see cref="System.EventArgs"/> instance containing the event data.</param>
private void ButtonHaltClick(object sender, EventArgs ea)
{
try
{
ProfilePositionMode ppm = _epos.Operation.ProfilePositionMode;
ppm.ActivateProfilePositionMode();
textBoxAOM.Text = _epos.Operation.OperationMode.GetOperationModeAsString();
ppm.HaltPositionMovement();
}
catch (DeviceException e)
{
StopRefresh();
ShowMessageBox(e.ErrorMessage, e.ErrorCode);
}
catch (Exception e)
{
StopRefresh();
MessageBox.Show(e.Message);
}
}
/// <summary>
/// Inicializace motoru
/// </summary>
/// <param name="connector">connector sběrnice</param>
/// <param name="stateObserver">posluchač stavu motoru</param>
/// <param name="nodeNumber">číslo node</param>
/// <param name="id">id motoru</param>
/// <param name="mode">defaultní nastavení módu</param>
/// <param name="reverse">příznak obrácení směru točení</param>
/// <param name="multiplier">násobitel otáček v případě, že je motor za převodovkou</param>
/// <param name="positionVeocity">rychlost motoru v otáčkách při pozicování</param>
/// <param name="positionAceleration">zrychlení motoru v otáčkách při pozicování</param>
/// <param name="positionDeceleration">zpomalení motoru v otáčkách při pozicování</param>
/// <param name="velocity">maximální rychlost motoru při rychlostním řízení</param>
/// <param name="aceleration">zrychlení motoru při rychlostním řízení</param>
/// <param name="deceleration">zpomalení motoru při rychlostním řízení</param>
public void inicialize(DeviceManager connector, IStateObserver stateObserver, Action motorErrorOccuredObserver, int nodeNumber, MotorId id, MotorMode mode, bool reverse, int multiplier, uint positionVelocity, uint positionAceleration, uint positionDeceleration, uint velocity, uint aceleration, uint deceleration)
{
try
{
this.mode = mode;
this.stateObserver = stateObserver;
this.motorErrorOccuredObserver = motorErrorOccuredObserver;
this.id = id;
this.multiplier = multiplier;
if (reverse)
{
rev = -1;
}
motor = connector.CreateDevice(Convert.ToUInt16(nodeNumber));
stateHandler = motor.Operation.MotionInfo;
sm = motor.Operation.StateMachine;
if (sm.GetFaultState())
sm.ClearFault();
sm.SetEnableState();
maxSpeed = (int)velocity;
velocityHandler = motor.Operation.ProfileVelocityMode;
velocityHandler.SetVelocityProfile(aceleration, deceleration);
positionHandler = motor.Operation.ProfilePositionMode;
positionHandler.SetPositionProfile(positionVelocity, positionAceleration, positionDeceleration);
homingHandler = motor.Operation.HomingMode;
changeMode(mode);
setStateObserver();
state = MotorState.enabled;
stateObserver.motorStateChanged(MotorState.enabled, "", id, 0, 0, 0, 0);
targetPosition = stateHandler.GetPositionIs();
targetAngle = getAngleFromPosition(targetPosition);
}
catch (DeviceException e)
{
sm = null;
disableStateObserver();
state = MotorState.error;
stateObserver.motorStateChanged(MotorState.error, String.Format("{0}\nError: {1}", e.ErrorMessage, errorDictionary.getComunicationErrorMessage(e.ErrorCode)), id, 0, 0, 0, 0);
}
}
/// <summary>
/// constructor for Engine
/// </summary>
/// <param name="engineType">
/// select robot to drive;
/// select arm to move arm
/// </param>
/// <exception cref="ArgumentOutOfRangeException">thrown, if the engine type could not found</exception>
/// <exception cref="Exception">thrown, when a process in constructor fails</exception>
public Engine(EngineType engineType)
{
try
{
switch (engineType)
{
case EngineType.Robot:
//connect to motor to drive
_connector = new DeviceManager("EPOS2", "MAXON SERIAL V2", "USB", "USB0");
//motor to drive needs acceleration and deceleration
ProfileAcceleration = 3000;
ProfileDeceleration = 3000;
break;
case EngineType.Arm:
//connect to motor to move arm
_connector = new DeviceManager("EPOS", "MAXON_RS232", "RS232", "COM4");
//motor to move arm does not need acceleration and deceleration
ProfileAcceleration = 0;
ProfileDeceleration = 0;
break;
default:
throw new ArgumentOutOfRangeException(nameof(engineType), engineType, null);
}
//get baud rate info
var b = _connector.Baudrate;
//set connection properties
_connector.Baudrate = b;
_connector.Timeout = 500;
_epos1 = _connector.CreateDevice(Convert.ToUInt16(1));
_epos2 = _connector.CreateDevice(Convert.ToUInt16(2));
//ProfilePositionMode and ProfileVelocityMode are assigned to objects from the library
_ppm1 = _epos1.Operation.ProfilePositionMode;
_ppm2 = _epos2.Operation.ProfilePositionMode;
_pvm1 = _epos1.Operation.ProfileVelocityMode;
_pvm2 = _epos2.Operation.ProfileVelocityMode;
//StateMachines are assigned to objects from the library
_sm1 = _epos1.Operation.StateMachine;
_sm2 = _epos2.Operation.StateMachine;
//MotionInfo is assigned to an object from the library
_mi1 = _epos1.Operation.MotionInfo;
_mi2 = _epos2.Operation.MotionInfo;
//motors may be disabled
Enable();
//motors should initialize in VelocityMode
ActivateVelocityMode();
}
//an error may occur
catch (Exception)
{
ConsoleFormatter.Error("Failed to connect to the engine.",
"Type: " + engineType,
"Help: Check if the cable is plugged in properly.");
throw;
}
}
private void joy_update(object sender, EventArgs e)
{
if (ctrl != null && ctrl.IsConnected == true)
{
state = ctrl.GetState();
gamepad = ctrl.GetState().Gamepad;
var joy = new xbox_joy();
var buttons = "";
#region get joy data
gamepad = ctrl.GetState().Gamepad;
buttons = gamepad.Buttons.ToString();
joy.A = buttons.Contains("A");
joy.B = buttons.Contains("B");
joy.X = buttons.Contains("X");
joy.Y = buttons.Contains("Y");
joy.start = buttons.Contains("Start");
joy.back = buttons.Contains("Back");
joy.left_shoulder = buttons.Contains("LeftShoulder");
joy.right_shoulder = buttons.Contains("RightShoulder");
joy.rightthumb = buttons.Contains("RightThumb");
joy.leftthumb = buttons.Contains("LeftThumb");
joy.dpadUp = buttons.Contains("DPadUp");
joy.dpadLeft = buttons.Contains("DPadLeft");
joy.dpadDown = buttons.Contains("DPadDown");
joy.dpadRight = buttons.Contains("DPadRight");
joy.left_axis_x = gamepad.LeftThumbX + 32768;
joy.left_axis_y = gamepad.LeftThumbY + 32768;
joy.right_axis_x = gamepad.RightThumbX + 32768;
joy.right_axis_y = gamepad.RightThumbY + 32768;
joy.left_trigger = gamepad.LeftTrigger;
joy.right_trigger = gamepad.RightTrigger;
#endregion
this.label_left_trigger.Text = joy.left_trigger.ToString();
this.label_right_trigger.Text = joy.right_trigger.ToString();
this.label_left_axis_x.Text = joy.left_axis_x.ToString();
this.label_left_axis_y.Text = joy.left_axis_y.ToString();
this.label_right_axis_x.Text = joy.right_axis_x.ToString();
this.label_right_axis_y.Text = joy.right_axis_y.ToString();
if (joy.start)
{
this.checkBox_use_joy.Checked = true;
}
if (joy.back)
{
this.checkBox_use_joy.Checked = false;
}
this.checkBox_use_joy.Visible = true;
// MOVE !!!!
if (checkBox_use_joy.Checked == true)
{
try
{
ProfilePositionMode ppm = _epos_A.Operation.ProfilePositionMode;
ppm.ActivateProfilePositionMode();
textBoxAOM_A.Text = _epos_A.Operation.OperationMode.GetOperationModeAsString();
ppm.MoveToPosition(joy.left_axis_x, true, true);
}
catch { }
try
{
ProfilePositionMode ppm = _epos_B.Operation.ProfilePositionMode;
ppm.ActivateProfilePositionMode();
textBoxAOM_B.Text = _epos_B.Operation.OperationMode.GetOperationModeAsString();
ppm.MoveToPosition(joy.right_axis_x, true, true);
}
catch { }
}
}
else
{
this.checkBox_use_joy.Visible = false;
}
}
