/// <summary>
/// [Native] Sync two motors as driving motors, specifying whether to brake or coast and the turn ratio.
/// </summary>
/// <param name="port">The ports of the selected motors. Exactly two motors must be selected.</param>
/// <param name="power">The power at which to move the motors, between -100 and 100.</param>
/// <param name="stop">Whether to brake or coast during operation. Braking uses far more power.</param>
/// <param name="turnratio">The turn ratio of the two motors, between -100 and 100. A ratio of zero will make both motors move straight.
/// A negative ratio will move the left motor more and a positive ratio would move the right motor more.</param>
public void MoveMotors(MotorPortSyncable port, int power, MotorStop stop, int turnratio)
{
int[] pow = new int[] { power, power };
//Arc and Point rotation
//Both arc and point rotation may be used for joystick control
double ratio = (double)turnratio;
double powr = (double)power;
//pow = new int[] { Convert.ToInt32(Math.Floor(((50 - diff) / 100) * powr)),
//Convert.ToInt32(Math.Floor(((50 + diff) / 100) * powr)) };
//pow = new int[] { Convert.ToInt32(Math.Floor(((50 - diff) / 100) * powr)),
//Convert.ToInt32(Math.Floor(((50 + diff) / 100) * powr)) };
//slave motor power = master power * turn ratio
//find which motor is the master
if (turnratio != 0)
{
bool masterleft = true;
if (power < 0)
{
if (turnratio < 0)
{
masterleft = true;
}
else
{
masterleft = false;
}
}
else
{
if (turnratio <= 0)
{
masterleft = false;
}
else
{
masterleft = true;
}
}
double equ = ((-2 * Math.Abs(ratio)) + 100) / 100 * powr;
if (masterleft == true)
{
pow = new int[] { power, Convert.ToInt32(Math.Floor(equ)) };
}
else
{
pow = new int[] { Convert.ToInt32(Math.Floor(equ)), power };
}
}
MotorMode mode = MotorMode.MotorOn;
if (stop == MotorStop.Coast)
{
mode = MotorMode.On_Regulated;
if ((power < 75) && (power > -75))
{
mode = MotorMode.MotorOn;
}
}
if (stop == MotorStop.Brake)
{
mode = MotorMode.On_Brake_Regulated;
if ((power < 75) && (power > -75))
{
mode = MotorMode.On_Brake;
}
}
Motor[] motors = new Motor[] { };
if (port == MotorPortSyncable.AB) { motors = new Motor[] { Motor.A, Motor.B }; }
if (port == MotorPortSyncable.AC) { motors = new Motor[] { Motor.A, Motor.C }; }
if (port == MotorPortSyncable.BC) { motors = new Motor[] { Motor.B, Motor.C }; }
if (port == MotorPortSyncable.BA) { motors = new Motor[] { Motor.B, Motor.A }; }
if (port == MotorPortSyncable.CA) { motors = new Motor[] { Motor.C, Motor.A }; }
if (port == MotorPortSyncable.CB) { motors = new Motor[] { Motor.C, Motor.B }; }
SetOutputState(motors[0], (sbyte)pow[0], mode,
MotorReg.Speed, (sbyte)turnratio, MotorState.Running, (uint)0);
SetOutputState(motors[1], (sbyte)pow[1], mode,
MotorReg.Speed, (sbyte)turnratio, MotorState.Running, (uint)0);
return;
}
/// <summary>
/// [Native] Smoothly start or stop motors, specifying whether to brake or coast.
/// </summary>
/// <param name="port">The ports of the selected motors.</param>
/// <param name="power">The power at which to move the motors, between -100 and 100.</param>
/// <param name="stop">Whether to brake or coast during operation. Braking uses far more power.</param>
/// <param name="smooth">Whether to smoothly spin up or down the motors.</param>
/// <param name="degrees">The number of degrees the motor revolves until is it fully spun up or down. Smaller values
/// make the motors move more smoothly. Set degrees to zero to run infinitely.</param>
public void MoveMotors(MotorPort port, int power, MotorStop stop, MotorSmooth smooth, int degrees)
{
MotorMode mode = MotorMode.On_Regulated;
if (stop == MotorStop.Coast)
{
mode = MotorMode.On_Regulated;
if ((power < 75) && (power > -75)) { mode = MotorMode.MotorOn; }
}
if (stop == MotorStop.Brake)
{
mode = MotorMode.On_Brake_Regulated;
if ((power < 75) && (power > -75)) { mode = MotorMode.On_Brake; }
}
if (port != MotorPort.AB && port != MotorPort.AC && port != MotorPort.BC)
{
SetOutputState((Motor)((byte)port), (sbyte)power, mode,
MotorReg.Speed, 0, MotorState.Running, 0);
}
else
{
MotorState state = MotorState.Rampup;
if (smooth == MotorSmooth.Smooth_Stop)
{
state = MotorState.RampDown;
}
Motor[] motors = new Motor[] { };
if (port == MotorPort.AB) { motors = new Motor[] { Motor.A, Motor.B }; }
if (port == MotorPort.AC) { motors = new Motor[] { Motor.A, Motor.C }; }
if (port == MotorPort.BC) { motors = new Motor[] { Motor.B, Motor.C }; }
SetOutputState(motors[0], (sbyte)power, mode,
MotorReg.Sync, 0, state, (uint)degrees);
SetOutputState(motors[1], (sbyte)power, mode,
MotorReg.Sync, 0, state, (uint)degrees);
}
return;
}
/// <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>
/// [Native] Move NXT motors.
/// </summary>
/// <param name="port">The ports of the selected motors.</param>
/// <param name="power">The power at which to move the motors, between -100 and 100.</param>
public void MoveMotors(MotorPort port, int power)
{
MotorMode mode = MotorMode.On_Regulated;
if ((power < 75) && (power > -75)) { mode = MotorMode.MotorOn; }
if (port != MotorPort.AB && port != MotorPort.AC && port != MotorPort.BC)
{
SetOutputState((Motor)((byte)port), (sbyte)power, mode,
MotorReg.Speed, 0, MotorState.Running, 0);
}
else
{
Motor[] motors = new Motor[] { };
if (port == MotorPort.AB) { motors = new Motor[] { Motor.A, Motor.B }; }
if (port == MotorPort.AC) { motors = new Motor[] { Motor.A, Motor.C }; }
if (port == MotorPort.BC) { motors = new Motor[] { Motor.B, Motor.C }; }
SetOutputState(motors[0], (sbyte)power, mode,
MotorReg.Sync, 0, MotorState.Running, (uint)0);
SetOutputState(motors[1], (sbyte)power, mode,
MotorReg.Sync, 0, MotorState.Running, (uint)0);
}
return;
}
/// <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(Motor motor, Protocol.MotorPortSingle port)
{
if (motor != null)
{
motorArr[port] = motor;
motor.Brick = this;
motor.Port = port;
}
}