From 7f58f366c27b37314de4807e8b075deb3b4c1225 Mon Sep 17 00:00:00 2001 From: Patrick Fairbank Date: Wed, 2 Jul 2014 00:11:57 -0700 Subject: [PATCH] Added communication with the driver station. --- arena.go | 12 ++ driver_station_connection.go | 182 +++++++++++++++++++++++ driver_station_connection_test.go | 233 ++++++++++++++++++++++++++++++ 3 files changed, 427 insertions(+) create mode 100644 arena.go create mode 100644 driver_station_connection.go create mode 100644 driver_station_connection_test.go diff --git a/arena.go b/arena.go new file mode 100644 index 0000000..c7bc8bb --- /dev/null +++ b/arena.go @@ -0,0 +1,12 @@ +// Copyright 2014 Team 254. All Rights Reserved. +// Author: pat@patfairbank.com (Patrick Fairbank) +// +// Functions for controlling the arena and match play. + +package main + +import () + +var arena struct { + DriverStationConnections map[string]*DriverStationConnection +} diff --git a/driver_station_connection.go b/driver_station_connection.go new file mode 100644 index 0000000..c5bd09e --- /dev/null +++ b/driver_station_connection.go @@ -0,0 +1,182 @@ +// Copyright 2014 Team 254. All Rights Reserved. +// Author: pat@patfairbank.com (Patrick Fairbank) +// +// Model and methods for interacting with a team's Driver Station. + +package main + +import ( + "fmt" + "hash/crc32" + "net" + "strconv" + "time" +) + +// UDP port numbers that the Driver Station sends and receives on. +const driverStationSendPort = 1120 +const driverStationReceivePort = 1160 +const driverStationProtocolVersion = "11191100" + +type DriverStationStatus struct { + TeamId int + AllianceStation string + RobotLinked bool + Auto bool + Enabled bool + EmergencyStop bool + BatteryVoltage float64 + DsVersion string + PacketCount int + MissedPacketCount int + DsRobotTripTimeMs int +} + +type DriverStationConnection struct { + TeamId int + AllianceStation string + Auto bool + Enabled bool + EmergencyStop bool + DriverStationStatus *DriverStationStatus + LastPacketTime time.Time + LastRobotLinkedTime time.Time + conn net.Conn + packetCount int +} + +// Opens a UDP connection for communicating to the driver station. +func NewDriverStationConnection(teamId int, station string) (*DriverStationConnection, error) { + conn, err := net.Dial("udp4", fmt.Sprintf("10.%d.%d.5:%d", teamId/100, teamId%100, driverStationSendPort)) + if err != nil { + return nil, err + } + return &DriverStationConnection{TeamId: teamId, AllianceStation: station, conn: conn}, nil +} + +// Builds and sends the next control packet to the Driver Station. +func (dsConn *DriverStationConnection) SendControlPacket() error { + packet := dsConn.encodeControlPacket() + _, err := dsConn.conn.Write(packet[:]) + if err != nil { + return err + } + + return nil +} + +func (dsConn *DriverStationConnection) Close() error { + return dsConn.conn.Close() +} + +// Sets up a watch on the UDP port that Driver Stations send on. +func DsPacketListener() (*net.UDPConn, error) { + udpAddress, err := net.ResolveUDPAddr("udp4", fmt.Sprintf(":%d", driverStationReceivePort)) + if err != nil { + return nil, err + } + listen, err := net.ListenUDP("udp4", udpAddress) + if err != nil { + return nil, err + } + return listen, nil +} + +// Loops indefinitely to read packets and update connection status. +func ListenForDsPackets(listener *net.UDPConn) { + var data [50]byte + for { + listener.Read(data[:]) + dsStatus := decodeStatusPacket(data) + + // Update the status and last packet times for this alliance/team in the global struct. + dsConn := arena.DriverStationConnections[dsStatus.AllianceStation] + if dsConn != nil && dsConn.TeamId == dsStatus.TeamId { + dsConn.DriverStationStatus = dsStatus + dsConn.LastPacketTime = time.Now() + if dsStatus.RobotLinked { + dsConn.LastRobotLinkedTime = time.Now() + } + } + } +} + +// Serializes the control information into a packet. +func (dsConn *DriverStationConnection) encodeControlPacket() [74]byte { + var packet [74]byte + + // Packet number, stored big-endian in two bytes. + packet[0] = byte((dsConn.packetCount >> 8) & 0xff) + packet[1] = byte(dsConn.packetCount & 0xff) + + // Robot status byte. 0x01=competition mode, 0x02=link, 0x04=check version, 0x08=request DS ID, + // 0x10=autonomous, 0x20=enable, 0x40=e-stop not on + packet[2] = 0x03 + if dsConn.Auto { + packet[2] |= 0x10 + } + if dsConn.Enabled { + packet[2] |= 0x20 + } + if !dsConn.EmergencyStop { + packet[2] |= 0x40 + } + + // Alliance station, stored as ASCII characters 'R/B' and '1/2/3'. + packet[3] = dsConn.AllianceStation[0] + packet[4] = dsConn.AllianceStation[1] + + // Static protocol version repeated twice. + for i := 0; i < 8; i++ { + packet[10+i] = driverStationProtocolVersion[i] + } + for i := 0; i < 8; i++ { + packet[18+i] = driverStationProtocolVersion[i] + } + + // Calculate and store the 4-byte CRC32 checksum. + checksum := crc32.ChecksumIEEE(packet[:]) + packet[70] = byte((checksum >> 24) & 0xff) + packet[71] = byte((checksum >> 16) & 0xff) + packet[72] = byte((checksum >> 8) & 0xff) + packet[73] = byte((checksum) & 0xff) + + // Increment the packet count for next time. + dsConn.packetCount++ + + return packet +} + +// Deserializes a packet from the DS into a structure representing the DS/robot status. +func decodeStatusPacket(data [50]byte) *DriverStationStatus { + dsStatus := new(DriverStationStatus) + + // Robot status byte. + dsStatus.RobotLinked = (data[2] & 0x02) != 0 + dsStatus.Auto = (data[2] & 0x10) != 0 + dsStatus.Enabled = (data[2] & 0x20) != 0 + dsStatus.EmergencyStop = (data[2] & 0x40) == 0 + + // Team number, stored in two bytes as hundreds and then ones (like the IP address). + dsStatus.TeamId = int(data[4])*100 + int(data[5]) + + // Alliance station, stored as ASCII characters 'R/B' and '1/2/3'. + dsStatus.AllianceStation = string(data[10:12]) + + // Driver Station software version, stored as 8-byte string. + dsStatus.DsVersion = string(data[18:26]) + + // Number of missed packets sent from the DS to the robot, stored in two big-endian bytes. + dsStatus.MissedPacketCount = int(data[26])*256 + int(data[27]) + + // Total number of packets sent from the DS to the robot, stored in two big-endian bytes. + dsStatus.PacketCount = int(data[28])*256 + int(data[29]) + + // Average DS-robot trip time in milliseconds, stored in two big-endian bytes. + dsStatus.DsRobotTripTimeMs = int(data[29])*256 + int(data[30]) + + // Robot battery voltage, stored (bizarrely) what it looks like in decimal but as two hexadecimal numbers. + dsStatus.BatteryVoltage, _ = strconv.ParseFloat(fmt.Sprintf("%x.%x", data[40], data[41]), 32) + + return dsStatus +} diff --git a/driver_station_connection_test.go b/driver_station_connection_test.go new file mode 100644 index 0000000..dc0a94d --- /dev/null +++ b/driver_station_connection_test.go @@ -0,0 +1,233 @@ +// Copyright 2014 Team 254. All Rights Reserved. +// Author: pat@patfairbank.com (Patrick Fairbank) + +package main + +import ( + "fmt" + "github.com/stretchr/testify/assert" + "net" + "testing" + "time" +) + +func TestEncodeControlPacket(t *testing.T) { + dsConn, err := NewDriverStationConnection(254, "R1") + assert.Nil(t, err) + defer dsConn.Close() + + data := dsConn.encodeControlPacket() + assert.Equal(t, [74]byte{0, 0, 67, 82, 49, 0, 0, 0, 0, 0, 49, 49, 49, 57, 49, 49, 48, 48, 49, 49, 49, 57, + 49, 49, 48, 48, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 110, 235, 5, 29}, data) + + // Check the different alliance station values as well as the checksums. + dsConn.AllianceStation = "R2" + data = dsConn.encodeControlPacket() + assert.Equal(t, [74]byte{0, 1, 67, 82, 50, 0, 0, 0, 0, 0, 49, 49, 49, 57, 49, 49, 48, 48, 49, 49, 49, 57, + 49, 49, 48, 48, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 114, 141, 17, 174}, data) + dsConn.AllianceStation = "R3" + data = dsConn.encodeControlPacket() + assert.Equal(t, [74]byte{0, 2, 67, 82, 51, 0, 0, 0, 0, 0, 49, 49, 49, 57, 49, 49, 48, 48, 49, 49, 49, 57, + 49, 49, 48, 48, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 232, 206, 203, 150}, data) + dsConn.AllianceStation = "B1" + data = dsConn.encodeControlPacket() + assert.Equal(t, [74]byte{0, 3, 67, 66, 49, 0, 0, 0, 0, 0, 49, 49, 49, 57, 49, 49, 48, 48, 49, 49, 49, 57, + 49, 49, 48, 48, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 99, 57, 55, 68}, data) + dsConn.AllianceStation = "B2" + data = dsConn.encodeControlPacket() + assert.Equal(t, [74]byte{0, 4, 67, 66, 50, 0, 0, 0, 0, 0, 49, 49, 49, 57, 49, 49, 48, 48, 49, 49, 49, 57, + 49, 49, 48, 48, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 34, 101, 225, 16}, data) + dsConn.AllianceStation = "B3" + data = dsConn.encodeControlPacket() + assert.Equal(t, [74]byte{0, 5, 67, 66, 51, 0, 0, 0, 0, 0, 49, 49, 49, 57, 49, 49, 48, 48, 49, 49, 49, 57, + 49, 49, 48, 48, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 140, 207, 133, 117}, data) + + // Check packet count rollover. + dsConn.packetCount = 255 + data = dsConn.encodeControlPacket() + assert.Equal(t, byte(0), data[0]) + assert.Equal(t, byte(255), data[1]) + data = dsConn.encodeControlPacket() + assert.Equal(t, byte(1), data[0]) + assert.Equal(t, byte(0), data[1]) + data = dsConn.encodeControlPacket() + assert.Equal(t, byte(1), data[0]) + assert.Equal(t, byte(1), data[1]) + dsConn.packetCount = 65535 + data = dsConn.encodeControlPacket() + assert.Equal(t, byte(255), data[0]) + assert.Equal(t, byte(255), data[1]) + data = dsConn.encodeControlPacket() + assert.Equal(t, byte(0), data[0]) + assert.Equal(t, byte(0), data[1]) + + // Check different robot statuses. + dsConn.Auto = true + data = dsConn.encodeControlPacket() + assert.Equal(t, byte(83), data[2]) + + dsConn.Enabled = true + data = dsConn.encodeControlPacket() + assert.Equal(t, byte(115), data[2]) + + dsConn.Auto = false + data = dsConn.encodeControlPacket() + assert.Equal(t, byte(99), data[2]) + + dsConn.EmergencyStop = true + data = dsConn.encodeControlPacket() + assert.Equal(t, byte(35), data[2]) +} + +func TestSendControlPacket(t *testing.T) { + dsConn, err := NewDriverStationConnection(254, "R1") + assert.Nil(t, err) + defer dsConn.Close() + + // No real way of checking this since the destination IP is remote, so settle for there being no errors. + err = dsConn.SendControlPacket() + assert.Nil(t, err) +} + +func TestDecodeStatusPacket(t *testing.T) { + // Check with no linked robot. + data := [50]byte{0, 0, 64, 1, 2, 54, 0, 0, 0, 0, 82, 49, 0, 0, 0, 0, 0, 0, 48, 50, 49, 50, 49, 51, 48, 48, + 152, 160, 152, 160, 255, 255, 255, 255, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 7, 189, 111} + dsStatus := decodeStatusPacket(data) + assert.Equal(t, 254, dsStatus.TeamId) + assert.Equal(t, "R1", dsStatus.AllianceStation) + assert.Equal(t, false, dsStatus.RobotLinked) + assert.Equal(t, false, dsStatus.Auto) + assert.Equal(t, false, dsStatus.Enabled) + assert.Equal(t, false, dsStatus.EmergencyStop) + assert.Equal(t, 0, dsStatus.BatteryVoltage) + assert.Equal(t, "02121300", dsStatus.DsVersion) + assert.Equal(t, 39072, dsStatus.PacketCount) + assert.Equal(t, 39072, dsStatus.MissedPacketCount) + assert.Equal(t, 41215, dsStatus.DsRobotTripTimeMs) + + // Check different team numbers. + data = [50]byte{0, 0, 64, 1, 7, 66, 0, 0, 0, 0, 82, 49, 0, 0, 0, 0, 0, 0, 48, 50, 49, 50, 49, 51, 48, 48, + 152, 160, 152, 160, 255, 255, 255, 255, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 7, 189, 111} + dsStatus = decodeStatusPacket(data) + assert.Equal(t, 766, dsStatus.TeamId) + data = [50]byte{0, 0, 64, 1, 51, 36, 0, 0, 0, 0, 82, 49, 0, 0, 0, 0, 0, 0, 48, 50, 49, 50, 49, 51, 48, 48, + 152, 160, 152, 160, 255, 255, 255, 255, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 7, 189, 111} + dsStatus = decodeStatusPacket(data) + assert.Equal(t, 5136, dsStatus.TeamId) + + // Check different alliance stations. + data = [50]byte{0, 0, 64, 1, 51, 36, 0, 0, 0, 0, 66, 51, 0, 0, 0, 0, 0, 0, 48, 50, 49, 50, 49, 51, 48, 48, + 152, 160, 152, 160, 255, 255, 255, 255, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 7, 189, 111} + dsStatus = decodeStatusPacket(data) + assert.Equal(t, "B3", dsStatus.AllianceStation) + + // Check different robot statuses. + data = [50]byte{0, 0, 66, 1, 7, 66, 0, 0, 0, 0, 82, 49, 0, 0, 0, 0, 0, 0, 48, 50, 49, 50, 49, 51, 48, 48, + 152, 160, 152, 160, 255, 255, 255, 255, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 7, 189, 111} + dsStatus = decodeStatusPacket(data) + assert.Equal(t, true, dsStatus.RobotLinked) + data = [50]byte{0, 0, 98, 1, 7, 66, 0, 0, 0, 0, 82, 49, 0, 0, 0, 0, 0, 0, 48, 50, 49, 50, 49, 51, 48, 48, + 152, 160, 152, 160, 255, 255, 255, 255, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 7, 189, 111} + dsStatus = decodeStatusPacket(data) + assert.Equal(t, true, dsStatus.Enabled) + data = [50]byte{0, 0, 114, 1, 7, 66, 0, 0, 0, 0, 82, 49, 0, 0, 0, 0, 0, 0, 48, 50, 49, 50, 49, 51, 48, 48, + 152, 160, 152, 160, 255, 255, 255, 255, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 7, 189, 111} + dsStatus = decodeStatusPacket(data) + assert.Equal(t, true, dsStatus.Auto) + data = [50]byte{0, 0, 50, 1, 7, 66, 0, 0, 0, 0, 82, 49, 0, 0, 0, 0, 0, 0, 48, 50, 49, 50, 49, 51, 48, 48, + 152, 160, 152, 160, 255, 255, 255, 255, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 7, 189, 111} + dsStatus = decodeStatusPacket(data) + assert.Equal(t, true, dsStatus.EmergencyStop) + + // Check different battery voltages. + data = [50]byte{0, 0, 64, 1, 7, 66, 0, 0, 0, 0, 82, 49, 0, 0, 0, 0, 0, 0, 48, 50, 49, 50, 49, 51, 48, 48, + 152, 160, 152, 160, 255, 255, 255, 255, 82, 0, 0, 0, 0, 0, 25, 117, 0, 0, 0, 0, 42, 7, 189, 111} + dsStatus = decodeStatusPacket(data) + assert.Equal(t, 19.75, dsStatus.BatteryVoltage) + + // TODO(patrick): Check packet counts and trip time. +} + +func TestListenForDsPackets(t *testing.T) { + listener, err := DsPacketListener() + assert.Nil(t, err) + go ListenForDsPackets(listener) + + arena.DriverStationConnections = make(map[string]*DriverStationConnection) + dsConn, err := NewDriverStationConnection(254, "B1") + defer dsConn.Close() + assert.Nil(t, err) + arena.DriverStationConnections["B1"] = dsConn + dsConn, err = NewDriverStationConnection(1114, "R3") + defer dsConn.Close() + assert.Nil(t, err) + arena.DriverStationConnections["R3"] = dsConn + + // Create a socket to send fake DS packets to localhost. + conn, err := net.Dial("udp4", fmt.Sprintf("127.0.0.1:%d", driverStationReceivePort)) + assert.Nil(t, err) + + // Check receiving a packet from an expected team. + packet := [50]byte{0, 0, 48, 1, 2, 54, 0, 0, 0, 0, 66, 49, 0, 0, 0, 0, 0, 0, 48, 50, 49, 50, 49, 51, 48, 48, + 152, 160, 152, 160, 255, 255, 255, 255, 82, 0, 0, 0, 0, 0, 25, 117, 0, 0, 0, 0, 42, 7, 189, 111} + _, err = conn.Write(packet[:]) + assert.Nil(t, err) + time.Sleep(time.Millisecond * 10) // Allow some time for the goroutine to process the incoming packet. + dsStatus := arena.DriverStationConnections["B1"].DriverStationStatus + if assert.NotNil(t, dsStatus) { + assert.Equal(t, 254, dsStatus.TeamId) + assert.Equal(t, "B1", dsStatus.AllianceStation) + assert.Equal(t, false, dsStatus.RobotLinked) + assert.Equal(t, true, dsStatus.Auto) + assert.Equal(t, true, dsStatus.Enabled) + assert.Equal(t, true, dsStatus.EmergencyStop) + assert.Equal(t, 19.75, dsStatus.BatteryVoltage) + assert.Equal(t, "02121300", dsStatus.DsVersion) + assert.Equal(t, 39072, dsStatus.PacketCount) + assert.Equal(t, 39072, dsStatus.MissedPacketCount) + assert.Equal(t, 41215, dsStatus.DsRobotTripTimeMs) + } + assert.True(t, time.Since(arena.DriverStationConnections["B1"].LastPacketTime).Seconds() < 0.1) + assert.True(t, time.Since(arena.DriverStationConnections["B1"].LastRobotLinkedTime).Seconds() > 100) + packet[2] = byte(98) + _, err = conn.Write(packet[:]) + assert.Nil(t, err) + time.Sleep(time.Millisecond * 10) + dsStatus2 := arena.DriverStationConnections["B1"].DriverStationStatus + if assert.NotNil(t, dsStatus2) { + assert.Equal(t, true, dsStatus2.RobotLinked) + assert.Equal(t, false, dsStatus2.Auto) + assert.Equal(t, true, dsStatus2.Enabled) + assert.Equal(t, false, dsStatus2.EmergencyStop) + } + assert.True(t, time.Since(arena.DriverStationConnections["B1"].LastPacketTime).Seconds() < 0.1) + assert.True(t, time.Since(arena.DriverStationConnections["B1"].LastRobotLinkedTime).Seconds() < 0.1) + + // Should ignore a packet coming from an expected team in the wrong position. + packet[10] = 'R' + packet[11] = '3' + packet[2] = 48 + _, err = conn.Write(packet[:]) + assert.Nil(t, err) + time.Sleep(time.Millisecond * 10) + assert.Nil(t, arena.DriverStationConnections["R3"].DriverStationStatus) + assert.Equal(t, true, arena.DriverStationConnections["B1"].DriverStationStatus.RobotLinked) + + // Should ignore a packet coming from an unexpected team. + packet[4] = byte(15) + packet[5] = byte(3) + packet[10] = 'B' + packet[11] = '1' + packet[2] = 48 + _, err = conn.Write(packet[:]) + assert.Nil(t, err) + time.Sleep(time.Millisecond * 10) + assert.Equal(t, true, arena.DriverStationConnections["B1"].DriverStationStatus.RobotLinked) +}