NCGEARS/cheesy-arena-lite
1
0
Fork 0
mirror of https://github.com/Team254/cheesy-arena-lite.git synced 2026-03-09 13:46:44 -04:00
Code Issues Actions Packages Projects Releases 1 Wiki Activity

Implement DS UDP packet listening.

Browse Source
This commit is contained in:
Patrick Fairbank
2016-08-28 20:17:34 -07:00
parent 998cdf782b
commit 38aeb20e21
4 changed files with 57 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
catalyst_test.go
View File

@@ -30,7 +30,10 @@ func TestConfigureCatalyst(t *testing.T) {
catalystTelnetPort += 1
mockTelnet(t, catalystTelnetPort, "interface Vlan15\nip address 10.2.54.61\n", &command)
assert.Nil(t, ConfigureTeamEthernet(nil, &Team{Id: 1114}, nil, nil, &Team{Id: 254}, nil))
assert.Equal(t, "password\nenable\npassword\nterminal length 0\nconfig terminal\nno access-list 112\n"+
"access-list 112 permit ip 10.11.14.0 0.0.0.255 host 10.0.100.5\ninterface Vlan12\nip address "+
"10.11.14.61 255.255.255.0\nend\ncopy running-config startup-config\n\nexit\n", command)
assert.Equal(t, "password\nenable\npassword\nterminal length 0\nconfig terminal\n"+
"ip dhcp excluded-address 10.11.14.1 10.11.14.100\nno ip dhcp pool dhcp12\nip dhcp pool dhcp12\n"+
"network 10.11.14.0 255.255.255.0\ndefault-router 10.11.14.61\nlease 7\nno access-list 112\n"+
"access-list 112 permit ip 10.11.14.0 0.0.0.255 host 10.0.100.5\n"+
"access-list 112 permit udp any eq bootpc any eq bootps\ninterface Vlan12\n"+
"ip address 10.11.14.61 255.255.255.0\nend\ncopy running-config startup-config\n\nexit\n", command)
}

78
driver_station_connection.go
View File

@@ -13,11 +13,12 @@ import (
)
const (
driverStationTcpListenPort = 1750
driverStationUdpSendPort = 1120
driverStationLinkTimeoutSec = 5
robotLinkTimeoutSec = 1
maxTcpPacketBytes = 4096
driverStationTcpListenPort = 1750
driverStationUdpSendPort = 1120
driverStationUdpReceivePort = 1160
driverStationTcpLinkTimeoutSec = 5
driverStationUdpLinkTimeoutSec = 1
maxTcpPacketBytes = 4096
)
type DriverStationConnection struct {
@@ -58,8 +59,45 @@ func NewDriverStationConnection(teamId int, allianceStation string, tcpConn net.
if err != nil {
return nil, err
}
return &DriverStationConnection{TeamId: teamId, AllianceStation: allianceStation, DsLinked: true,
lastPacketTime: time.Now(), tcpConn: tcpConn, udpConn: udpConn}, nil
return &DriverStationConnection{TeamId: teamId, AllianceStation: allianceStation, tcpConn: tcpConn, udpConn: udpConn}, nil
}
// Loops indefinitely to read packets and update connection status.
func ListenForDsUdpPackets() {
udpAddress, _ := net.ResolveUDPAddr("udp4", fmt.Sprintf(":%d", driverStationUdpReceivePort))
listener, err := net.ListenUDP("udp4", udpAddress)
if err != nil {
log.Fatalln("Error opening driver station UDP socket: %v", err.Error())
}
log.Printf("Listening for driver stations on UDP port %d\n", driverStationUdpReceivePort)
var data [50]byte
for {
listener.Read(data[:])
teamId := int(data[4])<<8 + int(data[5])
var dsConn *DriverStationConnection
for _, allianceStation := range mainArena.AllianceStations {
if allianceStation.team.Id == teamId {
dsConn = allianceStation.DsConn
break
}
}
if dsConn != nil {
dsConn.DsLinked = true
dsConn.lastPacketTime = time.Now()
dsConn.RobotLinked = data[3]&0x20 != 0
if dsConn.RobotLinked {
dsConn.lastRobotLinkedTime = time.Now()
// Robot battery voltage, stored as volts * 256.
dsConn.BatteryVoltage = float64(data[6]) + float64(data[7])/256
}
}
}
}
// Sends a control packet to the Driver Station and checks for timeout conditions.
@@ -69,7 +107,7 @@ func (dsConn *DriverStationConnection) Update() error {
return err
}
if time.Since(dsConn.lastPacketTime).Seconds() > driverStationLinkTimeoutSec {
if time.Since(dsConn.lastPacketTime).Seconds() > driverStationUdpLinkTimeoutSec {
dsConn.DsLinked = false
dsConn.RobotLinked = false
dsConn.BatteryVoltage = 0
@@ -205,39 +243,24 @@ func (dsConn *DriverStationConnection) sendControlPacket() error {
// Deserializes a packet from the DS into a structure representing the DS/robot status.
func (dsConn *DriverStationConnection) decodeStatusPacket(data [36]byte) {
if data[6]&0x01 != 0 && data[6]&0x08 == 0 {
// Robot is not connected.
if time.Since(dsConn.lastRobotLinkedTime).Seconds() > robotLinkTimeoutSec {
dsConn.RobotLinked = false
}
return
}
dsConn.RobotLinked = true
// Average DS-robot trip time in milliseconds.
dsConn.DsRobotTripTimeMs = int(data[1]) / 2
// Number of missed packets sent from the DS to the robot.
dsConn.MissedPacketCount = int(data[2]) - dsConn.missedPacketOffset
// Robot battery voltage, stored as volts * 256.
dsConn.BatteryVoltage = float64(data[3]) + float64(data[4])/256
dsConn.lastRobotLinkedTime = time.Now()
}
// Listens for TCP connection requests to Cheesy Arena from driver stations.
func ListenForDriverStations() {
l, err := net.Listen("tcp", fmt.Sprintf("%s:%d", driverStationTcpListenAddress, driverStationTcpListenPort))
if err != nil {
log.Printf("Error opening driver station socket: %v", err.Error())
log.Printf("Error opening driver station TCP socket: %v", err.Error())
log.Printf("Change IP address to %s and restart Cheesy Arena to fix.", driverStationTcpListenAddress)
return
}
defer l.Close()
log.Printf("Listening for driver stations on port %d\n", driverStationTcpListenPort)
log.Printf("Listening for driver stations on TCP port %d\n", driverStationTcpListenPort)
for {
tcpConn, err := l.Accept()
if err != nil {
@@ -299,7 +322,7 @@ func ListenForDriverStations() {
func (dsConn *DriverStationConnection) handleTcpConnection() {
buffer := make([]byte, maxTcpPacketBytes)
for {
dsConn.tcpConn.SetReadDeadline(time.Now().Add(time.Second * driverStationLinkTimeoutSec))
dsConn.tcpConn.SetReadDeadline(time.Now().Add(time.Second * driverStationTcpLinkTimeoutSec))
_, err := dsConn.tcpConn.Read(buffer)
if err != nil {
fmt.Printf("Error reading from connection for Team %d: %v\n", dsConn.TeamId, err.Error())
@@ -308,9 +331,6 @@ func (dsConn *DriverStationConnection) handleTcpConnection() {
break
}
dsConn.DsLinked = true
dsConn.lastPacketTime = time.Now()
packetType := int(buffer[2])
switch packetType {
case 28:

16
driver_station_connection_test.go
View File

@@ -138,21 +138,9 @@ func TestDecodeStatusPacket(t *testing.T) {
assert.Nil(t, err)
defer dsConn.Close()
// Check with no linked robot.
data := [36]byte{22, 28, 103, 19, 192, 0, 247, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
data := [36]byte{22, 28, 103, 19, 192, 0, 246, 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}
dsConn.decodeStatusPacket(data)
assert.Equal(t, false, dsConn.RobotLinked)
assert.Equal(t, 0.0, dsConn.BatteryVoltage)
assert.Equal(t, 0, dsConn.MissedPacketCount)
assert.Equal(t, 0, dsConn.DsRobotTripTimeMs)
// Check with linked robot.
data = [36]byte{22, 28, 103, 19, 192, 0, 246, 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}
dsConn.decodeStatusPacket(data)
assert.Equal(t, true, dsConn.RobotLinked)
assert.Equal(t, 19.75, dsConn.BatteryVoltage)
assert.Equal(t, 103, dsConn.MissedPacketCount)
assert.Equal(t, 14, dsConn.DsRobotTripTimeMs)
}
@@ -220,8 +208,6 @@ func TestListenForDriverStations(t *testing.T) {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
tcpConn.Write(dataSend2[:])
time.Sleep(time.Millisecond * 10)
assert.Equal(t, true, dsConn.RobotLinked)
assert.Equal(t, 19.75, dsConn.BatteryVoltage)
assert.Equal(t, 103, dsConn.MissedPacketCount)
assert.Equal(t, 14, dsConn.DsRobotTripTimeMs)
}

1
main.go
View File

@@ -22,6 +22,7 @@ func main() {
// Run the webserver and DS packet listener in goroutines and use the main one for the arena state machine.
go ServeWebInterface()
go ListenForDriverStations()
go ListenForDsUdpPackets()
go MonitorBandwidth()
mainArena.Setup()
mainArena.Run()