public CommandPdu CreateWriteFirmwareBlockCommand(byte[] encryptedFirmwareBlock)
{
var pdu = new CommandPdu(CommandCode.WriteFirmwareBlock)
{
PayloadData = encryptedFirmwareBlock
};
pdu.Header.PayloadLength = (byte)pdu.PayloadData.Length;
return(pdu);
}
private void SendCommand(CommandPdu command)
{
if (_serial == null)
{
return;
}
if (!_serial.IsOpen)
{
return;
}
var commandBytes = command.ToByteArray();
_serial.Write(commandBytes, 0, commandBytes.Length);
}
public void QueueExecute(CommandPdu cmd)
{
if (cmd == null || string.IsNullOrWhiteSpace(cmd.Command))
{
return;
}
try {
if (this.commands.TryGetValue(cmd.Command.Trim().ToUpper(), out ICommandExecutor executor))
{
executor?.Execute(cmd);
}
} catch (Exception e) {
this.Logger.Error(e.Message);
}
}
public CommandPdu CreateStartScanCommand(byte discoveryMode = 3, ushort scanDuration = 100, ushort scanInterval = 53, ushort scanWindow = 53, byte scanResults = 5, bool discoveryActiveScan = false)
{
var pdu = new CommandPdu(CommandCode.StartScan);
pdu.PayloadData = new byte[9];
pdu.PayloadData[0] = discoveryMode;
pdu.PayloadData[1] = scanResults;
pdu.PayloadData[2] = (byte)(scanDuration & 0xFF);
pdu.PayloadData[3] = (byte)((scanDuration << 8) & 0xFF);
pdu.PayloadData[4] = (byte)(scanInterval & 0xFF);
pdu.PayloadData[5] = (byte)((scanInterval << 8) & 0xFF);
pdu.PayloadData[6] = (byte)(scanWindow & 0xFF);
pdu.PayloadData[7] = (byte)((scanWindow << 8) & 0xFF);
pdu.PayloadData[8] = Convert.ToByte(discoveryActiveScan);
pdu.Header.PayloadLength = 9;
return(pdu);
}
public void Execute(CommandPdu command)
{
if (command == null ||
string.IsNullOrWhiteSpace(command.Command) ||
command.Arguments == null)
{
return;
}
double leftDuty = 0;
double rightDuty = 0;
double effort;
switch (command.Command.Trim().ToLower())
{
case Constants.Commands.Go.FORWARD:
if (command.Arguments.Length != 1)
{
throw new ArgumentException("Expected 1 argument");
}
effort = double.Parse(command.Arguments[0] ?? "0");
leftDuty = Math.Abs(effort);
rightDuty = Math.Abs(effort);
break;
case Constants.Commands.Go.BACKWARD:
if (command.Arguments.Length != 1)
{
throw new ArgumentException("Expected 1 argument");
}
effort = double.Parse(command.Arguments[0] ?? "0");
leftDuty = -Math.Abs(effort);
rightDuty = -Math.Abs(effort);
break;
case Constants.Commands.Go.LEFT:
if (command.Arguments.Length != 1)
{
throw new ArgumentException("Expected 1 argument");
}
effort = double.Parse(command.Arguments[0] ?? "0");
leftDuty = -Math.Abs(effort);
rightDuty = Math.Abs(effort);
break;
case Constants.Commands.Go.RIGHT:
if (command.Arguments.Length != 1)
{
throw new ArgumentException("Expected 1 argument");
}
effort = double.Parse(command.Arguments[0] ?? "0");
leftDuty = Math.Abs(effort);
rightDuty = -Math.Abs(effort);
break;
case Constants.Commands.Go.TANK:
if (command.Arguments.Length != 2)
{
throw new ArgumentException("Expected 2 arguments");
}
double forwardSpeed = double.Parse(command.Arguments[0] ?? "0");
double antiClockwiseSpin = double.Parse(command.Arguments[1] ?? "0");
leftDuty = forwardSpeed - antiClockwiseSpin;
rightDuty = forwardSpeed + antiClockwiseSpin;
break;
case Constants.Commands.Go.STOP:
default:
leftDuty = 0;
rightDuty = 0;
break;
}
this.Motors.SetDutyCycle(this.leftMotorId, leftDuty);
this.Motors.SetDutyCycle(this.rightMotorId, rightDuty);
}
