diff --git a/db/migrations/20140524160241_CreateEventSettings.sql b/db/migrations/20140524160241_CreateEventSettings.sql index a0b806d..6fa6151 100644 --- a/db/migrations/20140524160241_CreateEventSettings.sql +++ b/db/migrations/20140524160241_CreateEventSettings.sql @@ -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), diff --git a/field/arena.go b/field/arena.go index d4ad8ff..410f0f6 100644 --- a/field/arena.go +++ b/field/arena.go @@ -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 + } +} diff --git a/field/arena_test.go b/field/arena_test.go index d116b91..10afd19 100644 --- a/field/arena_test.go +++ b/field/arena_test.go @@ -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) } diff --git a/field/driver_station_connection.go b/field/driver_station_connection.go index 5eca354..dbe67d6 100644 --- a/field/driver_station_connection.go +++ b/field/driver_station_connection.go @@ -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 } diff --git a/field/driver_station_connection_test.go b/field/driver_station_connection_test.go index c12661c..464824b 100644 --- a/field/driver_station_connection_test.go +++ b/field/driver_station_connection_test.go @@ -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]) diff --git a/field/plc.go b/field/plc.go new file mode 100644 index 0000000..fea77d5 --- /dev/null +++ b/field/plc.go @@ -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 +} diff --git a/field/plc_test.go b/field/plc_test.go new file mode 100644 index 0000000..263155f --- /dev/null +++ b/field/plc_test.go @@ -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))) + } +} diff --git a/field/realtime_score.go b/field/realtime_score.go index 5f03759..ed416a5 100644 --- a/field/realtime_score.go +++ b/field/realtime_score.go @@ -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)} } diff --git a/field/team_match_log.go b/field/team_match_log.go index ca3813d..4002ac2 100644 --- a/field/team_match_log.go +++ b/field/team_match_log.go @@ -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) } diff --git a/model/event_settings.go b/model/event_settings.go index 33db7c8..0bdbc5d 100644 --- a/model/event_settings.go +++ b/model/event_settings.go @@ -24,6 +24,7 @@ type EventSettings struct { SwitchAddress string SwitchPassword string BandwidthMonitoringEnabled bool + PlcAddress string AdminPassword string ReaderPassword string StemTvPublishingEnabled bool diff --git a/static/js/audience_display.js b/static/js/audience_display.js index 1b486a9..ac9bd1d 100644 --- a/static/js/audience_display.js +++ b/static/js/audience_display.js @@ -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); }; diff --git a/static/js/fta_display.js b/static/js/fta_display.js index 6217239..c1b7cba 100644 --- a/static/js/fta_display.js +++ b/static/js/fta_display.js @@ -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) { diff --git a/static/js/match_play.js b/static/js/match_play.js index 7f03cd3..3799a09 100644 --- a/static/js/match_play.js +++ b/static/js/match_play.js @@ -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. diff --git a/templates/match_play.html b/templates/match_play.html index dbae5c9..a09a197 100644 --- a/templates/match_play.html +++ b/templates/match_play.html @@ -188,6 +188,13 @@ + {{if .EventSettings.PlcAddress}} + PLC Status +

+
+ E-Stop +

+ {{end}} diff --git a/templates/setup_field.html b/templates/setup_field.html index b826d9e..e568e0d 100644 --- a/templates/setup_field.html +++ b/templates/setup_field.html @@ -37,65 +37,106 @@
-
- Light Control + PLC +
+
+
+ + + + + {{range $i, $value := .Inputs}} + + + + + {{end}} +
Inputs
{{$i}}{{$value}}
+
+
+ + + + + {{range $i, $value := .Counters}} + + + + + {{end}} +
Counters
{{$i}}{{$value}}
+
+
+ + + + + {{range $i, $value := .Coils}} + + + + + {{end}} +
Coils
{{$i}}{{$value}}
+
+
diff --git a/templates/setup_settings.html b/templates/setup_settings.html index 581520e..d6fc6d5 100644 --- a/templates/setup_settings.html +++ b/templates/setup_settings.html @@ -199,6 +199,15 @@ +
+ PLC +
+ +
+ +
+
+
diff --git a/web/audience_display.go b/web/audience_display.go index 7c19217..755039e 100644 --- a/web/audience_display.go +++ b/web/audience_display.go @@ -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 { diff --git a/web/match_play.go b/web/match_play.go index afa37be..bf38143 100644 --- a/web/match_play.go +++ b/web/match_play.go @@ -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} } diff --git a/web/match_review.go b/web/match_review.go index d322986..be5fab7 100644 --- a/web/match_review.go +++ b/web/match_review.go @@ -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 diff --git a/web/setup_field.go b/web/setup_field.go index 9506497..2eb5209 100644 --- a/web/setup_field.go +++ b/web/setup_field.go @@ -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) +} diff --git a/web/setup_field_test.go b/web/setup_field_test.go index 29a2a1a..807409d 100644 --- a/web/setup_field_test.go +++ b/web/setup_field_test.go @@ -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) } diff --git a/web/setup_settings.go b/web/setup_settings.go index 84cdbc5..f2bbe5b 100644 --- a/web/setup_settings.go +++ b/web/setup_settings.go @@ -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") diff --git a/web/web.go b/web/web.go index f4ad334..e9594cd 100644 --- a/web/web.go +++ b/web/web.go @@ -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")