mirror of
https://github.com/Team254/cheesy-arena-lite.git
synced 2026-03-09 13:46:44 -04:00
Added communication with the driver station.
This commit is contained in:
Patrick Fairbank
12
arena.go
Normal file
12
arena.go
Normal file
@@ -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
|
||||
}
|
||||
182
driver_station_connection.go
Normal file
182
driver_station_connection.go
Normal file
@@ -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
|
||||
}
|
||||
233
driver_station_connection_test.go
Normal file
233
driver_station_connection_test.go
Normal file
@@ -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)
|
||||
}
|
||||
Reference in New Issue
Block a user