public void RunForRevolutions(float speed, float revs)
{
AngleA = travelAngle;
float degrees = revs * 360f;
AngleB = AngleA + degrees;
mode = MotorMode.RevRun;
}
/// <summary>
/// Nastaví aktuální pozici motoru
/// </summary>
/// <param name="position">aktuální pozice motoru</param>
public void setHomingPosition(int position)
{
MotorMode previouseMode = mode;
changeMode(MotorMode.homing);
this.position = position;
changeMode(previouseMode);
}
/// <summary>
/// Nastaví motor do defaultní pozice
/// </summary>
public void setDefaultPosition()
{
MotorMode previouseMode = mode;
changeMode(MotorMode.position);
moveToPosition((int)Properties.Settings.Default[id.ToString() + "_default"]);
changeMode(previouseMode);
}
// Use this for initialization
void Start()
{
currentMode = MotorMode.isTrackingHand;
enc = gameObject.GetComponent <EncoderController>();
serialPort = gameObject.GetComponent <SerialConnector>();
serialPort.Connect(portNumber);
enc.resetCount(InitialStringLength + 1000);
records = new List <SpeedRecord>();
isPulling = false;
}
public void setMotorMode(MotorMode mode)
// モーターの現在状態を切り替える
// MotorMode.isTrackingHand = コマ投げ出し前（手の位置に応じて巻き取り・繰り出し）
// MotorMode.isShowingResistance = コマ投げ出し後
// MotorMode.isRewinding = プレイ終了後巻取り中
// MotorMode.isFree = フリー状態
{
if (serialPort == null)
{
return;
}
// if mode is unchanged, do nothing. This is to avoid sending the status code to Machine repeatedly.
if (currentMode == mode)
{
return;
}
currentMode = mode;
if (serialPort == null)
{
return;
}
//Debug.Log("Motor Switching to " + currentMode);
switch (currentMode)
{
case MotorMode.isTrackingHand:
serialPort.SendChar('B');
break;
case MotorMode.isShowingResistance:
serialPort.SendChar('B'); // 'C' でブレーキの予定だったか、重すぎるのでフリーに
timeZero = Time.fixedTime;
records.Clear();
calibrateToLength(0);
isPulling = true;
break;
case MotorMode.isRewinding:
serialPort.SendChar('E');
break;
case MotorMode.isFree:
serialPort.SendChar('D');
break;
}
}
/// <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>
/// Nastaví motor do defaultní pozice
/// </summary>
public void setDefaultPosition()
{
MotorMode previouseMode = mode;
changeMode(MotorMode.position);
try
{
moveToPosition((int)Properties.Settings.Default[id.ToString() + "_default"]);
}
catch (DeviceException e)
{
state = MotorState.error;
stateObserver.motorStateChanged(MotorState.error, String.Format("{0}\nError: {1}", e.ErrorMessage, errorDictionary.getComunicationErrorMessage(e.ErrorCode)), id, 0, 0, 0, 0);
motorErrorOccuredObserver();
}
changeMode(previouseMode);
}
internal MotorServiceImpl(MotorControllerPrx proxy)
{
this.proxy = proxy;
numMotors = 4; // todo: this should be retrieved from the qwerk
// create and initialize the all-on mask array
maskAllOn = new bool[numMotors];
for (int i = 0; i < maskAllOn.Length; i++)
{
maskAllOn[i] = true;
}
// create the array used for zero velocity/position and default acceleration
nullIntArray = new int[numMotors];
defaultAccelerationArray = new int[numMotors];
for (int i = 0; i < numMotors; i++)
{
defaultAccelerationArray[i] = DEFAULT_ACCELERATION;
}
// build the mask arrays for each motor and store them in a map indexed on motor id
motorIdToMaskArrayMap = new Dictionary<int, bool[]>(numMotors);
for (int i = 0; i < numMotors; i++)
{
bool[] mask = new bool[numMotors];
mask[i] = true;
motorIdToMaskArrayMap.Add(i, mask);
}
// build the motor mode arrays and store them in a map indexed on motor mode
motorModeToModeArrayMap = new Dictionary<MotorMode, MotorMode[]>(numMotors);
MotorMode[] motorModeOffArray = new MotorMode[numMotors];
MotorMode[] motorModePositionControlArray = new MotorMode[numMotors];
MotorMode[] motorModeSpeedControlArray = new MotorMode[numMotors];
for (int i = 0; i < numMotors; i++)
{
motorModeOffArray[i] = MotorMode.MotorOff;
motorModePositionControlArray[i] = MotorMode.MotorPositionControl;
motorModeSpeedControlArray[i] = MotorMode.MotorSpeedControl;
}
motorModeToModeArrayMap.Add(MotorMode.MotorOff, motorModeOffArray);
motorModeToModeArrayMap.Add(MotorMode.MotorPositionControl, motorModePositionControlArray);
motorModeToModeArrayMap.Add(MotorMode.MotorSpeedControl, motorModeSpeedControlArray);
}
internal MotorServiceImpl(MotorControllerPrx proxy)
{
this.proxy = proxy;
numMotors = 4; // todo: this should be retrieved from the qwerk
// create and initialize the all-on mask array
maskAllOn = new bool[numMotors];
for (int i = 0; i < maskAllOn.Length; i++)
{
maskAllOn[i] = true;
}
// create the array used for zero velocity/position and default acceleration
nullIntArray = new int[numMotors];
defaultAccelerationArray = new int[numMotors];
for (int i = 0; i < numMotors; i++)
{
defaultAccelerationArray[i] = DEFAULT_ACCELERATION;
}
// build the mask arrays for each motor and store them in a map indexed on motor id
motorIdToMaskArrayMap = new Dictionary <int, bool[]>(numMotors);
for (int i = 0; i < numMotors; i++)
{
bool[] mask = new bool[numMotors];
mask[i] = true;
motorIdToMaskArrayMap.Add(i, mask);
}
// build the motor mode arrays and store them in a map indexed on motor mode
motorModeToModeArrayMap = new Dictionary <MotorMode, MotorMode[]>(numMotors);
MotorMode[] motorModeOffArray = new MotorMode[numMotors];
MotorMode[] motorModePositionControlArray = new MotorMode[numMotors];
MotorMode[] motorModeSpeedControlArray = new MotorMode[numMotors];
for (int i = 0; i < numMotors; i++)
{
motorModeOffArray[i] = MotorMode.MotorOff;
motorModePositionControlArray[i] = MotorMode.MotorPositionControl;
motorModeSpeedControlArray[i] = MotorMode.MotorSpeedControl;
}
motorModeToModeArrayMap.Add(MotorMode.MotorOff, motorModeOffArray);
motorModeToModeArrayMap.Add(MotorMode.MotorPositionControl, motorModePositionControlArray);
motorModeToModeArrayMap.Add(MotorMode.MotorSpeedControl, motorModeSpeedControlArray);
}
/// <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)
{
this.mode = mode;
this.stateObserver = stateObserver;
this.motorErrorOccuredObserver = motorErrorOccuredObserver;
this.id = id;
this.multiplier = multiplier;
if (reverse)
{
rev = -1;
}
changeMode(mode);
setStateObserver();
state = MotorState.enabled;
stateObserver.motorStateChanged(MotorState.enabled, "", id, 0, 0, 0, 0);
}
/// <summary>
/// Nastaví aktuální pozici motoru
/// </summary>
/// <param name="position">aktuální pozice motoru</param>
public void setHomingPosition(int position)
{
if (homingHandler != null)
{
MotorMode previouseMode = mode;
changeMode(MotorMode.homing);
try
{
homingHandler.DefinePosition(position);
}
catch (DeviceException e)
{
state = MotorState.error;
stateObserver.motorStateChanged(MotorState.error, String.Format("{0}\nError: {1}", e.ErrorMessage, errorDictionary.getComunicationErrorMessage(e.ErrorCode)), id, 0, 0, 0, 0);
motorErrorOccuredObserver();
}
changeMode(previouseMode);
}
}
public void Output(Motor nMotor, int nPower, MotorMode nMode, MotorRegulation nReg, int nTurnRatio, MotorRunState nRunState, long nTacho)
{
byte[] data = new byte[12];
data[0] = 0x80;
data[1] = 0x04;
data[2] = (byte)nMotor;
data[3] = (byte)nPower;
data[4] = (byte)nMode;
data[5] = (byte)nReg;
data[6] = (byte)nTurnRatio;
data[7] = (byte)nRunState;
data[8] = (byte)(nTacho & 0xff);
data[9] = (byte)(nTacho >> 8);
data[10] = (byte)(nTacho >> 16);
data[11] = (byte)(nTacho >> 24);
Tx(data);
}
void FixedUpdate()
{
if (!editorFlag)
{
travelAngle += GetDelta();
if (dbgLogAngle)
{
Debug.Log(travelAngle);
}
//Debug.Log (wheel.GetComponent<Rigidbody> ().IsSleeping ());
if (mode == MotorMode.RevRun)
{
if (travelAngle > AngleB) //Воу, это не работает при отрицательных углах! исправить!
//SendUnlockSequenceToProcessor() - разблокируем процессор, прродолжая выполнение программы
{
mode = MotorMode.Off;
}
// ТОДО: добавить защиту от полной блокировки программы путем добавления тайм-аута, который принудительно продолжает выполнение программы.
}
}
}
/// <summary>
/// Přepnutí módu motoru
/// </summary>
/// <param name="mode">mód</param>
public void changeMode(MotorMode mode)
{
this.mode = mode;
if (velocityHandler != null && positionHandler != null && homingHandler != null)
{
try
{
switch (mode)
{
case MotorMode.velocity: velocityHandler.ActivateProfileVelocityMode(); break;
case MotorMode.position: positionHandler.ActivateProfilePositionMode(); break;
case MotorMode.homing: homingHandler.ActivateHomingMode(); break;
}
}
catch (DeviceException e)
{
state = MotorState.error;
stateObserver.motorStateChanged(MotorState.error, String.Format("{0}\nError: {1}", e.ErrorMessage, errorDictionary.getComunicationErrorMessage(e.ErrorCode)), id, 0, 0, 0, 0);
motorErrorOccuredObserver();
}
}
}
public void GetMotorState(Motor port, out byte power, out MotorMode mode, out MotorRegulation regulation, out byte turnRatio, out MotorRunState state, out long tachoLimit, out long tachoCount, out long blockTachoCount, out long rotationCount)
{
mre.Reset();
GetOutputState(port);
if (!mre.WaitOne(1000, false))
{
throw new NxtTimeout();
}
if (nError > 0)
{
throw new InvalidResult();
}
power = result[4];
mode = (MotorMode)result[5];
regulation = (MotorRegulation)result[6];
turnRatio = result[7];
state = (MotorRunState)result[8];
tachoLimit = Buffer2Long(result, 9);
tachoCount = Buffer2SignedLong(result, 13);
blockTachoCount = Buffer2SignedLong(result, 17);
rotationCount = Buffer2SignedLong(result, 21);
}
/// <summary>
/// <para>[Internal] Run motors on the NXT brick.</para>
/// </summary>
/// <param name="motorPort">MotorPort Port</param>
/// <param name="power">Power Set Point, between -100 and 100.</param>
/// <param name="mode">Mode</param>
/// <param name="regulationMode">Regulation Mode</param>
/// <param name="turnRatio">Turn Ratio, between -100 and 100.</param>
/// <param name="runState">Run State</param>
/// <param name="tachoLimit">Tacho Limit, 0=run forever</param>
/// <returns>Returns true if operation was a success, false otherwise. If false, check LastError.</returns>
internal void SetOutputState(Motor motorPort
, sbyte power, MotorMode mode
, MotorReg regulationMode, sbyte turnRatio
, MotorState runState, UInt32 tachoLimit)
{
if (power < -100) power = -100;
if (power > 100) power = 100;
if (turnRatio < -100) turnRatio = -100;
if (turnRatio > 100) turnRatio = 100;
byte[] request = new byte[12];
request[0] = (byte)(0x00);
request[1] = (byte)(DirectCommand.SetOutputState);
request[2] = (byte)motorPort;
request[3] = (byte)power;
request[4] = (byte)mode;
request[5] = (byte)regulationMode;
request[6] = (byte)turnRatio;
request[7] = (byte)runState;
SetUInt32(tachoLimit, request, 8);
CompleteRequest(request);
}
/// <summary>
/// Přepnutí módu motoru
/// </summary>
/// <param name="mode">mód</param>
public void changeMode(MotorMode mode)
{
this.mode = mode;
}
/// <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)
{
this.mode = mode;
this.stateObserver = stateObserver;
this.motorErrorOccuredObserver = motorErrorOccuredObserver;
this.id = id;
this.multiplier = multiplier;
if (reverse)
{
rev = -1;
}
changeMode(mode);
setStateObserver();
state = MotorState.enabled;
stateObserver.motorStateChanged(MotorState.enabled, "", 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>
/// <param name="minPosition">minimální pozice motoru</param>
/// <param name="maxPosition">maximální pozice motoru</param>
/// <param name="angleMap">Pole definující vztah úhlu ze škály uvedené u motoru a opravdového úhlu a pozicí motoru. Jeden řádek obsahuje {úhel ze škály ve stupních; opravdový úhel; pozice motoru}</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, int[,] angleMap)
{
this.angleMap = angleMap;
inicialize(connector, stateObserver, motorErrorOccuredObserver, nodeNumber, id, mode, reverse, multiplier, positionVelocity, positionAceleration, positionDeceleration, velocity, aceleration, deceleration, angleMap[angleMap.GetLength(0) - 1, 2], angleMap[0, 2], angleMap[angleMap.GetLength(0) - 1, 1], angleMap[0, 1]);
}
/// <summary>
/// Přepnutí módu motoru
/// </summary>
/// <param name="mode">mód</param>
public void changeMode(MotorMode mode)
{
this.mode = mode;
if (velocityHandler != null && positionHandler != null && homingHandler != null)
{
try
{
switch (mode)
{
case MotorMode.velocity: velocityHandler.ActivateProfileVelocityMode(); break;
case MotorMode.position: positionHandler.ActivateProfilePositionMode(); break;
case MotorMode.homing: homingHandler.ActivateHomingMode(); break;
}
}
catch (DeviceException e)
{
state = MotorState.error;
stateObserver.motorStateChanged(MotorState.error, String.Format("{0}\nError: {1}", e.ErrorMessage, errorDictionary.getComunicationErrorMessage(e.ErrorCode)), id, 0, 0, 0, 0);
motorErrorOccuredObserver();
}
}
}
/// <summary>
/// Teleops the initialize.
/// </summary>
public override void TeleopInit()
{
Set(0.0f);
Apply();
mMode = MotorMode.UserControl;
}
public void setResistance(float resistance)//抵抗 resistance の単位は [N]
{
currentMode = MotorMode.isShowingResistance;
}
public void Output(Motor nMotor, int nPower, MotorMode nMode, MotorRegulation nReg, int nTurnRatio, MotorRunState nRunState, long nTacho)
{
byte[] data = new byte[12];
data[0] = 0x80;
data[1] = 0x04;
data[2] = (byte)nMotor;
data[3] = (byte)nPower;
data[4] = (byte)nMode;
data[5] = (byte)nReg;
data[6] = (byte)nTurnRatio;
data[7] = (byte)nRunState;
data[8] = (byte)(nTacho & 0xff);
data[9] = (byte)(nTacho >> 8);
data[10] = (byte)(nTacho >> 16);
data[11] = (byte)(nTacho >> 24);
Tx(data);
}
/// <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>
/// <param name="minPosition">minimální pozice motoru</param>
/// <param name="maxPosition">maximální pozice motoru</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, int minPosition, int maxPosition, int minAngle, int maxAngle)
{
hasPositionLimit = true;
this.maxPosition = maxPosition;
this.minPosition = minPosition;
this.minAngle = minAngle;
this.maxAngle = maxAngle;
inicialize(connector, stateObserver, motorErrorOccuredObserver, nodeNumber, id, mode, reverse, multiplier, positionVelocity, positionAceleration, positionDeceleration, velocity, aceleration, deceleration);
}
public abstract MotorState execute(bool[] mask, MotorMode[] modes, int[] positions, int[] velocities, int[] accelerations);
/// <summary>
/// Set the connected sensor's mode
/// </summary>
/// <param name="mode">The requested mode.</param>
public void SetMode(MotorMode mode)
{
Mode = (byte)mode;
}
/// <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>
/// Přepnutí módu motoru
/// </summary>
/// <param name="mode">mód</param>
public void changeMode(MotorMode mode)
{
this.mode = mode;
}
/// <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>
/// <param name="minPosition">minimální pozice motoru</param>
/// <param name="maxPosition">maximální pozice motoru</param>
/// <param name="angleMap">Pole definující vztah úhlu ze škály uvedené u motoru a opravdového úhlu a pozicí motoru. Jeden řádek obsahuje {úhel ze škály ve stupních; opravdový úhel; pozice motoru}</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, int[,] angleMap)
{
this.angleMap = angleMap;
inicialize(connector, stateObserver, motorErrorOccuredObserver, nodeNumber, id, mode, reverse, multiplier, positionVelocity, positionAceleration, positionDeceleration, velocity, aceleration, deceleration, angleMap[angleMap.GetLength(0) - 1, 2], angleMap[0, 2], angleMap[angleMap.GetLength(0) - 1, 1], angleMap[0, 1]);
}
public void RunForSeconds(float speed, float time)
{
mode = MotorMode.TimedRun;
}
public void GetMotorState(Motor port, out byte power, out MotorMode mode, out MotorRegulation regulation, out byte turnRatio, out MotorRunState state, out long tachoLimit, out long tachoCount, out long blockTachoCount, out long rotationCount)
{
mre.Reset();
GetOutputState(port);
if (!mre.WaitOne(1000, false))
throw new NxtTimeout();
if (nError > 0)
throw new InvalidResult();
power = result[4];
mode = (MotorMode)result[5];
regulation = (MotorRegulation)result[6];
turnRatio = result[7];
state = (MotorRunState)result[8];
tachoLimit = Buffer2Long(result, 9);
tachoCount = Buffer2SignedLong(result, 13);
blockTachoCount = Buffer2SignedLong(result, 17);
rotationCount = Buffer2SignedLong(result, 21);
}
/// <summary>
/// Set the connected sensor's mode
/// </summary>
/// <param name="mode">The requested mode.</param>
public void SetMode(MotorMode mode)
{
Mode = (byte)mode;
}
/// <summary>
/// Initializes a new instance of the <see cref="MotorState"/> class.
/// </summary>
///
/// <param name="power">Power, [-100, 100].</param>
/// <param name="turnRatio">Turn ratio, [-100, 100].</param>
/// <param name="mode">Mode (bit field).</param>
/// <param name="regulation">Regulation mode.</param>
/// <param name="runState">Run state.</param>
/// <param name="tachoLimit">The value determines motor's run limit.</param>
///
public MotorState( int power, int turnRatio, MotorMode mode,
MotorRegulationMode regulation, MotorRunState runState, int tachoLimit )
{
Power = power;
TurnRatio = turnRatio;
Mode = mode;
Regulation = regulation;
RunState = runState;
TachoLimit = tachoLimit;
TachoCount = 0;
BlockTachoCount = 0;
RotationCount = 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>
/// <param name="minPosition">minimální pozice motoru</param>
/// <param name="maxPosition">maximální pozice motoru</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, int minPosition, int maxPosition, int minAngle, int maxAngle)
{
hasPositionLimit = true;
this.maxPosition = maxPosition;
this.minPosition = minPosition;
this.minAngle = minAngle;
this.maxAngle = maxAngle;
inicialize(connector, stateObserver, motorErrorOccuredObserver, nodeNumber, id, mode, reverse, multiplier, positionVelocity, positionAceleration, positionDeceleration, velocity, aceleration, deceleration);
}
public override MotorState execute(bool[] mask, MotorMode[] modes, int[] positions, int[] velocities, int[] accelerations)
{
return execute(new MotorCommand(mask, modes, positions, velocities, accelerations));
}
/// <summary>
/// Disableds the initialize.
/// </summary>
public override void DisabledInit()
{
Apply();
mMode = MotorMode.Stopped;
}
