// 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); }