public IMotorModel this[MotorPort port]
{
get
{
return(motors[(int)port]);
}
}
/// <summary>
/// Read a motor encoder in degrees
/// </summary>
/// <param name="port">The Motor port to use, can be MotorLeft or MotorRight</param>
/// <returns>Returns the encoder position in degrees</returns>
public int GetMotorEncoder(MotorPort port)
{
SpiMessageType message_type;
if (port == MotorPort.MotorLeft)
{
message_type = SpiMessageType.GetMotorEncoderLeft;
}
else if (port == MotorPort.MotorRight)
{
message_type = SpiMessageType.GetMotorEncoderRight;
}
else
{
throw new IOException($"{nameof(GetMotorEncoder)} error. Must be one motor port at a time, PortMotorLeft or PortMotorRight");
}
var encoder = SpiRead32(message_type);
if ((encoder & 0x80000000) > 0)
{
encoder = (int)(encoder - 0x100000000);
}
return((int)(encoder / MotorTicksPerDegree));
}
protected void ResetTacho(MotorPort port)
{
lock (_commands)
{
Motors[port].ResetTacho();
}
}
public PositionPID(MotorPort port, Int32 position, bool brake, sbyte maxPower, float P, float I, float D, float sampleTime) :
base(P, I, D, sampleTime, (float)maxPower, -((float)maxPower))
{
this.motor = new Motor(port);
target = position;
this.brake = brake;
}
} // Sağ Yön Dönüş İsteği
private void solyon_butonu_CheckedChanged(object sender, EventArgs e)
{
int trackBarValue3 = hiz_trackbar.Value;
byte[] outData3 = BitConverter.GetBytes(trackBarValue3 + 20);
MotorPort.Write(outData3, 0, 4);
} // Sol Yön Dönüş İsteği
public PositionPID(MotorPort port, Int32 position, bool brake, sbyte maxPower, float P, float I, float D, float sampleTime)
: base(P,I,D,sampleTime, (float) maxPower, -((float) maxPower))
{
this.motor = new Motor(port);
target = position;
this.brake = brake;
}
public static void FinalizePRogram()
{
BIOS_FINALE_BEEP();
Writeln("Finalization called!");
if (IsRobot)
{
for (MotorPort i = MotorPort.OutA; i <= MotorPort.OutD; i++)
{
// Turn off all motors
Motor FinaleMotor = new Motor(i);
FinaleMotor.Brake();
FinaleMotor.ResetTacho();
FinaleMotor.Off();
}
for (SensorPort i = SensorPort.In1; i <= SensorPort.In4; i++)
{
// Cannot finalize all sensors
}
}
else
{
// Windows
}
Writeln("Finalization finished.");
LogFileWriter.Close();
LogFileWriter = null;
}
public IMotorModel this[MotorPort port]
{
get
{
return motors[(int)port];
}
}
/// <summary>
/// Read a motor status
/// </summary>
/// <param name="port">The Motor port to use, can be MotorLeft or MotorRight</param>
/// <returns>Returns MotorStatus containing the status of the motor</returns>
public MotorStatus GetMotorStatus(MotorPort port)
{
MotorStatus motorStatus = new MotorStatus();
SpiMessageType message_type = (port == MotorPort.MotorRight) ? SpiMessageType.GetMotorStatusRight : SpiMessageType.GetMotorStatusLeft;
byte[] outArray = { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
var reply = SpiTransferArray(outArray);
if (reply[3] == SpiCorrectDataReturned)
{
motorStatus.Speed = reply[5];
if ((motorStatus.Speed & 0x80) > 0)
{
motorStatus.Speed = -motorStatus.Speed;
}
motorStatus.Encoder = (int)(BinaryPrimitives.ReadInt32BigEndian(new Span <byte>(reply).Slice(6)) / MotorTicksPerDegree);
motorStatus.Dps = ((reply[10] << 8) | reply[11]);
if ((motorStatus.Dps & 0x8000) > 0)
{
motorStatus.Dps = motorStatus.Dps - 0x10000;
}
motorStatus.Flags = (MotorStatusFlags)reply[4];
}
else
{
throw new IOException($"{nameof(GetMotorStatus)} error: no SPI response");
}
return(motorStatus);
}
public Tank(MotorPort left, MotorPort right)
{
vehicle = new Vehicle(left, right);
vehicle.Off();
tankRadius = tankWidthInCm / 2;
leftMotor = new Motor(left);
rightMotor = new Motor(right);
}
} // Motoru Kapatma
//DC MOTOR KONTROLÜ
//SERVO MOTOR KONTROLÜ
private void servo_ac_butonu_Click(object sender, EventArgs e)
{
int servoMotorValue = 250;
byte[] outData3 = BitConverter.GetBytes(servoMotorValue);
MotorPort.Write(outData3, 0, 4);
Console.Write("ABC");
}
/// <summary>
/// Set the motor power in percent
/// </summary>
/// <param name="port">The Motor port to use, can be MotorLeft and/or MotorRight</param>
/// <param name="power">The power from - 100 to 100, or -128 for float</param>
public void SetMotorPower(MotorPort port, int power)
{
power = Math.Clamp(power, -128, 127);
byte bPower = (byte)(power & 0xFF);
byte[] outArray = { SpiAddress, (byte)SpiMessageType.SetMotorPower, (byte)port, bPower };
var ret = SpiTransferArray(outArray);
}
private void servo_kapat_butonu_Click(object sender, EventArgs e)
{
int servoMotorValue2 = 240;
byte[] outData30 = BitConverter.GetBytes(servoMotorValue2);
MotorPort.Write(outData30, 0, 4);
Console.Write("xyC");
}
private void Execute(NxtBrick.MotorState state)
{
// parellelize this slow command
Task.Factory.StartNew(() =>
{
Brick.SetMotorState(MotorPort, state);
Debug.WriteLine("Motor on Port " + MotorPort.ToString() + ": " + state.Power + "%");
});
}
/// <summary>
/// Convert a motor port to bitfield.
/// </summary>
/// <returns>The port to bitfield.</returns>
internal OutputBitfield MotorPortToBitfield(MotorPort port){
if(port == MotorPort.OutA)
return OutputBitfield.OutA;
if(port == MotorPort.OutB)
return OutputBitfield.OutB;
if(port == MotorPort.OutC)
return OutputBitfield.OutC;
return OutputBitfield.OutD;
}
} // Sol Yön Dönüş İsteği
private void makineler_form_FormClosing(object sender, FormClosingEventArgs e)
{
if (MotorPort.IsOpen)
{
byte[] outData4 = BitConverter.GetBytes(20);
MotorPort.Write(outData4, 0, 4);
byte[] outData5 = BitConverter.GetBytes(240);
MotorPort.Write(outData5, 0, 4);
MotorPort.Close();
}
} // Motoru Kapatma
public T this[MotorPort port]
{
get
{
return(_values[(int)port]);
}
set
{
_values[(int)port] = value;
}
}
/// <summary>
/// Create a motor
/// </summary>
/// <param name="brick">GoPiGo3 brick</param>
/// <param name="port">Motor port</param>
/// <param name="timeout">Timeout in milliseconds</param>
public Motor(GoPiGo brick, MotorPort port, int timeout)
{
if (port == MotorPort.Both)
{
throw new ArgumentException($"Motor class can only have 1 motor");
}
_goPiGo = brick;
Port = port;
_periodRefresh = timeout;
_timer = new Timer(UpdateSensor, this, TimeSpan.FromMilliseconds(timeout), TimeSpan.FromMilliseconds(timeout));
}
/// <summary>
/// Sets the type of the output. I don't know what this is used for.
/// </summary>
/// <param name="port">Motor port to use.</param>
/// <param name="type">Type to use.</param>
/// <param name="reply">If set to <c>true</c> reply from brick will be send.</param>
public void SetType(MotorPort port, byte type, bool reply = false){
var command = new Command(0,0,200,reply);
command.Append(ByteCodes.OutputSetType);
command.Append(this.DaisyChainLayer);
command.Append(port);
command.Append(type, ParameterFormat.Short);
connection.Send(command);
if(reply){
var brickReply = connection.Receive();
Error.CheckForError(brickReply,200);
}
}
private bool IsRunning(MotorPort port)
{
try
{
if (_goPiGo.GetMotorStatus(port).Speed == 0)
{
return(false);
}
}
catch (IOException)
{ }
return(true);
}
public RobotMotors(MotorPort mainMotor, MotorPort secondaryMotor, MotorPort handMotor)
{
_motors[0] = new Motor(mainMotor);
_motors[1] = new Motor(secondaryMotor);
_motors[2] = new Motor(handMotor);
// default values
MainMotorSpeed = SByte.MaxValue;
SecondaryMotorSpeed = SByte.MaxValue;
HandMotorSpeed = SByte.MaxValue;
_thread = new Thread(MotorPollThread);
_thread.IsBackground = true;
_thread.Start();
}
/// <summary>
/// Convert a motor port to bitfield.
/// </summary>
/// <returns>The port to bitfield.</returns>
internal OutputBitfield MotorPortToBitfield(MotorPort port)
{
if (port == MotorPort.OutA)
{
return(OutputBitfield.OutA);
}
if (port == MotorPort.OutB)
{
return(OutputBitfield.OutB);
}
if (port == MotorPort.OutC)
{
return(OutputBitfield.OutC);
}
return(OutputBitfield.OutD);
}
public WaitHandle CreateRotateTask(MotorPort port, int targetTacho)
{
Motor motor = Motors[port];
// current pos
int motorTacho = motor.GetTachoCount();
// get target value
targetTacho = (int)Math.Round(targetTacho * RatioSettings[port], MidpointRounding.AwayFromZero);
int err = (targetTacho - motorTacho);
if (err != 0)
{
sbyte speed = SpeedSettings[port];
return(motor.SpeedProfile((err > 0) ? speed : (sbyte)-speed, 0, (uint)Math.Abs(err), 0, true));
}
return(_completedTask);
}
public RobotBase(MotorPort[] motorPorts)
{
Motors = new MotorSettingCollection <Motor>(motorPorts.Length);
SpeedSettings = new MotorSettingCollection <sbyte>(motorPorts.Length);
RatioSettings = new MotorSettingCollection <float>(motorPorts.Length);
for (int i = 0; i < motorPorts.Length; i++)
{
MotorPort port = motorPorts[i];
Motors[port] = new Motor(port);
SpeedSettings[port] = SByte.MaxValue;
RatioSettings[port] = 1;
}
//
_cancel = _cancelSrc.Token;
_thread = new Thread(MotorPollThread);
_thread.IsBackground = true;
_thread.Start();
}
} // Motor Bağlantısı Aktif Etme
private void hiz_trackbar_Scroll(object sender, EventArgs e)
{
int trackBarValue = hiz_trackbar.Value;
byte[] outData;
if (sagyon_butonu.Checked == true)
{
outData = BitConverter.GetBytes(trackBarValue);
MotorPort.Write(outData, 0, 4);
}
else if (solyon_butonu.Checked == true)
{
outData = BitConverter.GetBytes(trackBarValue + 20);
MotorPort.Write(outData, 0, 4);
}
motorhiz_ibresi.Text = Convert.ToString(hiz_trackbar.Value * 10) + "%";
}//Sağ veya Sol butonlarına göre hız arttırma
} //Groupbox ve Hız Göstergesi Bildirimleri
private void motorBaglan_butonu_Click(object sender, EventArgs e)
{
try
{
MotorPort.PortName = ports_combobox.Text;
MotorPort.BaudRate = 9600;
MotorPort.Open();
}
catch (Exception)
{
port_label.Text = "Port Pasif!";
port_label.ForeColor = Color.Red;
}//Motora bağlanmak için Arduinoya İstek Yollama
if (MotorPort.IsOpen)
{
dcmotoryon_groupbox.Enabled = true;
dcmotorhiz_groupbox.Enabled = true;
servomotor_groupbox.Enabled = true;
port_label.Text = "Port Aktif!";
port_label.ForeColor = Color.Green;
} // Motorun Açık olması durumunda Groupboxları aktif etme
} // Motor Bağlantısı Aktif Etme
public string SetOutputState(MotorPort motor, int speed, Mode mode, RegulationMode regMode, int turnRatio, RunState runState)
{
byte[] message = new byte[13]; // message to be sent
// Populate message
message[0] = 0x00; // Feedback required
message[1] = (byte)DirectCommand.SetOutputState;
message[2] = (byte)motor; // output port
message[3] = (byte)speed; // power set point
message[4] = (byte)mode; // mode byte
message[5] = (byte)regMode; // reg mode
message[6] = (byte)turnRatio; //turn ratio
message[7] = (byte)runState; //run state
//figure something out with this - make programmerable
message[8] = 0x00; //tacho limit
message[9] = 0x00;
message[10] = 0x00;
message[11] = 0x00;
message[12] = 0x00;
// Send message
serial_port.Send(message, ref reply);
returnMessage = CheckResponce(ref reply[2]);
return returnMessage;
}
public string GetOutputState(MotorPort Motor)
{
byte[] message = new byte[3];
message[0] = 0x00;
message[1] = (byte)DirectCommand.GetOutputState;
message[2] = (byte)Motor;
serial_port.Send(message, ref reply);
returnMessage = "";
if (reply == null)
return "";
for (int i = 0; i < 25; i++)
{
returnMessage += Convert.ToUInt32(reply[i]) + " : ";
}
returnMessage += ";";
return returnMessage;
}
public string ResetMotorPosition(MotorPort Motor, bool AbsolutePosition)
{
byte[] message = new byte[4];
message[0] = 0x00;
message[1] = 0x0A;
message[2] = (byte)Motor;
message[3] = 0;
serial_port.Send(message, ref reply);
returnMessage = CheckResponce(ref reply[2]);
return returnMessage;
}
/// <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>
/// [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;
}
public Blinker (MotorPort leftPort, MotorPort rightPort)
{
// this.
// this.
}
/// <summary>
/// Gets the tacho count.
/// </summary>
/// <returns>The tacho count.</returns>
/// <param name="port">Motor port to read from</param>
public Int32 GetCount (MotorPort port)
{
return BitConverter.ToInt32(tachoMemory.Read((int)port * TachoBufferSize+ TachoDataOffset, 4),0);
}
public int GetRawTacho(MotorPort port)
{
return(_motors[(int)port].GetTachoCount());
}
/// <summary>
/// Set the motor target position in degrees
/// </summary>
/// <param name="port">The Motor port to use, can be MotorLeft and/or MotorRight</param>
/// <param name="positionDegree">The target position in degree</param>
public void SetMotorPosition(MotorPort port, int positionDegree)
{
byte[] outArray = { SpiAddress, (byte)SpiMessageType.SetMotorPosition, (byte)port, (byte)((positionDegree >> 24) & 0xFF), (byte)((positionDegree >> 16) & 0xFF), (byte)((positionDegree >> 8) & 0xFF), (byte)(positionDegree & 0xFF) };
var ret = SpiTransferArray(outArray);
}
internal async Task<GetOutputStateResponse> GetOutputStateAsyncInternal(MotorPort motorPort)
{
await brick.SendCommandAsyncInternal(RequestTelegram.GetOutputState(motorPort));
return ResponseTelegram.GetOutputState(brick.LastResponse);
}
/// <summary>
/// Create a motor
/// </summary>
/// <param name="brick">GoPiGo3 brick</param>
/// <param name="port">Motor port</param>
public Motor(GoPiGo brick, MotorPort port)
: this(brick, port, 1000)
{
}
/// <summary>
/// Offset a motor encoder
/// </summary>
/// <param name="port">The Motor port to use, can be MotorLeft and/or MotorRight</param>
/// <param name="positionOffset">The encoder offset. Zero the encoder by offsetting it by the current position</param>
public void OffsetMotorEncoder(MotorPort port, int positionOffset)
{
positionOffset = (int)(positionOffset * MotorTicksPerDegree);
byte[] outArray = new byte[] { SpiAddress, (byte)SpiMessageType.OffsetMotorEncoder, (byte)port, (byte)((positionOffset >> 24) & 0xFF), (byte)((positionOffset >> 16) & 0xFF), (byte)((positionOffset >> 8) & 0xFF), (byte)(positionOffset & 0xFF) };
SpiTransferArray(outArray);
}
/// <summary>
/// Append a motor port
/// </summary>
/// <param name="port">Motor port to append</param>
public void Append(MotorPort port){
Append((sbyte) port, ParameterFormat.Short);
}
/// <summary>
/// Initializes a new instance of the <see cref="MonoBrickFirmware.Movement.Motor"/> class.
/// </summary>
/// <param name="port">Port.</param>
public Motor(MotorPort port) : base()
{
this.BitField = MotorPortToBitfield(port);
Reverse = false;
}
/// <summary>
/// Initializes a new instance of the Vehicle class.
/// </summary>
/// <param name='left'>
/// The left motor of the vehicle
/// </param>
/// <param name='right'>
/// The right motor of the vehicle
/// </param>
public Vehicle(MotorPort left, MotorPort right)
{
LeftPort = left;
RightPort = right;
}
/// <summary>
/// Gets the tacho count.
/// </summary>
/// <returns>The tacho count.</returns>
/// <param name="port">Motor port to use</param>
public Int32 GetCount(MotorPort port){
var command = new Command(4,0,212,true);
command.Append(ByteCodes.OutputGetCount);
command.Append(this.DaisyChainLayer);
command.Append(port);
command.Append((byte)0, VariableScope.Global);
var brickReply = connection.SendAndReceive(command);
Error.CheckForError(brickReply,212);
return brickReply.GetInt32(3);
}
/// <summary>
/// Gets the speed of the motor
/// </summary>
/// <returns>The speed.</returns>
/// <param name="port">Motor port to read</param>
public sbyte GetSpeed(MotorPort port){
var command = new Command(8,0,220,true);
command.Append(ByteCodes.OutputRead);
command.Append(this.DaisyChainLayer);
command.Append(port);
command.Append((byte)0, VariableScope.Global);
command.Append((byte)4, VariableScope.Global);
var brickReply = connection.SendAndReceive(command);
Error.CheckForError(brickReply,220);
return brickReply.GetSbyte(3);
//The tacho speed from outputRead does not work
// I have also tried to place the tacho reply in offset 1 (and with 5 global bytes in the reply) but get a error each time
}
internal async Task ResetMotorPositionAsyncInternal(MotorPort motorPort, bool relative)
{
await brick.SendCommandAsyncInternal(RequestTelegram.ResetMotorPosition(motorPort, relative));
}
/// <summary>
/// Resets the motor position on the specified motor port.
/// </summary>
/// <param name="motorPort">The motor to reset.</param>
/// <param name="relative"><c>True</c>, if position relative to last movement, <c>false</c> for absolute position.</param>
public void ResetMotorPosition( MotorPort motorPort, bool relative )
{
var data = CommandHelper.InitializeData(LegoCommandCode.ResetMotorPosition, CommandType.DirectCommandWithoutResponse, 4);
data[ 2 ] = (byte) motorPort;
data[ 3 ] = Convert.ToByte( relative );
Transmit( data ); // Return package: 0:0x02, 1:Command, 2:StatusByte
}
/// <summary>
/// Gets the speed of the motor
/// </summary>
/// <returns>The speed.</returns>
/// <param name="port">Motor port to read</param>
public sbyte GetSpeed(MotorPort port){
return (sbyte) tachoMemory.Read ((int)port * TachoBufferSize + SpeedDataOffset,1)[0];
}
/// <summary>
/// Initializes a new instance of the <see cref="MonoBrickFirmware.IO.MotorSync"/> class.
/// </summary>
/// <param name="port1">Port1.</param>
/// <param name="port2">Port2.</param>
public MotorSync(MotorPort port1, MotorPort port2)
{
this.BitField = base.MotorPortToBitfield(port1) | base.MotorPortToBitfield(port2);
}
/// <summary>
/// Set the motor target position KD constant
/// If you set KP higher, the motor will be more responsive to errors in position, at the cost of perhaps overshooting and oscillating.
/// KD slows down the motor as it approaches the target, and helps to prevent overshoot.
/// In general, if you increase kp, you should also increase KD to keep the motor from overshooting and oscillating.
/// See as well https://en.wikipedia.org/wiki/PID_controller
/// </summary>
/// <param name="port">The Motor port to use, can be MotorLeft and/or MotorRight</param>
/// <param name="kd">The KD constant (default 70)</param>
public void SetMotorPositionKD(MotorPort port, byte kd = 70)
{
byte[] outArray = { SpiAddress, (byte)SpiMessageType.SetMotorPositionKd, (byte)port, kd };
var ret = SpiTransferArray(outArray);
}
/// <summary>
/// Set the motor speed limit
/// </summary>
/// <param name="port">The Motor port to use, can be MotorLeft and/or MotorRight</param>
/// <param name="powerPercent">The power limit in percent (0 to 100), with 0 being no limit (100)</param>
/// <param name="dps">The speed limit in degrees per second, with 0 being no limit</param>
public void SetMotorLimits(MotorPort port, byte powerPercent = 0, int dps = 0)
{
byte[] outArray = { SpiAddress, (byte)SpiMessageType.SetMotorLimits, (byte)port, powerPercent, (byte)((dps >> 8) & 0xFF), (byte)(dps & 0xFF) };
var ret = SpiTransferArray(outArray);
}
/// <summary>
/// Append the a motor port
/// </summary>
/// <param name="port">Port to append</param>
public void Append (MotorPort port)
{
Append((byte) port);
}
/// <summary>
/// Returns the current state of the specified motor.
/// </summary>
/// <param name="motorPort">The port where the motor is connected.</param>
/// <returns>The current state of the specified motor.</returns>
public MotorState GetOutputState( MotorPort motorPort )
{
byte[] data = CommandHelper.InitializeData(LegoCommandCode.GetOutputState, CommandType.DirectCommandWithResponse, 3);
data[ 2 ] = (byte) motorPort;
TransmitAndWait( data, 25 );
// Parse the response.
return new MotorState
{
Power = (sbyte) LastResponse[ 4 ],
Mode = (MotorModes) LastResponse[ 5 ],
Regulation = (LegoRegulationMode)LastResponse[6],
TurnRatio = (sbyte) LastResponse[ 7 ],
RunState = ( RunState) LastResponse[ 8 ],
TachoLimit = LastResponse.ToUInt32( 9 ),
TachoCount = LastResponse.ToInt32( 13 ),
BlockTachoCount = LastResponse.ToInt32( 17 ),
RotationCount = LastResponse.ToInt32( 21 )
};
}
public OutPort(CSerialPort serial_port, MotorPort motor_port)
{
this.serial_port = serial_port;
this.motor_port = motor_port;
}
/// <summary>
/// Set the motor target position KP constant
/// If you set KP higher, the motor will be more responsive to errors in position, at the cost of perhaps overshooting and oscillating.
/// KD slows down the motor as it approaches the target, and helps to prevent overshoot.
/// In general, if you increase KP, you should also increase KD to keep the motor from overshooting and oscillating.
/// See as well https://en.wikipedia.org/wiki/PID_controller
/// </summary>
/// <param name="port">The Motor port to use, can be MotorLeft and/or MotorRight</param>
/// <param name="kp">The KP constant (default 25)</param>
public void SetMotorPositionKP(MotorPort port, byte kp = 25)
{
byte[] outArray = { SpiAddress, (byte)SpiMessageType.SetMotorPositionKp, (byte)port, kp };
var ret = SpiTransferArray(outArray);
}
/// <summary>
/// Sends a command to the motor connected to the specified port.
/// </summary>
/// <param name="motorPort">The port where the motor is connected to.</param>
/// <param name="power">
/// Power (also referred as speed) set point. Range: -100-100.
/// The absolute value of <paramref name="power"/> is used as a percentage of the full power capabilities of the motor.
/// The sign of <paramref name="power"/> specifies rotation direction.
/// Positive values for <paramref name="power"/> instruct the firmware to turn the motor forward,
/// while negative values instruct the firmware to turn the motor backward.
/// "Forward" and "backward" are relative to a standard orientation for a particular type of motor.
/// Note that direction is not a meaningful concept for outputs like lamps.
/// Lamps are affected only by the absolute value of <paramref name="power"/>.
/// </param>
/// <param name="mode">Motor mode (bit-field).</param>
/// <param name="regulation">Regulation mode.</param>
/// <param name="turnRatio">
/// This property specifies the proportional turning ratio for synchronized turning using two motors. Range: -100-100.
/// <remarks>
/// Negative <paramref name="turnRatio"/> values shift power towards the left motor,
/// whereas positive <paramref name="turnRatio"/> values shift power towards the right motor.
/// In both cases, the actual power applied is proportional to the <paramref name="power"/> set-point,
/// such that an absolute value of 50% for <paramref name="turnRatio"/> normally results in one motor stopping,
/// and an absolute value of 100% for <paramref name="turnRatio"/> normally results in the two motors
/// turning in opposite directions at equal power.
/// </remarks>
/// </param>
/// <param name="runState">Motor run state.</param>
/// <param name="tachoLimit">
/// Tacho limit. This property specifies the rotational distance in
/// degrees that you want to turn the motor. Range: 0-4294967295, O: run forever.
/// The sign of the <paramref name="power"/> property specifies the direction of rotation.
/// </param>
public void SetOutputState(MotorPort motorPort, sbyte power, MotorModes mode, LegoRegulationMode regulation,
sbyte turnRatio, RunState runState, UInt32 tachoLimit )
{
var data = CommandHelper.InitializeData(LegoCommandCode.SetOutputState, CommandType.DirectCommandWithoutResponse, 13);
data[ 2 ] = (byte) motorPort;
data[ 3 ] = (byte) power;
data[ 4 ] = (byte) mode;
data[ 5 ] = (byte) regulation;
data[ 6 ] = (byte) turnRatio;
data[ 7 ] = (byte) runState;
data[ 8 ] = (byte) ( tachoLimit & 0xFF ); // Byte 8-12: tacho limit ULONG
data[ 9 ] = (byte) ( tachoLimit >> 8 );
data[ 10 ] = (byte) ( tachoLimit >> 16 );
data[ 11 ] = (byte) ( tachoLimit >> 24 );
Transmit( data ); // Return package: 0:0x02, 1:Command, 2:StatusByte
}
/// <summary>
/// Set the motor target speed in degrees per second
/// </summary>
/// <param name="port">The Motor port to use, can be MotorLeft and/or MotorRight</param>
/// <param name="dps">The target speed in degrees per second</param>
public void SetMotorDps(MotorPort port, int dps)
{
byte[] outArray = { SpiAddress, (byte)SpiMessageType.SetMotorDps, (byte)port, (byte)((dps >> 8) & 0xFF), (byte)(dps & 0xFF) };
var ret = SpiTransferArray(outArray);
}
/// <summary>
/// Stop the NXT from moving
/// </summary>
public void NXTStop(MotorPort motorPort = MotorPort.MotorAll)
{
if (motorPort == MotorPort.MotorA)
OutPortA.SetOutputState(0, Mode.Break, RegulationMode.Motor_On, (int)0, RunState.Running);
if (motorPort == MotorPort.MotorB)
OutPortB.SetOutputState(0, Mode.Break, RegulationMode.Motor_On, (int)0, RunState.Running);
if (motorPort == MotorPort.MotorC)
OutPortC.SetOutputState(0, Mode.Break, RegulationMode.Motor_On, (int)0, RunState.Running);
if (motorPort == MotorPort.MotorAll)
{
OutPortAll.SetOutputState(0, Mode.Break, RegulationMode.Motor_Idle, (int)0, RunState.Running);
}
}
/// <summary>
/// Initializes a new instance of the <see cref="MonoBrick.NXT.Vehicle"/> class.
/// </summary>
/// <param name='left'>
/// The left motor of the vehicle
/// </param>
/// <param name='right'>
/// The right motor of the vehicle
/// </param>
public Vehicle(MotorPort left, MotorPort right){
this.left.Port = left;
this.right.Port = right;
Sync = false;
}
internal async Task SetOutputStateAsyncInternal(MotorPort motorPort, short power, MotorModes motorMode, MotorRegulationMode motorRegulation,
short turnRatio, MotorRunState runState, uint tachoLimit)
{
await brick.SendCommandAsyncInternal(RequestTelegram.SetOutputState(motorPort, power, motorMode, motorRegulation,
turnRatio, runState, tachoLimit));
}
GetOutputStateAsync(MotorPort motorPort)
{
return GetOutputStateAsyncInternal(motorPort)
#if WINRT
.AsAsyncOperation<GetOutputStateResponse>()
#endif
;
}
/// <summary>
/// Initializes a new instance of the <see cref="MonoBrickFirmware.IO.Motor"/> class.
/// </summary>
/// <param name="port">Port.</param>
public Motor(MotorPort port)
{
this.BitField = MotorPortToBitfield(port);
Reverse = false;
controller = new PositionPID (port, 0, false, 100, standardPValue, standardIValue, standardIValue, controllerSampleTime);
}
