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