/// <summary>
/// Copy generic state to this state, transforming data members.
/// </summary>
/// <param name="genericState"></param>
public void CopyFromGenericState(pxmotor.MotorState genericState)
{
if (!string.IsNullOrEmpty(genericState.Name))
{
this.Name = genericState.Name;
}
switch (genericState.HardwareIdentifier)
{
case 1:
this.MotorPort = NxtMotorPort.MotorA;
break;
case 2:
this.MotorPort = NxtMotorPort.MotorB;
break;
case 3:
this.MotorPort = NxtMotorPort.MotorC;
break;
}
this.TargetPower = genericState.CurrentPower;
this.ReversePolarity = genericState.ReversePolarity;
}
/// <summary>
/// Post Configure Sensor Connection with body and return the response port.
/// </summary>
/// <param name="motorPort"></param>
/// <returns></returns>
public virtual PortSet <DefaultUpdateResponseType, Fault> ConnectToBrick(NxtMotorPort motorPort)
{
ConnectToBrick op = new ConnectToBrick(motorPort);
this.PostUnknownType(op);
return(op.ResponsePort);
}
/// <summary>
/// <para>SETOUTPUTSTATE</para>
/// </summary>
/// <remarks>
/// <para>Reference: BDK, Appendix 2, p. 6.</para>
/// </remarks>
/// <param name="motorPort">Motor port</param>
/// <param name="power">Power set point</param>
/// <param name="mode">Mode</param>
/// <param name="regulationMode">Regulation mode</param>
/// <param name="turnRatio">Turn ratio</param>
/// <param name="runState">Run state</param>
/// <param name="tachoLimit">Tacho limit, 0: run forever</param>
public void SetOutputState(NxtMotorPort motorPort, sbyte power, NxtMotorMode mode, NxtMotorRegulationMode regulationMode, sbyte turnRatio, NxtMotorRunState 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)(ReplyRequired ? 0x00 : 0x80);
request[1] = (byte)NxtCommand.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;
Util.SetUInt32(tachoLimit, request, 8);
Send(request);
}
/// <summary>
/// <para>Attaches a motor to the NXT brick.</para>
/// </summary>
/// <param name="motor">The motor</param>
/// <param name="port">The port to attach the motor to</param>
private void AttachMotor(NxtMotor motor, NxtMotorPort port)
{
if (motor != null)
{
motorArr[port] = motor;
motor.Brick = this;
motor.Port = port;
}
}
/// <summary>
/// <para>RESETMOTORPOSITION</para>
/// </summary>
/// <remarks>
/// <para>Reference: BDK, Appendix 2, p. 9.</para>
/// </remarks>
/// <param name="motorPort">Output port</param>
/// <param name="relative">Relative? True: position relative to last movement, False: absolute position</param>
public void ResetMotorPosition(NxtMotorPort motorPort, bool relative)
{
byte[] request = new byte[] {
(byte)(ReplyRequired ? 0x00 : 0x80),
(byte)NxtCommand.ResetMotorPosition,
(byte)motorPort,
(byte)(relative ? 0xFF : 0x00)
};
Send(request);
}
/// <summary>
/// <para>GETOUTPUTSTATE</para>
/// </summary>
/// <remarks>
/// <para>Reference: BDK, Appendix 2, p. 8.</para>
/// </remarks>
/// <param name="motorPort">Ourput Port</param>
/// <returns>Returns a parsed NxtGetOutputStateReply with the reply</returns>
public NxtGetOutputStateReply?GetOutputState(NxtMotorPort motorPort)
{
byte[] request = new byte[] {
0x00,
(byte)NxtCommand.GetOutputState,
(byte)motorPort
};
byte[] reply = Send(request);
if (reply == null)
{
return(null);
}
byte motorPortOut = reply[3];
if (motorPortOut != (byte)motorPort)
{
throw new NxtException(string.Format("Output motor port, {0}, was different from input motor port, {1}.", motorPortOut, motorPort));
}
NxtGetOutputStateReply result;
result.power = (sbyte)reply[4];
result.mode = (NxtMotorMode)reply[5];
result.regulationMode = (NxtMotorRegulationMode)reply[6];
result.turnRatio = (sbyte)reply[7];
result.runState = (NxtMotorRunState)reply[8];
result.tachoLimit = Util.GetUInt32(reply, 9);
result.tachoCount = Util.GetInt32(reply, 13);
result.blockTachoCount = Util.GetInt32(reply, 17);
result.rotationCount = Util.GetInt32(reply, 21);
return(result);
}
/// <summary>
/// Get the HardwareIdentifier representation of a NxtMotorPort (1-3).
/// </summary>
/// <param name="port"></param>
/// <returns></returns>
public static int HardwareIdentifier(NxtMotorPort port)
{
return ((int)(PortNumber(port) + 1));
}
/// <summary>
/// LEGO NXT Wheel Configuration
/// </summary>
/// <param name="motorPort"></param>
/// <param name="reversePolarity"></param>
/// <param name="wheelDiameter"></param>
public WheelConfiguration(NxtMotorPort motorPort, bool reversePolarity, double wheelDiameter)
{
this.MotorPort = motorPort;
this.ReversePolarity = reversePolarity;
this.WheelDiameter = wheelDiameter;
}
/// <summary>
/// <para>GETOUTPUTSTATE</para>
/// </summary>
/// <remarks>
/// <para>Reference: BDK, Appendix 2, p. 8.</para>
/// </remarks>
/// <param name="motorPort">Ourput Port</param>
/// <returns>Returns a parsed NxtGetOutputStateReply with the reply</returns>
public NxtGetOutputStateReply? GetOutputState(NxtMotorPort motorPort)
{
byte[] request = new byte[] {
0x00,
(byte) NxtCommand.GetOutputState,
(byte) motorPort
};
byte[] reply = Send(request);
if (reply == null) return null;
byte motorPortOut = reply[3];
if (motorPortOut != (byte)motorPort)
throw new NxtException(string.Format("Output motor port, {0}, was different from input motor port, {1}.", motorPortOut, motorPort));
NxtGetOutputStateReply result;
result.power = (sbyte)reply[4];
result.mode = (NxtMotorMode)reply[5];
result.regulationMode = (NxtMotorRegulationMode)reply[6];
result.turnRatio = (sbyte)reply[7];
result.runState = (NxtMotorRunState)reply[8];
result.tachoLimit = Util.GetUInt32(reply, 9);
result.tachoCount = Util.GetInt32(reply, 13);
result.blockTachoCount = Util.GetInt32(reply, 17);
result.rotationCount = Util.GetInt32(reply, 21);
return result;
}
/// <summary>
/// <para>SETOUTPUTSTATE</para>
/// </summary>
/// <remarks>
/// <para>Reference: BDK, Appendix 2, p. 6.</para>
/// </remarks>
/// <param name="motorPort">Motor port</param>
/// <param name="power">Power set point</param>
/// <param name="mode">Mode</param>
/// <param name="regulationMode">Regulation mode</param>
/// <param name="turnRatio">Turn ratio</param>
/// <param name="runState">Run state</param>
/// <param name="tachoLimit">Tacho limit, 0: run forever</param>
public void SetOutputState(NxtMotorPort motorPort, sbyte power, NxtMotorMode mode, NxtMotorRegulationMode regulationMode, sbyte turnRatio, NxtMotorRunState 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)(ReplyRequired ? 0x00 : 0x80);
request[1] = (byte)NxtCommand.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;
Util.SetUInt32(tachoLimit, request, 8);
Send(request);
}
/// <summary>
/// <para>RESETMOTORPOSITION</para>
/// </summary>
/// <remarks>
/// <para>Reference: BDK, Appendix 2, p. 9.</para>
/// </remarks>
/// <param name="motorPort">Output port</param>
/// <param name="relative">Relative? True: position relative to last movement, False: absolute position</param>
public void ResetMotorPosition(NxtMotorPort motorPort, bool relative)
{
byte[] request = new byte[] {
(byte) (ReplyRequired ? 0x00 : 0x80),
(byte) NxtCommand.ResetMotorPosition,
(byte) motorPort,
(byte) (relative ? 0xFF : 0x00)
};
Send(request);
}
/// <summary>
/// LEGO NXT Command: GetOutputState
/// </summary>
/// <param name="outputPort"></param>
public LegoGetOutputState(NxtMotorPort outputPort)
: base(25, LegoCommand.NxtDirectCommand, (byte)LegoCommandCode.GetOutputState, 0x00)
{
base.RequireResponse = true;
OutputPort = outputPort;
}
/// <summary>
/// Configure Device Connection
/// </summary>
/// <param name="motorPort"></param>
public ConnectToBrick(NxtMotorPort motorPort)
: base(new MotorConfig(motorPort))
{
}
/// <summary>
/// <para>Attaches a motor to the NXT brick.</para>
/// </summary>
/// <param name="motor">The motor</param>
/// <param name="port">The port to attach the motor to</param>
private void AttachMotor(NxtMotor motor, NxtMotorPort port)
{
if (motor != null)
{
motorArr[port] = motor;
motor.Brick = this;
motor.Port = port;
}
}
public MotorPort(NxtMotorPort motor, int brick)
: this()
{
Motor = motor;
Brick = brick;
}
static void LinkMotorAxis()
{
Console.WriteLine("\nYou now have to specify which Motor represents which 3-dimensional Axis.");
Console.WriteLine("Enter ('A'/'B'/'C') the Motor controlling X-Axis (left-right) movement:");
MotorXaxis = Console.ReadKey().KeyChar;
Console.WriteLine("\nEnter ('A'/'B'/'C') the Motor controlling Y-Axis (foward-backward) movement: (This should be the axis that pushes the drill foward)");
MotorYaxis = Console.ReadKey().KeyChar;
Console.WriteLine("\nEnter ('A'/'B'/'C') the Motor controlling Z-Axis (up-down) movement:");
MotorZaxis = Console.ReadKey().KeyChar;
Xmotor = new McNxtMotor();
Ymotor = new McNxtMotor();
Zmotor = new McNxtMotor();
switch (MotorXaxis)
{
case 'a':
brick.MotorA = Xmotor;
XmotorPort = NxtMotorPort.PortA;
break;
case 'b':
brick.MotorB = Xmotor;
XmotorPort = NxtMotorPort.PortB;
break;
case 'c':
brick.MotorC = Xmotor;
XmotorPort = NxtMotorPort.PortC;
break;
}
switch (MotorYaxis)
{
case 'a':
brick.MotorA = Ymotor;
YmotorPort = NxtMotorPort.PortA;
break;
case 'b':
brick.MotorB = Ymotor;
YmotorPort = NxtMotorPort.PortB;
break;
case 'c':
brick.MotorC = Ymotor;
YmotorPort = NxtMotorPort.PortC;
break;
}
switch (MotorZaxis)
{
case 'a':
brick.MotorA = Zmotor;
ZmotorPort = NxtMotorPort.PortA;
break;
case 'b':
brick.MotorB = Zmotor;
ZmotorPort = NxtMotorPort.PortB;
break;
case 'c':
brick.MotorC = Zmotor;
ZmotorPort = NxtMotorPort.PortC;
break;
}
}
/// <summary>
/// Convert Hardware Identifier (1-3) to MotorPort (A-C)
/// </summary>
/// <param name="hardwareIdentifier"></param>
/// <param name="defaultMotorPort"></param>
/// <returns></returns>
internal static NxtMotorPort HardwareIdentifierToMotorPort(int hardwareIdentifier, NxtMotorPort defaultMotorPort)
{
NxtMotorPort motorPort;
switch (hardwareIdentifier)
{
case 1:
motorPort = NxtMotorPort.MotorA;
break;
case 2:
motorPort = NxtMotorPort.MotorB;
break;
case 3:
motorPort = NxtMotorPort.MotorC;
break;
default:
motorPort = defaultMotorPort;
break;
}
return motorPort;
}
/// <summary>
/// LEGO NXT Motor Configuration.
/// </summary>
/// <param name="motorPort"></param>
public MotorConfig(NxtMotorPort motorPort)
{
this.MotorPort = motorPort;
}
/// <summary>
/// Get the integer representation of a NxtMotorPort (0-2).
/// </summary>
/// <param name="port"></param>
/// <returns></returns>
public static byte PortNumber(NxtMotorPort port)
{
switch (port)
{
case NxtMotorPort.MotorA:
return 0;
case NxtMotorPort.MotorB:
return 1;
case NxtMotorPort.MotorC:
return 2;
case NxtMotorPort.AnyMotorPort:
return 255;
default:
return 0;
}
}
/// <summary>
/// LEGO NXT Command: Set Output State
/// </summary>
/// <param name="motorPort"></param>
/// <param name="powerSetPoint"></param>
/// <param name="mode"></param>
/// <param name="regulationMode"></param>
/// <param name="turnRatio"></param>
/// <param name="runState"></param>
/// <param name="rotationLimit"></param>
public LegoSetOutputState(NxtMotorPort motorPort,
int powerSetPoint,
LegoOutputMode mode,
LegoRegulationMode regulationMode,
int turnRatio,
RunState runState,
long rotationLimit)
: base(3, LegoCommand.NxtDirectCommand, (byte)LegoCommandCode.SetOutputState, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00)
{
this.MotorPort = motorPort;
this.PowerSetPoint = powerSetPoint;
this.Mode = mode;
this.RegulationMode = regulationMode;
this.TurnRatio = turnRatio;
this.RunState = runState;
this.EncoderLimit = rotationLimit;
}
/// <summary>
/// Convert Motor Port (A-C) to HardwareIdentifier (1-3)
/// </summary>
/// <param name="motorPort"></param>
/// <returns></returns>
internal static int MotorPortToHardwareIdentifier(NxtMotorPort motorPort)
{
// Set the hardware identifier from the connected motor port.
switch (motorPort)
{
case NxtMotorPort.MotorA:
return 1;
case NxtMotorPort.MotorB:
return 2;
case NxtMotorPort.MotorC:
return 3;
}
return 0;
}
/// <summary>
/// Reset Motor Position
/// </summary>
/// <param name="outputPort"></param>
/// <param name="relative"></param>
public LegoResetMotorPosition(NxtMotorPort outputPort, bool relative)
: base(3, NxtDirectCommand, (byte)LegoCommandCode.ResetMotorPosition, 0x00, 0x00)
{
OutputPort = outputPort;
Relative = relative;
}
static void LinkMotorAxis()
{
Console.WriteLine("\nYou now have to specify which Motor represents which 3-dimensional Axis.");
Console.WriteLine("Enter ('A'/'B'/'C') the Motor controlling X-Axis (left-right) movement:");
MotorXaxis = Console.ReadKey().KeyChar;
Console.WriteLine("\nEnter ('A'/'B'/'C') the Motor controlling Y-Axis (foward-backward) movement: (This should be the axis that pushes the drill foward)");
MotorYaxis = Console.ReadKey().KeyChar;
Console.WriteLine("\nEnter ('A'/'B'/'C') the Motor controlling Z-Axis (up-down) movement:");
MotorZaxis = Console.ReadKey().KeyChar;
Xmotor = new McNxtMotor();
Ymotor = new McNxtMotor();
Zmotor = new McNxtMotor();
switch (MotorXaxis)
{
case 'a':
brick.MotorA = Xmotor;
XmotorPort = NxtMotorPort.PortA;
break;
case 'b':
brick.MotorB = Xmotor;
XmotorPort = NxtMotorPort.PortB;
break;
case 'c':
brick.MotorC = Xmotor;
XmotorPort = NxtMotorPort.PortC;
break;
}
switch (MotorYaxis)
{
case 'a':
brick.MotorA = Ymotor;
YmotorPort = NxtMotorPort.PortA;
break;
case 'b':
brick.MotorB = Ymotor;
YmotorPort = NxtMotorPort.PortB;
break;
case 'c':
brick.MotorC = Ymotor;
YmotorPort = NxtMotorPort.PortC;
break;
}
switch (MotorZaxis)
{
case 'a':
brick.MotorA = Zmotor;
ZmotorPort = NxtMotorPort.PortA;
break;
case 'b':
brick.MotorB = Zmotor;
ZmotorPort = NxtMotorPort.PortB;
break;
case 'c':
brick.MotorC = Zmotor;
ZmotorPort = NxtMotorPort.PortC;
break;
}
}
