public RobotEngineMc()
{
_brick = new McNxtBrick(NxtCommLinkType.Bluetooth, Config.SerialPortNo)
{
MotorA = new McNxtMotor(),
MotorB = new McNxtMotor(),
MotorC = new McNxtMotor()
};
_motorCam = (McNxtMotor)_brick.MotorC;
_motorSync = new McNxtMotorSync(_brick.MotorA, _brick.MotorB);
}
public static void WaitForMotorToFinish(McNxtMotor motor)
{
do
{
if (motor.TachoCount != null)
{
_runningTacho = motor.TachoCount.Value;
}
Thread.Sleep(500); // now wait half a second!
motor.Poll();
} while (motor.TachoCount.HasValue
&& motor.TachoCount.Value != _runningTacho);
}
public static int RunAndWaitOnCompletion(
McNxtMotor motor, uint tacho, MotorDirection direction)
{
_runningTacho = 0;
sbyte speed = GetSpeed(direction);
motor.Run(speed, tacho);
WaitForMotorToFinish(motor);
Debug.WriteLine("Real tacho {0} degrees", motor.TachoCount);
if (motor.TachoCount == null)
throw new NxtException("Error getting tacho count from motor.");
return motor.TachoCount.Value;
}
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;
}
}