// Sends a rotate command to the robot and returns right away
// The robot will go forward until it hits a dead end or until
// it's crossed intersectionGoal intersections. This async call
// will allow the thread to filter for intersection detected messages.
// intersectionGoal = 0 for unlimited
private void goStraightAsync(int intersectionGoal)
{
byte[] filterHead = { 0xEE };
StringBuilder buf = new StringBuilder();
buf.Append("PID_LINE ");
buf.Append(intersectionGoal + " ");
buf.Append(straightPowerHigh + " ");
buf.Append(straightPowerLow + " ");
buf.Append(straightLowPowerZone + " ");
buf.Append(straightKP + " ");
buf.Append(straightKI + " ");
buf.Append(straightKD + " ");
buf.Append(dampingFactor + " ");
buf.Append(iSectDetectDelay);
RobotCommand c = new RobotCommand(buf.ToString());
PacketFilter filter = new PacketFilter(link, filterHead, c.Sequence);
filter.init();
link.sendCommand(c);
this.lastPacketFilter = filter;
}
// Helper method to send a command to the default mailbox
public void sendCommand(RobotCommand command)
{
sendCommand(command, this.mailbox);
}
// Sends a bluetooth command and then waits for the ACK
public BluetoothPacket sendCommandAndWait(String command)
{
RobotCommand cmd = new RobotCommand(command);
sendCommand(cmd);
return waitForSequence(cmd.Sequence);
}
// Sends a command to the specified mailbox on the robot
public void sendCommand(RobotCommand command, int mailbox)
{
byte[] seqBytes = BitConverter.GetBytes(command.Sequence);
byte[] packet = new byte[command.Command.Length + 9];
packet[0] = 0x00;
packet[1] = 0x09;
packet[2] = (byte)mailbox;
packet[3] = (byte)(command.Command.Length + 5);
for (int i = 0; i < 4; i++)
{
packet[i + 4] = seqBytes[i];
}
for (int i = 0; i < command.Command.Length; i++)
{
packet[i + 8] = (byte)command.Command[i];
}
packet[command.Command.Length + 8] = 0x00;
btSend(packet);
}