Add PLC integration for field sensors, motors, and lights.

This commit is contained in:
Patrick Fairbank
2017-09-03 20:51:20 -07:00
parent fa70323b85
commit d5ec68b77e
23 changed files with 713 additions and 59 deletions

View File

@@ -18,6 +18,7 @@ CREATE TABLE event_settings (
switchaddress VARCHAR(255),
switchpassword VARCHAR(255),
bandwidthmonitoringenabled bool,
plcaddress VARCHAR(255),
tbadownloadenabled bool,
adminpassword VARCHAR(255),
readerpassword VARCHAR(255),

View File

@@ -36,6 +36,7 @@ type Arena struct {
EventSettings *model.EventSettings
accessPoint *AccessPoint
networkSwitch *NetworkSwitch
Plc Plc
TbaClient *partner.TbaClient
StemTvClient *partner.StemTvClient
AllianceStations map[string]*AllianceStation
@@ -54,6 +55,8 @@ type Arena struct {
AllianceStationDisplays map[string]string
AllianceStationDisplayScreen string
MuteMatchSounds bool
FieldTestMode string
matchAborted bool
matchStateNotifier *Notifier
MatchTimeNotifier *Notifier
RobotStatusNotifier *Notifier
@@ -73,13 +76,15 @@ type ArenaStatus struct {
AllianceStations map[string]*AllianceStation
MatchState int
CanStartMatch bool
PlcIsHealthy bool
FieldEstop bool
}
type AllianceStation struct {
DsConn *DriverStationConnection
EmergencyStop bool
Bypass bool
Team *model.Team
DsConn *DriverStationConnection
Estop bool
Bypass bool
Team *model.Team
}
// Creates the arena and sets it to its initial state.
@@ -145,6 +150,7 @@ func (arena *Arena) LoadSettings() error {
// Initialize the components that depend on settings.
arena.accessPoint = NewAccessPoint(settings.ApAddress, settings.ApUsername, settings.ApPassword)
arena.networkSwitch = NewNetworkSwitch(settings.SwitchAddress, settings.SwitchPassword)
arena.Plc.SetAddress(settings.PlcAddress)
arena.TbaClient = partner.NewTbaClient(settings.TbaEventCode, settings.TbaSecretId, settings.TbaSecret)
arena.StemTvClient = partner.NewStemTvClient(settings.StemTvEventCode)
@@ -285,6 +291,7 @@ func (arena *Arena) AbortMatch() error {
return fmt.Errorf("Cannot abort match when it is not in progress.")
}
arena.MatchState = PostMatch
arena.matchAborted = true
arena.AudienceDisplayScreen = "blank"
arena.AudienceDisplayNotifier.Notify(nil)
if !arena.MuteMatchSounds {
@@ -299,6 +306,7 @@ func (arena *Arena) ResetMatch() error {
return fmt.Errorf("Cannot reset match while it is in progress.")
}
arena.MatchState = PreMatch
arena.matchAborted = false
arena.AllianceStations["R1"].Bypass = false
arena.AllianceStations["R2"].Bypass = false
arena.AllianceStations["R3"].Bypass = false
@@ -342,6 +350,8 @@ func (arena *Arena) Update() {
if !arena.MuteMatchSounds {
arena.PlaySoundNotifier.Notify("match-start")
}
arena.FieldTestMode = ""
arena.Plc.ResetCounts()
case AutoPeriod:
auto = true
enabled = true
@@ -417,6 +427,10 @@ func (arena *Arena) Update() {
arena.sendDsPacket(auto, enabled)
arena.RobotStatusNotifier.Notify(nil)
}
// Handle field sensors/lights/motors.
arena.handlePlcInput()
arena.handlePlcOutput()
}
// Loops indefinitely to track and update the arena components.
@@ -425,6 +439,7 @@ func (arena *Arena) Run() {
go arena.listenForDriverStations()
go arena.listenForDsUdpPackets()
go arena.monitorBandwidth()
go arena.Plc.Run()
for {
arena.Update()
@@ -445,7 +460,8 @@ func (arena *Arena) BlueScoreSummary() *game.ScoreSummary {
}
func (arena *Arena) GetStatus() *ArenaStatus {
return &ArenaStatus{arena.AllianceStations, arena.MatchState, arena.checkCanStartMatch() == nil}
return &ArenaStatus{arena.AllianceStations, arena.MatchState, arena.checkCanStartMatch() == nil,
arena.Plc.IsHealthy, arena.Plc.GetFieldEstop()}
}
// Loads a team into an alliance station, cleaning up the previous team there if there is one.
@@ -513,7 +529,7 @@ func (arena *Arena) checkCanStartMatch() error {
return fmt.Errorf("Cannot start match while there is a match still in progress or with results pending.")
}
for _, allianceStation := range arena.AllianceStations {
if allianceStation.EmergencyStop {
if allianceStation.Estop {
return fmt.Errorf("Cannot start match while an emergency stop is active.")
}
if !allianceStation.Bypass {
@@ -522,6 +538,16 @@ func (arena *Arena) checkCanStartMatch() error {
}
}
}
if arena.EventSettings.PlcAddress != "" {
if !arena.Plc.IsHealthy {
return fmt.Errorf("Cannot start match while PLC is not healthy.")
}
if arena.Plc.GetFieldEstop() {
return fmt.Errorf("Cannot start match while field emergency stop is active.")
}
}
return nil
}
@@ -530,7 +556,8 @@ func (arena *Arena) sendDsPacket(auto bool, enabled bool) {
dsConn := allianceStation.DsConn
if dsConn != nil {
dsConn.Auto = auto
dsConn.Enabled = enabled && !allianceStation.EmergencyStop && !allianceStation.Bypass
dsConn.Enabled = enabled && !allianceStation.Estop && !allianceStation.Bypass
dsConn.Estop = allianceStation.Estop
err := dsConn.update(arena)
if err != nil {
log.Printf("Unable to send driver station packet for team %d.", allianceStation.Team.Id)
@@ -551,3 +578,137 @@ func (arena *Arena) getAssignedAllianceStation(teamId int) string {
return ""
}
// Updates the score given new input information from the field PLC.
func (arena *Arena) handlePlcInput() {
// Handle emergency stops.
if arena.Plc.GetFieldEstop() && arena.MatchTimeSec() > 0 && !arena.matchAborted {
arena.AbortMatch()
}
redEstops, blueEstops := arena.Plc.GetTeamEstops()
arena.handleEstop("R1", redEstops[0])
arena.handleEstop("R2", redEstops[1])
arena.handleEstop("R3", redEstops[2])
arena.handleEstop("B1", blueEstops[0])
arena.handleEstop("B2", blueEstops[1])
arena.handleEstop("B3", blueEstops[2])
matchStartTime := arena.MatchStartTime
currentTime := time.Now()
if arena.MatchState == PreMatch {
// Set a match start time in the future.
matchStartTime = currentTime.Add(time.Second)
}
matchEndTime := game.GetMatchEndTime(matchStartTime)
inGracePeriod := currentTime.Before(matchEndTime.Add(game.BoilerTeleopGracePeriodSec * time.Second))
if arena.MatchState == PostMatch && (!inGracePeriod || arena.matchAborted) {
// Don't do anything if we're past the end of the match, otherwise we may overwrite manual edits.
return
}
redScore := &arena.RedRealtimeScore.CurrentScore
oldRedScore := *redScore
blueScore := &arena.BlueRealtimeScore.CurrentScore
oldBlueScore := *blueScore
// Handle balls.
redLow, redHigh, blueLow, blueHigh := arena.Plc.GetBalls()
arena.RedRealtimeScore.boiler.UpdateState(redLow, redHigh, matchStartTime, currentTime)
redScore.AutoFuelLow = arena.RedRealtimeScore.boiler.AutoFuelLow
redScore.AutoFuelHigh = arena.RedRealtimeScore.boiler.AutoFuelHigh
redScore.FuelLow = arena.RedRealtimeScore.boiler.FuelLow
redScore.FuelHigh = arena.RedRealtimeScore.boiler.FuelHigh
arena.BlueRealtimeScore.boiler.UpdateState(blueLow, blueHigh, matchStartTime, currentTime)
blueScore.AutoFuelLow = arena.BlueRealtimeScore.boiler.AutoFuelLow
blueScore.AutoFuelHigh = arena.BlueRealtimeScore.boiler.AutoFuelHigh
blueScore.FuelLow = arena.BlueRealtimeScore.boiler.FuelLow
blueScore.FuelHigh = arena.BlueRealtimeScore.boiler.FuelHigh
// Handle rotors.
redRotors, blueRotors := arena.Plc.GetRotors()
arena.RedRealtimeScore.rotorSet.UpdateState(redRotors, matchStartTime, currentTime)
redScore.AutoRotors = arena.RedRealtimeScore.rotorSet.AutoRotors
redScore.Rotors = arena.RedRealtimeScore.rotorSet.Rotors
arena.BlueRealtimeScore.rotorSet.UpdateState(blueRotors, matchStartTime, currentTime)
blueScore.AutoRotors = arena.BlueRealtimeScore.rotorSet.AutoRotors
blueScore.Rotors = arena.BlueRealtimeScore.rotorSet.Rotors
// Handle touchpads.
redTouchpads, blueTouchpads := arena.Plc.GetTouchpads()
for i := 0; i < 3; i++ {
arena.RedRealtimeScore.touchpads[i].UpdateState(redTouchpads[i], currentTime)
arena.BlueRealtimeScore.touchpads[i].UpdateState(blueTouchpads[i], currentTime)
}
redScore.Takeoffs = game.CountTouchpads(&arena.RedRealtimeScore.touchpads, matchStartTime, currentTime)
blueScore.Takeoffs = game.CountTouchpads(&arena.BlueRealtimeScore.touchpads, matchStartTime, currentTime)
if !oldRedScore.Equals(redScore) || !oldBlueScore.Equals(blueScore) {
arena.RealtimeScoreNotifier.Notify(nil)
}
}
// Writes light/motor commands to the field PLC.
func (arena *Arena) handlePlcOutput() {
if arena.FieldTestMode != "" {
// PLC output is being manually overridden.
if arena.FieldTestMode == "flash" {
arena.Plc.SetTouchpadLights([3]bool{arena.Plc.BlinkState, arena.Plc.BlinkState, arena.Plc.BlinkState},
[3]bool{arena.Plc.BlinkState, arena.Plc.BlinkState, arena.Plc.BlinkState})
}
return
}
// Handle balls.
matchEndTime := game.GetMatchEndTime(arena.MatchStartTime)
inGracePeriod := time.Now().Before(matchEndTime.Add(game.BoilerTeleopGracePeriodSec * time.Second))
if arena.MatchTimeSec() > 0 || arena.MatchState == PostMatch && !arena.matchAborted && inGracePeriod {
arena.Plc.SetBoilerMotors(true)
} else {
arena.Plc.SetBoilerMotors(false)
}
// Handle rotors.
redScore := &arena.RedRealtimeScore.CurrentScore
blueScore := &arena.BlueRealtimeScore.CurrentScore
if arena.MatchTimeSec() > 0 {
arena.Plc.SetRotorMotors(redScore.AutoRotors+redScore.Rotors, blueScore.AutoRotors+blueScore.Rotors)
} else {
arena.Plc.SetRotorMotors(0, 0)
}
arena.Plc.SetRotorLights(redScore.AutoRotors, blueScore.AutoRotors)
// Handle touchpads.
var redTouchpads, blueTouchpads [3]bool
currentTime := time.Now()
matchStartTime := arena.MatchStartTime
blinkStopTime := matchEndTime.Add(-time.Duration(game.MatchTiming.EndgameTimeLeftSec-2) * time.Second)
if arena.MatchState == EndgamePeriod && currentTime.Before(blinkStopTime) {
// Blink the touchpads at the endgame start point.
for i := 0; i < 3; i++ {
redTouchpads[i] = arena.Plc.BlinkState
blueTouchpads[i] = arena.Plc.BlinkState
}
} else {
if arena.MatchState == PreMatch {
// Allow touchpads to be triggered before a match.
matchStartTime = currentTime
}
for i := 0; i < 3; i++ {
redState := arena.RedRealtimeScore.touchpads[i].GetState(matchStartTime, currentTime)
redTouchpads[i] = redState == 2 || redState == 1 && arena.Plc.BlinkState
blueState := arena.BlueRealtimeScore.touchpads[i].GetState(matchStartTime, currentTime)
blueTouchpads[i] = blueState == 2 || blueState == 1 && arena.Plc.BlinkState
}
}
arena.Plc.SetTouchpadLights(redTouchpads, blueTouchpads)
}
func (arena *Arena) handleEstop(station string, state bool) {
allianceStation := arena.AllianceStations[station]
if state {
allianceStation.Estop = true
} else if arena.MatchTimeSec() == 0 {
// Don't reset the e-stop while a match is in progress.
allianceStation.Estop = false
}
}

View File

@@ -115,7 +115,7 @@ func TestArenaMatchFlow(t *testing.T) {
assert.Equal(t, true, arena.AllianceStations["B3"].DsConn.Enabled)
// Check e-stop and bypass.
arena.AllianceStations["B3"].EmergencyStop = true
arena.AllianceStations["B3"].Estop = true
arena.lastDsPacketTime = arena.lastDsPacketTime.Add(-300 * time.Millisecond)
arena.Update()
assert.Equal(t, TeleopPeriod, arena.MatchState)
@@ -127,7 +127,7 @@ func TestArenaMatchFlow(t *testing.T) {
assert.Equal(t, TeleopPeriod, arena.MatchState)
assert.Equal(t, false, arena.AllianceStations["B3"].DsConn.Auto)
assert.Equal(t, false, arena.AllianceStations["B3"].DsConn.Enabled)
arena.AllianceStations["B3"].EmergencyStop = false
arena.AllianceStations["B3"].Estop = false
arena.lastDsPacketTime = arena.lastDsPacketTime.Add(-300 * time.Millisecond)
arena.Update()
assert.Equal(t, TeleopPeriod, arena.MatchState)
@@ -308,12 +308,12 @@ func TestMatchStartRobotLinkEnforcement(t *testing.T) {
arena.MatchState = PreMatch
// Check with a single team e-stopped, not linked and bypassed.
arena.AllianceStations["R1"].EmergencyStop = true
arena.AllianceStations["R1"].Estop = true
err = arena.StartMatch()
if assert.NotNil(t, err) {
assert.Contains(t, err.Error(), "while an emergency stop is active")
}
arena.AllianceStations["R1"].EmergencyStop = false
arena.AllianceStations["R1"].Estop = false
arena.AllianceStations["R1"].DsConn.RobotLinked = false
err = arena.StartMatch()
if assert.NotNil(t, err) {
@@ -349,12 +349,12 @@ func TestMatchStartRobotLinkEnforcement(t *testing.T) {
arena.AllianceStations["B1"].Bypass = true
arena.AllianceStations["B2"].Bypass = true
arena.AllianceStations["B3"].Bypass = true
arena.AllianceStations["B3"].EmergencyStop = true
arena.AllianceStations["B3"].Estop = true
err = arena.StartMatch()
if assert.NotNil(t, err) {
assert.Contains(t, err.Error(), "while an emergency stop is active")
}
arena.AllianceStations["B3"].EmergencyStop = false
arena.AllianceStations["B3"].Estop = false
err = arena.StartMatch()
assert.Nil(t, err)
}

View File

@@ -32,7 +32,7 @@ type DriverStationConnection struct {
AllianceStation string
Auto bool
Enabled bool
EmergencyStop bool
Estop bool
DsLinked bool
RobotLinked bool
BatteryVoltage float64
@@ -165,7 +165,7 @@ func (dsConn *DriverStationConnection) encodeControlPacket(arena *Arena) [22]byt
if dsConn.Enabled {
packet[3] |= 0x04
}
if dsConn.EmergencyStop {
if dsConn.Estop {
packet[3] |= 0x80
}

View File

@@ -74,7 +74,7 @@ func TestEncodeControlPacket(t *testing.T) {
data = dsConn.encodeControlPacket(arena)
assert.Equal(t, byte(4), data[3])
dsConn.EmergencyStop = true
dsConn.Estop = true
data = dsConn.encodeControlPacket(arena)
assert.Equal(t, byte(132), data[3])

323
field/plc.go Normal file
View File

@@ -0,0 +1,323 @@
// Copyright 2017 Team 254. All Rights Reserved.
// Author: pat@patfairbank.com (Patrick Fairbank)
//
// Methods for interfacing with the field PLC.
package field
import (
"fmt"
"github.com/goburrow/modbus"
"log"
"time"
)
type Plc struct {
IsHealthy bool
BlinkState bool
address string
handler *modbus.TCPClientHandler
client modbus.Client
Inputs [15]bool
Counters [10]uint16
Coils [24]bool
}
const (
modbusPort = 502
rotorGearToothCount = 15
plcLoopPeriodMs = 100
plcRetryIntevalSec = 3
)
// Discrete inputs
const (
fieldEstop = iota
redEstop1
redEstop2
redEstop3
redRotor1
redTouchpad1
redTouchpad2
redTouchpad3
blueEstop1
blueEstop2
blueEstop3
blueRotor1
blueTouchpad1
blueTouchpad2
blueTouchpad3
)
// 16-bit registers
const (
redRotor2Count = iota
redRotor3Count
redRotor4Count
redLowBoilerCount
redHighBoilerCount
blueRotor2Count
blueRotor3Count
blueRotor4Count
blueLowBoilerCount
blueHighBoilerCount
)
// Coils
const (
redSerializer = iota
redBallLift
redRotorMotor1
redRotorMotor2
redRotorMotor3
redRotorMotor4
redAutoLight1
redAutoLight2
redTouchpadLight1
redTouchpadLight2
redTouchpadLight3
blueSerializer
blueBallLift
blueRotorMotor1
blueRotorMotor2
blueRotorMotor3
blueRotorMotor4
blueAutoLight1
blueAutoLight2
blueTouchpadLight1
blueTouchpadLight2
blueTouchpadLight3
resetCounts
heartbeat
)
func (plc *Plc) SetAddress(address string) {
plc.address = address
plc.resetConnection()
}
// Loops indefinitely to read inputs from and write outputs to PLC.
func (plc *Plc) Run() {
for {
if plc.handler == nil {
if plc.address == "" {
time.Sleep(time.Second * plcRetryIntevalSec)
plc.IsHealthy = false
continue
}
err := plc.connect()
if err != nil {
log.Printf("PLC error: %v", err)
time.Sleep(time.Second * plcRetryIntevalSec)
plc.IsHealthy = false
continue
}
}
startTime := time.Now()
isHealthy := true
isHealthy = isHealthy && plc.readInputs()
isHealthy = isHealthy && plc.readCounters()
isHealthy = isHealthy && plc.writeCoils()
if !isHealthy {
plc.resetConnection()
}
plc.IsHealthy = isHealthy
plc.BlinkState = !plc.BlinkState
time.Sleep(time.Until(startTime.Add(time.Millisecond * plcLoopPeriodMs)))
}
}
// Returns the state of the field emergency stop button (true if e-stop is active).
func (plc *Plc) GetFieldEstop() bool {
return plc.address != "" && !plc.Inputs[fieldEstop]
}
// Returns the state of the red and blue driver station emergency stop buttons (true if e-stop is active).
func (plc *Plc) GetTeamEstops() ([3]bool, [3]bool) {
var redEstops, blueEstops [3]bool
if plc.address != "" {
redEstops[0] = !plc.Inputs[redEstop1]
redEstops[1] = !plc.Inputs[redEstop2]
redEstops[2] = !plc.Inputs[redEstop3]
blueEstops[0] = !plc.Inputs[blueEstop1]
blueEstops[1] = !plc.Inputs[blueEstop2]
blueEstops[2] = !plc.Inputs[blueEstop3]
}
return redEstops, blueEstops
}
// Returns the count of the red and blue low and high boilers.
func (plc *Plc) GetBalls() (int, int, int, int) {
return int(plc.Counters[redLowBoilerCount]), int(plc.Counters[redHighBoilerCount]),
int(plc.Counters[blueLowBoilerCount]), int(plc.Counters[blueHighBoilerCount])
}
// Returns the state of red and blue activated rotors.
func (plc *Plc) GetRotors() ([4]bool, [4]bool) {
var redRotors, blueRotors [4]bool
redRotors[0] = plc.Inputs[redRotor1]
redRotors[1] = int(plc.Counters[redRotor2Count]) >= rotorGearToothCount
redRotors[2] = int(plc.Counters[redRotor3Count]) >= rotorGearToothCount
redRotors[3] = int(plc.Counters[redRotor4Count]) >= rotorGearToothCount
blueRotors[0] = plc.Inputs[blueRotor1]
blueRotors[1] = int(plc.Counters[blueRotor2Count]) >= rotorGearToothCount
blueRotors[2] = int(plc.Counters[blueRotor3Count]) >= rotorGearToothCount
blueRotors[3] = int(plc.Counters[blueRotor4Count]) >= rotorGearToothCount
return redRotors, blueRotors
}
func (plc *Plc) GetTouchpads() ([3]bool, [3]bool) {
var redTouchpads, blueTouchpads [3]bool
redTouchpads[0] = plc.Inputs[redTouchpad1]
redTouchpads[1] = plc.Inputs[redTouchpad2]
redTouchpads[2] = plc.Inputs[redTouchpad3]
blueTouchpads[0] = plc.Inputs[blueTouchpad1]
blueTouchpads[1] = plc.Inputs[blueTouchpad2]
blueTouchpads[2] = plc.Inputs[blueTouchpad3]
return redTouchpads, blueTouchpads
}
// Resets the ball and rotor gear tooth counts to zero.
func (plc *Plc) ResetCounts() {
plc.Coils[resetCounts] = true
}
func (plc *Plc) SetBoilerMotors(on bool) {
plc.Coils[redSerializer] = on
plc.Coils[redBallLift] = on
plc.Coils[blueSerializer] = on
plc.Coils[blueBallLift] = on
}
// Turns on/off the rotor motors based on how many rotors each alliance has.
func (plc *Plc) SetRotorMotors(redRotors, blueRotors int) {
plc.Coils[redRotorMotor1] = redRotors >= 1
plc.Coils[redRotorMotor2] = redRotors >= 2
plc.Coils[redRotorMotor3] = redRotors >= 3
plc.Coils[redRotorMotor4] = redRotors == 4
plc.Coils[blueRotorMotor1] = blueRotors >= 1
plc.Coils[blueRotorMotor2] = blueRotors >= 2
plc.Coils[blueRotorMotor3] = blueRotors >= 3
plc.Coils[blueRotorMotor4] = blueRotors == 4
}
// Turns on/off the auto rotor lights based on how many auto rotors each alliance has.
func (plc *Plc) SetRotorLights(redAutoRotors, blueAutoRotors int) {
plc.Coils[redAutoLight1] = redAutoRotors >= 1
plc.Coils[redAutoLight2] = redAutoRotors == 2
plc.Coils[blueAutoLight1] = blueAutoRotors >= 1
plc.Coils[blueAutoLight2] = blueAutoRotors == 2
}
func (plc *Plc) SetTouchpadLights(redTouchpads, blueTouchpads [3]bool) {
plc.Coils[redTouchpadLight1] = redTouchpads[0]
plc.Coils[redTouchpadLight2] = redTouchpads[1]
plc.Coils[redTouchpadLight3] = redTouchpads[2]
plc.Coils[blueTouchpadLight1] = blueTouchpads[0]
plc.Coils[blueTouchpadLight2] = blueTouchpads[1]
plc.Coils[blueTouchpadLight3] = blueTouchpads[2]
}
func (plc *Plc) connect() error {
address := fmt.Sprintf("%s:%d", plc.address, modbusPort)
handler := modbus.NewTCPClientHandler(address)
handler.Timeout = 1 * time.Second
handler.SlaveId = 0xFF
err := handler.Connect()
if err != nil {
return err
}
log.Printf("Connected to PLC at %s", address)
plc.handler = handler
plc.client = modbus.NewClient(plc.handler)
plc.writeCoils() // Force initial write of the coils upon connection since they may not be triggered by a change.
return nil
}
func (plc *Plc) resetConnection() {
if plc.handler != nil {
plc.handler.Close()
plc.handler = nil
}
}
func (plc *Plc) readInputs() bool {
inputs, err := plc.client.ReadDiscreteInputs(0, uint16(len(plc.Inputs)))
if err != nil {
log.Printf("PLC error reading inputs: %v", err)
return false
}
if len(inputs)*8 < len(plc.Inputs) {
log.Printf("Insufficient length of PLC inputs: got %d bytes, expected %d bits.", len(inputs), len(plc.Inputs))
return false
}
copy(plc.Inputs[:], byteToBool(inputs, len(plc.Inputs)))
return true
}
func (plc *Plc) readCounters() bool {
registers, err := plc.client.ReadHoldingRegisters(0, uint16(len(plc.Counters)))
if err != nil {
log.Printf("PLC error reading registers: %v", err)
return false
}
if len(registers)/2 < len(plc.Counters) {
log.Printf("Insufficient length of PLC counters: got %d bytes, expected %d words.", len(registers),
len(plc.Counters))
return false
}
copy(plc.Counters[:], byteToUint(registers, len(plc.Counters)))
return true
}
func (plc *Plc) writeCoils() bool {
// Send a heartbeat to the PLC so that it can disable outputs if the connection is lost.
plc.Coils[heartbeat] = true
coils := boolToByte(plc.Coils[:])
_, err := plc.client.WriteMultipleCoils(0, uint16(len(plc.Coils)), coils)
if err != nil {
log.Printf("PLC error writing coils: %v", err)
return false
}
plc.Coils[resetCounts] = false // Only need to send a single pulse to reset the counters.
return true
}
func byteToBool(bytes []byte, size int) []bool {
bools := make([]bool, size)
for i := 0; i < size; i++ {
byteIndex := i / 8
bitIndex := uint(i % 8)
bitMask := byte(1 << bitIndex)
bools[i] = bytes[byteIndex]&bitMask != 0
}
return bools
}
func byteToUint(bytes []byte, size int) []uint16 {
uints := make([]uint16, size)
for i := 0; i < size; i++ {
uints[i] = uint16(bytes[2*i])<<8 + uint16(bytes[2*i+1])
}
return uints
}
func boolToByte(bools []bool) []byte {
bytes := make([]byte, (len(bools)+7)/8)
for i, bit := range bools {
if bit {
bytes[i/8] |= 1 << uint(i%8)
}
}
return bytes
}

36
field/plc_test.go Normal file
View File

@@ -0,0 +1,36 @@
// Copyright 2017 Team 254. All Rights Reserved.
// Author: pat@patfairbank.com (Patrick Fairbank)
package field
import (
"github.com/stretchr/testify/assert"
"testing"
)
func TestByteToBool(t *testing.T) {
bytes := []byte{7, 254, 3}
bools := byteToBool(bytes, 17)
if assert.Equal(t, 17, len(bools)) {
expectedBools := []bool{true, true, true, false, false, false, false, false, false, true, true, true, true,
true, true, true, true}
assert.Equal(t, expectedBools, bools)
}
}
func TestByteToUint(t *testing.T) {
bytes := []byte{1, 77, 2, 253, 21, 179}
uints := byteToUint(bytes, 3)
if assert.Equal(t, 3, len(uints)) {
assert.Equal(t, []uint16{333, 765, 5555}, uints)
}
}
func TestBoolToByte(t *testing.T) {
bools := []bool{true, true, false, false, true, false, false, false, false, true}
bytes := boolToByte(bools)
if assert.Equal(t, 2, len(bytes)) {
assert.Equal(t, []byte{19, 2}, bytes)
assert.Equal(t, bools, byteToBool(bytes, len(bools)))
}
}

View File

@@ -8,15 +8,15 @@ package field
import "github.com/Team254/cheesy-arena/game"
type RealtimeScore struct {
CurrentScore *game.Score
CurrentScore game.Score
Cards map[string]string
TeleopCommitted bool
FoulsCommitted bool
boiler game.Boiler
rotorSet game.RotorSet
touchpads [3]game.Touchpad
}
func NewRealtimeScore() *RealtimeScore {
realtimeScore := new(RealtimeScore)
realtimeScore.CurrentScore = new(game.Score)
realtimeScore.Cards = make(map[string]string)
return realtimeScore
return &RealtimeScore{Cards: make(map[string]string)}
}

View File

@@ -45,7 +45,7 @@ func NewTeamMatchLog(teamId int, match *model.Match) (*TeamMatchLog, error) {
// Adds a line to the log when a packet is received.
func (log *TeamMatchLog) LogDsPacket(matchTimeSec float64, packetType int, dsConn *DriverStationConnection) {
log.logger.Printf("%f,%d,%d,%s,%v,%v,%v,%v,%f,%d,%d", matchTimeSec, packetType, dsConn.TeamId,
dsConn.AllianceStation, dsConn.RobotLinked, dsConn.Auto, dsConn.Enabled, dsConn.EmergencyStop,
dsConn.AllianceStation, dsConn.RobotLinked, dsConn.Auto, dsConn.Enabled, dsConn.Estop,
dsConn.BatteryVoltage, dsConn.MissedPacketCount, dsConn.DsRobotTripTimeMs)
}

View File

@@ -24,6 +24,7 @@ type EventSettings struct {
SwitchAddress string
SwitchPassword string
BandwidthMonitoringEnabled bool
PlcAddress string
AdminPassword string
ReaderPassword string
StemTvPublishingEnabled bool

View File

@@ -54,11 +54,11 @@ var handleMatchTime = function(data) {
var handleRealtimeScore = function(data) {
$("#redScoreNumber").text(data.RedScoreSummary.Score);
$("#redPressurePoints").text(data.RedScoreSummary.PressurePoints);
$("#redRotors").text(data.RedScoreSummary.Rotors);
$("#redRotors").text(data.RedScore.AutoRotors + data.RedScore.Rotors);
$("#redTakeoffs").text(data.RedScore.Takeoffs);
$("#blueScoreNumber").text(data.BlueScoreSummary.Score);
$("#bluePressurePoints").text(data.BlueScoreSummary.PressurePoints);
$("#blueRotors").text(data.BlueScoreSummary.Rotors);
$("#blueRotors").text(data.BlueScore.AutoRotors + data.BlueScore.Rotors);
$("#blueTakeoffs").text(data.BlueScore.Takeoffs);
};

View File

@@ -50,7 +50,7 @@ var handleStatus = function(data) {
$("#status" + station + " .packet-loss").text("");
}
if (stationStatus.EmergencyStop) {
if (stationStatus.Estop) {
$("#status" + station + " .bypass-status-fta").attr("data-status-ok", false);
$("#status" + station + " .bypass-status-fta").text("ES");
} else if (stationStatus.Bypass) {

View File

@@ -87,7 +87,7 @@ var handleStatus = function(data) {
$("#status" + station + " .battery-status").text("");
}
if (stationStatus.EmergencyStop) {
if (stationStatus.Estop) {
$("#status" + station + " .bypass-status").attr("data-status-ok", false);
$("#status" + station + " .bypass-status").text("ES");
} else if (stationStatus.Bypass) {
@@ -127,6 +127,15 @@ var handleStatus = function(data) {
$("#editResults").prop("disabled", false);
break;
}
if (data.PlcIsHealthy) {
$("#plcStatus").text("Connected");
$("#plcStatus").attr("data-ready", true);
} else {
$("#plcStatus").text("Not Connected");
$("#plcStatus").attr("data-ready", false);
}
$("#fieldEstop").attr("data-ready", !data.FieldEstop)
};
// Handles a websocket message to update the match time countdown.

View File

@@ -188,6 +188,13 @@
</label>
</div>
</div>
{{if .EventSettings.PlcAddress}}
PLC Status
<p>
<span class="label label-scoring" id="plcStatus"></span><br />
<span class="label label-scoring" id="fieldEstop">E-Stop</span>
</p>
{{end}}
</div>
</div>
</div>

View File

@@ -37,65 +37,106 @@
</div>
<div class="col-lg-4">
<div class="well">
<form class="" action="/setup/field/lights" method="POST">
Light Control
<legend>PLC</legend>
<form class="" action="/setup/field/test" method="POST">
<div class="form-group">
<div class="radio">
<label>
<input type="radio" name="mode" value="off" onclick="this.form.submit()"
{{if eq .LightsMode "off"}}checked{{end}}>Off
<input type="radio" name="mode" value="" onclick="this.form.submit()"
{{if eq .FieldTestMode ""}}checked{{end}}>Off
</label>
</div>
<div class="radio">
<label>
<input type="radio" name="mode" value="all_white" onclick="this.form.submit()"
{{if eq .LightsMode "all_white"}}checked{{end}}>All White
<input type="radio" name="mode" value="boiler" onclick="this.form.submit()"
{{if eq .FieldTestMode "boiler"}}checked{{end}}>Boilers On
</label>
</div>
<div class="radio">
<label>
<input type="radio" name="mode" value="all_red" onclick="this.form.submit()"
{{if eq .LightsMode "all_red"}}checked{{end}}>All Red
<input type="radio" name="mode" value="rotor1" onclick="this.form.submit()"
{{if eq .FieldTestMode "rotor1"}}checked{{end}}>1 Rotor/Touchpad 1 On
</label>
</div>
<div class="radio">
<label>
<input type="radio" name="mode" value="all_blue" onclick="this.form.submit()"
{{if eq .LightsMode "all_blue"}}checked{{end}}>All Blue
<input type="radio" name="mode" value="rotor2" onclick="this.form.submit()"
{{if eq .FieldTestMode "rotor2"}}checked{{end}}>2 Rotors/Touchpad 2 On
</label>
</div>
<div class="radio">
<label>
<input type="radio" name="mode" value="all_green" onclick="this.form.submit()"
{{if eq .LightsMode "all_green"}}checked{{end}}>All Green
<input type="radio" name="mode" value="rotor3" onclick="this.form.submit()"
{{if eq .FieldTestMode "rotor3"}}checked{{end}}>3 Rotors/Touchpad 3 On
</label>
</div>
<div class="radio">
<label>
<input type="radio" name="mode" value="strobe" onclick="this.form.submit()"
{{if eq .LightsMode "strobe"}}checked{{end}}>Strobe
<input type="radio" name="mode" value="rotor4" onclick="this.form.submit()"
{{if eq .FieldTestMode "rotor4"}}checked{{end}}>4 Rotors On
</label>
</div>
<div class="radio">
<label>
<input type="radio" name="mode" value="fade_red" onclick="this.form.submit()"
{{if eq .LightsMode "fade_red"}}checked{{end}}>Fade Red
<input type="radio" name="mode" value="red" onclick="this.form.submit()"
{{if eq .FieldTestMode "red"}}checked{{end}}>All Red On
</label>
</div>
<div class="radio">
<label>
<input type="radio" name="mode" value="fade_blue" onclick="this.form.submit()"
{{if eq .LightsMode "fade_blue"}}checked{{end}}>Fade Blue
<input type="radio" name="mode" value="blue" onclick="this.form.submit()"
{{if eq .FieldTestMode "blue"}}checked{{end}}>All Blue On
</label>
</div>
<div class="radio">
<label>
<input type="radio" name="mode" value="fade_red_blue" onclick="this.form.submit()"
{{if eq .LightsMode "fade_red_blue"}}checked{{end}}>Fade Red/Blue
<input type="radio" name="mode" value="flash" onclick="this.form.submit()"
{{if eq .FieldTestMode "flash"}}checked{{end}}>Flash Touchpads
</label>
</div>
</div>
</form>
<div class="row">
<div class="col-lg-4">
<table>
<tr>
<th>Inputs</th>
</tr>
{{range $i, $value := .Inputs}}
<tr>
<td>{{$i}}</td>
<td>{{$value}}</td>
</tr>
{{end}}
</table>
</div>
<div class="col-lg-4">
<table>
<tr>
<th>Counters</th>
</tr>
{{range $i, $value := .Counters}}
<tr>
<td>{{$i}}</td>
<td>{{$value}}</td>
</tr>
{{end}}
</table>
</div>
<div class="col-lg-4">
<table>
<tr>
<th>Coils</th>
</tr>
{{range $i, $value := .Coils}}
<tr>
<td>{{$i}}</td>
<td>{{$value}}</td>
</tr>
{{end}}
</table>
</div>
</div>
</div>
</div>
</div>

View File

@@ -199,6 +199,15 @@
</div>
</div>
</fieldset>
<fieldset>
<legend>PLC</legend>
<div class="form-group">
<label class="col-lg-5 control-label">PLC Address</label>
<div class="col-lg-7">
<input type="text" class="form-control" name="plcAddress" value="{{.PlcAddress}}">
</div>
</div>
</fieldset>
<div class="form-group">
<div class="col-lg-7 col-lg-offset-5">
<button type="submit" class="btn btn-info">Save</button>

View File

@@ -98,7 +98,7 @@ func (web *Web) audienceDisplayWebsocketHandler(w http.ResponseWriter, r *http.R
BlueScore *game.Score
RedScoreSummary *game.ScoreSummary
BlueScoreSummary *game.ScoreSummary
}{web.arena.RedRealtimeScore.CurrentScore, web.arena.BlueRealtimeScore.CurrentScore,
}{&web.arena.RedRealtimeScore.CurrentScore, &web.arena.BlueRealtimeScore.CurrentScore,
web.arena.RedScoreSummary(), web.arena.BlueScoreSummary()}
err = websocket.Write("realtimeScore", data)
if err != nil {
@@ -160,7 +160,7 @@ func (web *Web) audienceDisplayWebsocketHandler(w http.ResponseWriter, r *http.R
BlueScore *game.Score
RedScoreSummary *game.ScoreSummary
BlueScoreSummary *game.ScoreSummary
}{web.arena.RedRealtimeScore.CurrentScore, web.arena.BlueRealtimeScore.CurrentScore,
}{&web.arena.RedRealtimeScore.CurrentScore, &web.arena.BlueRealtimeScore.CurrentScore,
web.arena.RedScoreSummary(), web.arena.BlueScoreSummary()}
case _, ok := <-scorePostedListener:
if !ok {

View File

@@ -415,7 +415,7 @@ func (web *Web) matchPlayWebsocketHandler(w http.ResponseWriter, r *http.Request
}
// Send out the status again after handling the command, as it most likely changed as a result.
err = websocket.Write("status", web.arena)
err = websocket.Write("status", web.arena.GetStatus())
if err != nil {
log.Printf("Websocket error: %s", err)
return
@@ -541,7 +541,7 @@ func (web *Web) commitMatchScore(match *model.Match, matchResult *model.MatchRes
func (web *Web) getCurrentMatchResult() *model.MatchResult {
return &model.MatchResult{MatchId: web.arena.CurrentMatch.Id, MatchType: web.arena.CurrentMatch.Type,
RedScore: web.arena.RedRealtimeScore.CurrentScore, BlueScore: web.arena.BlueRealtimeScore.CurrentScore,
RedScore: &web.arena.RedRealtimeScore.CurrentScore, BlueScore: &web.arena.BlueRealtimeScore.CurrentScore,
RedCards: web.arena.RedRealtimeScore.Cards, BlueCards: web.arena.BlueRealtimeScore.Cards}
}

View File

@@ -129,8 +129,8 @@ func (web *Web) matchReviewEditPostHandler(w http.ResponseWriter, r *http.Reques
if isCurrent {
// If editing the current match, just save it back to memory.
web.arena.RedRealtimeScore.CurrentScore = matchResult.RedScore
web.arena.BlueRealtimeScore.CurrentScore = matchResult.BlueScore
web.arena.RedRealtimeScore.CurrentScore = *matchResult.RedScore
web.arena.BlueRealtimeScore.CurrentScore = *matchResult.BlueScore
web.arena.RedRealtimeScore.Cards = matchResult.RedCards
web.arena.BlueRealtimeScore.Cards = matchResult.BlueCards

View File

@@ -6,6 +6,7 @@
package web
import (
"github.com/Team254/cheesy-arena/field"
"github.com/Team254/cheesy-arena/model"
"net/http"
)
@@ -24,7 +25,12 @@ func (web *Web) fieldGetHandler(w http.ResponseWriter, r *http.Request) {
data := struct {
*model.EventSettings
AllianceStationDisplays map[string]string
}{web.arena.EventSettings, web.arena.AllianceStationDisplays}
FieldTestMode string
Inputs []bool
Counters []uint16
Coils []bool
}{web.arena.EventSettings, web.arena.AllianceStationDisplays, web.arena.FieldTestMode, web.arena.Plc.Inputs[:],
web.arena.Plc.Counters[:], web.arena.Plc.Coils[:]}
err = template.ExecuteTemplate(w, "base", data)
if err != nil {
handleWebErr(w, err)
@@ -54,3 +60,64 @@ func (web *Web) fieldReloadDisplaysHandler(w http.ResponseWriter, r *http.Reques
web.arena.ReloadDisplaysNotifier.Notify(nil)
http.Redirect(w, r, "/setup/field", 303)
}
// Controls the field LEDs for testing or effect.
func (web *Web) fieldTestPostHandler(w http.ResponseWriter, r *http.Request) {
if !web.userIsAdmin(w, r) {
return
}
if web.arena.MatchState != field.PreMatch {
http.Error(w, "Arena must be in pre-match state", 400)
return
}
mode := r.PostFormValue("mode")
switch mode {
case "boiler":
web.arena.Plc.SetBoilerMotors(true)
web.arena.Plc.SetRotorMotors(0, 0)
web.arena.Plc.SetRotorLights(0, 0)
web.arena.Plc.SetTouchpadLights([3]bool{false, false, false}, [3]bool{false, false, false})
case "rotor1":
web.arena.Plc.SetBoilerMotors(false)
web.arena.Plc.SetRotorMotors(1, 1)
web.arena.Plc.SetRotorLights(1, 1)
web.arena.Plc.SetTouchpadLights([3]bool{true, false, false}, [3]bool{true, false, false})
case "rotor2":
web.arena.Plc.SetBoilerMotors(false)
web.arena.Plc.SetRotorMotors(2, 2)
web.arena.Plc.SetRotorLights(2, 2)
web.arena.Plc.SetTouchpadLights([3]bool{false, true, false}, [3]bool{false, true, false})
case "rotor3":
web.arena.Plc.SetBoilerMotors(false)
web.arena.Plc.SetRotorMotors(3, 3)
web.arena.Plc.SetRotorLights(2, 2)
web.arena.Plc.SetTouchpadLights([3]bool{false, false, true}, [3]bool{false, false, true})
case "rotor4":
web.arena.Plc.SetBoilerMotors(false)
web.arena.Plc.SetRotorMotors(4, 4)
web.arena.Plc.SetRotorLights(2, 2)
web.arena.Plc.SetTouchpadLights([3]bool{false, false, false}, [3]bool{false, false, false})
case "red":
web.arena.Plc.SetBoilerMotors(false)
web.arena.Plc.SetRotorMotors(4, 0)
web.arena.Plc.SetRotorLights(2, 0)
web.arena.Plc.SetTouchpadLights([3]bool{true, true, true}, [3]bool{false, false, false})
case "blue":
web.arena.Plc.SetBoilerMotors(false)
web.arena.Plc.SetRotorMotors(0, 4)
web.arena.Plc.SetRotorLights(0, 2)
web.arena.Plc.SetTouchpadLights([3]bool{false, false, false}, [3]bool{true, true, true})
case "flash":
fallthrough
default:
web.arena.Plc.SetBoilerMotors(false)
web.arena.Plc.SetRotorMotors(0, 0)
web.arena.Plc.SetRotorLights(0, 0)
web.arena.Plc.SetTouchpadLights([3]bool{false, false, false}, [3]bool{false, false, false})
}
web.arena.FieldTestMode = mode
http.Redirect(w, r, "/setup/field", 303)
}

View File

@@ -23,10 +23,7 @@ func TestSetupField(t *testing.T) {
assert.Contains(t, recorder.Body.String(), "12345")
assert.Contains(t, recorder.Body.String(), "selected")
// TODO(patrick): Replace with PLC mode.
/*
recorder = web.postHttpResponse("/setup/field/lights", "mode=strobe")
assert.Equal(t, 303, recorder.Code)
assert.Equal(t, "strobe", web.arena.Lights.currentMode)
*/
recorder = web.postHttpResponse("/setup/field/test", "mode=rotor2")
assert.Equal(t, 303, recorder.Code)
assert.Equal(t, "rotor2", web.arena.FieldTestMode)
}

View File

@@ -64,6 +64,7 @@ func (web *Web) settingsPostHandler(w http.ResponseWriter, r *http.Request) {
eventSettings.SwitchAddress = r.PostFormValue("switchAddress")
eventSettings.SwitchPassword = r.PostFormValue("switchPassword")
eventSettings.BandwidthMonitoringEnabled = r.PostFormValue("bandwidthMonitoringEnabled") == "on"
eventSettings.PlcAddress = r.PostFormValue("plcAddress")
eventSettings.AdminPassword = r.PostFormValue("adminPassword")
eventSettings.ReaderPassword = r.PostFormValue("readerPassword")

View File

@@ -150,6 +150,7 @@ func (web *Web) newHandler() http.Handler {
router.HandleFunc("/setup/field", web.fieldGetHandler).Methods("GET")
router.HandleFunc("/setup/field", web.fieldPostHandler).Methods("POST")
router.HandleFunc("/setup/field/reload_displays", web.fieldReloadDisplaysHandler).Methods("GET")
router.HandleFunc("/setup/field/test", web.fieldTestPostHandler).Methods("POST")
router.HandleFunc("/setup/lower_thirds", web.lowerThirdsGetHandler).Methods("GET")
router.HandleFunc("/setup/lower_thirds/websocket", web.lowerThirdsWebsocketHandler).Methods("GET")
router.HandleFunc("/setup/sponsor_slides", web.sponsorSlidesGetHandler).Methods("GET")