diff --git a/catalyst_test.go b/catalyst_test.go index 05ea6ea..a04c89a 100644 --- a/catalyst_test.go +++ b/catalyst_test.go @@ -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) } diff --git a/driver_station_connection.go b/driver_station_connection.go index eb7c423..472c290 100644 --- a/driver_station_connection.go +++ b/driver_station_connection.go @@ -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: diff --git a/driver_station_connection_test.go b/driver_station_connection_test.go index 9afc1ec..c4fd628 100644 --- a/driver_station_connection_test.go +++ b/driver_station_connection_test.go @@ -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) } diff --git a/main.go b/main.go index 549065a..63135c3 100644 --- a/main.go +++ b/main.go @@ -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()