mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[wpilib] DriverStation: Change alliance station to use optional (#5229)
Many teams have issues trying to read the DS too early. By switching to an optional, we cause teams to check 2 things. Either 1) they explicitly check, and their code is correct, or 2) they just read .value() and their code reboots in a loop. However, because the DS will eventually connect, this 2nd case is ok, and should theoretically be undetectable on the field.
This commit is contained in:
@@ -5,6 +5,7 @@
|
||||
package edu.wpi.first.hal;
|
||||
|
||||
public enum AllianceStationID {
|
||||
Unknown,
|
||||
Red1,
|
||||
Red2,
|
||||
Red3,
|
||||
|
||||
@@ -147,12 +147,13 @@ public class DriverStationJNI extends JNIWrapper {
|
||||
*/
|
||||
private static native int nativeGetAllianceStation();
|
||||
|
||||
public static final int kRed1AllianceStation = 0;
|
||||
public static final int kRed2AllianceStation = 1;
|
||||
public static final int kRed3AllianceStation = 2;
|
||||
public static final int kBlue1AllianceStation = 3;
|
||||
public static final int kBlue2AllianceStation = 4;
|
||||
public static final int kBlue3AllianceStation = 5;
|
||||
public static final int kUnknownAllianceStation = 0;
|
||||
public static final int kRed1AllianceStation = 1;
|
||||
public static final int kRed2AllianceStation = 2;
|
||||
public static final int kRed3AllianceStation = 3;
|
||||
public static final int kBlue1AllianceStation = 4;
|
||||
public static final int kBlue2AllianceStation = 5;
|
||||
public static final int kBlue3AllianceStation = 6;
|
||||
|
||||
/**
|
||||
* Gets the current alliance station ID.
|
||||
@@ -162,6 +163,8 @@ public class DriverStationJNI extends JNIWrapper {
|
||||
*/
|
||||
public static AllianceStationID getAllianceStation() {
|
||||
switch (nativeGetAllianceStation()) {
|
||||
case kUnknownAllianceStation:
|
||||
return AllianceStationID.Unknown;
|
||||
case kRed1AllianceStation:
|
||||
return AllianceStationID.Red1;
|
||||
case kRed2AllianceStation:
|
||||
|
||||
@@ -15,7 +15,27 @@
|
||||
#include "hal/FRCUsageReporting.h"
|
||||
#include "hal/HALBase.h"
|
||||
|
||||
// TODO Static asserts
|
||||
static_assert(edu_wpi_first_hal_DriverStationJNI_kUnknownAllianceStation ==
|
||||
HAL_AllianceStationID_kUnknown);
|
||||
static_assert(edu_wpi_first_hal_DriverStationJNI_kRed1AllianceStation ==
|
||||
HAL_AllianceStationID_kRed1);
|
||||
static_assert(edu_wpi_first_hal_DriverStationJNI_kRed2AllianceStation ==
|
||||
HAL_AllianceStationID_kRed2);
|
||||
static_assert(edu_wpi_first_hal_DriverStationJNI_kRed3AllianceStation ==
|
||||
HAL_AllianceStationID_kRed3);
|
||||
static_assert(edu_wpi_first_hal_DriverStationJNI_kBlue1AllianceStation ==
|
||||
HAL_AllianceStationID_kBlue1);
|
||||
static_assert(edu_wpi_first_hal_DriverStationJNI_kBlue2AllianceStation ==
|
||||
HAL_AllianceStationID_kBlue2);
|
||||
static_assert(edu_wpi_first_hal_DriverStationJNI_kBlue3AllianceStation ==
|
||||
HAL_AllianceStationID_kBlue3);
|
||||
|
||||
static_assert(edu_wpi_first_hal_DriverStationJNI_kMaxJoystickAxes ==
|
||||
HAL_kMaxJoystickAxes);
|
||||
static_assert(edu_wpi_first_hal_DriverStationJNI_kMaxJoystickPOVs ==
|
||||
HAL_kMaxJoystickPOVs);
|
||||
static_assert(edu_wpi_first_hal_DriverStationJNI_kMaxJoysticks ==
|
||||
HAL_kMaxJoysticks);
|
||||
|
||||
using namespace hal;
|
||||
using namespace wpi::java;
|
||||
|
||||
@@ -42,6 +42,7 @@ typedef struct HAL_ControlWord HAL_ControlWord;
|
||||
|
||||
// clang-format off
|
||||
HAL_ENUM(HAL_AllianceStationID) {
|
||||
HAL_AllianceStationID_kUnknown = 0,
|
||||
HAL_AllianceStationID_kRed1,
|
||||
HAL_AllianceStationID_kRed2,
|
||||
HAL_AllianceStationID_kRed3,
|
||||
@@ -51,7 +52,7 @@ HAL_ENUM(HAL_AllianceStationID) {
|
||||
};
|
||||
|
||||
HAL_ENUM(HAL_MatchType) {
|
||||
HAL_kMatchType_none,
|
||||
HAL_kMatchType_none = 0,
|
||||
HAL_kMatchType_practice,
|
||||
HAL_kMatchType_qualification,
|
||||
HAL_kMatchType_elimination,
|
||||
|
||||
@@ -84,6 +84,9 @@ void HALSimWSProviderDriverStation::RegisterCallbacks() {
|
||||
case HAL_AllianceStationID_kBlue3:
|
||||
station = "blue3";
|
||||
break;
|
||||
case HAL_AllianceStationID_kUnknown:
|
||||
station = "unknown";
|
||||
break;
|
||||
}
|
||||
static_cast<HALSimWSProviderDriverStation*>(param)->ProcessHalCallback(
|
||||
{{">station", station}});
|
||||
|
||||
@@ -533,7 +533,7 @@ int DriverStation::GetReplayNumber() {
|
||||
return info.replayNumber;
|
||||
}
|
||||
|
||||
DriverStation::Alliance DriverStation::GetAlliance() {
|
||||
std::optional<DriverStation::Alliance> DriverStation::GetAlliance() {
|
||||
int32_t status = 0;
|
||||
auto allianceStationID = HAL_GetAllianceStation(&status);
|
||||
switch (allianceStationID) {
|
||||
@@ -546,11 +546,11 @@ DriverStation::Alliance DriverStation::GetAlliance() {
|
||||
case HAL_AllianceStationID_kBlue3:
|
||||
return kBlue;
|
||||
default:
|
||||
return kInvalid;
|
||||
return {};
|
||||
}
|
||||
}
|
||||
|
||||
int DriverStation::GetLocation() {
|
||||
std::optional<int> DriverStation::GetLocation() {
|
||||
int32_t status = 0;
|
||||
auto allianceStationID = HAL_GetAllianceStation(&status);
|
||||
switch (allianceStationID) {
|
||||
@@ -564,7 +564,7 @@ int DriverStation::GetLocation() {
|
||||
case HAL_AllianceStationID_kBlue3:
|
||||
return 3;
|
||||
default:
|
||||
return 0;
|
||||
return {};
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
#include <string>
|
||||
|
||||
#include <units/time.h>
|
||||
@@ -21,7 +22,7 @@ namespace frc {
|
||||
*/
|
||||
class DriverStation final {
|
||||
public:
|
||||
enum Alliance { kRed, kBlue, kInvalid };
|
||||
enum Alliance { kRed, kBlue };
|
||||
enum MatchType { kNone, kPractice, kQualification, kElimination };
|
||||
|
||||
static constexpr int kJoystickPorts = 6;
|
||||
@@ -282,7 +283,7 @@ class DriverStation final {
|
||||
*
|
||||
* @return The Alliance enum (kRed, kBlue or kInvalid)
|
||||
*/
|
||||
static Alliance GetAlliance();
|
||||
static std::optional<Alliance> GetAlliance();
|
||||
|
||||
/**
|
||||
* Return the driver station location from the FMS.
|
||||
@@ -294,7 +295,7 @@ class DriverStation final {
|
||||
*
|
||||
* @return The location of the driver station (1-3, 0 for invalid)
|
||||
*/
|
||||
static int GetLocation();
|
||||
static std::optional<int> GetLocation();
|
||||
|
||||
/**
|
||||
* Wait for a DS connection.
|
||||
|
||||
@@ -129,6 +129,17 @@ TEST(DriverStationTest, AllianceStationId) {
|
||||
|
||||
auto cb = DriverStationSim::RegisterAllianceStationIdCallback(
|
||||
callback.GetCallback(), false);
|
||||
|
||||
// Unknown
|
||||
allianceStation = HAL_AllianceStationID_kUnknown;
|
||||
DriverStationSim::SetAllianceStationId(allianceStation);
|
||||
frc::sim::DriverStationSim::NotifyNewData();
|
||||
EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
|
||||
EXPECT_FALSE(DriverStation::GetAlliance().has_value());
|
||||
EXPECT_FALSE(DriverStation::GetLocation().has_value());
|
||||
EXPECT_TRUE(callback.WasTriggered());
|
||||
EXPECT_EQ(allianceStation, callback.GetLastValue());
|
||||
|
||||
// B1
|
||||
allianceStation = HAL_AllianceStationID_kBlue1;
|
||||
DriverStationSim::SetAllianceStationId(allianceStation);
|
||||
|
||||
@@ -18,14 +18,18 @@ void Robot::RobotPeriodic() {
|
||||
//
|
||||
// For example, "RET043" would indicate that the robot is on the red
|
||||
// alliance, enabled in teleop mode, with 43 seconds left in the match.
|
||||
auto string = fmt::format(
|
||||
"{}{}{}{:03}",
|
||||
frc::DriverStation::GetAlliance() == frc::DriverStation::Alliance::kRed
|
||||
? "R"
|
||||
: "B",
|
||||
frc::DriverStation::IsEnabled() ? "E" : "D",
|
||||
frc::DriverStation::IsAutonomous() ? "A" : "T",
|
||||
static_cast<int>(frc::Timer::GetMatchTime().value()));
|
||||
|
||||
std::string allianceString = "U";
|
||||
auto alliance = frc::DriverStation::GetAlliance();
|
||||
if (alliance.has_value()) {
|
||||
allianceString = alliance == frc::DriverStation::Alliance::kRed ? "R" : "B";
|
||||
}
|
||||
|
||||
auto string =
|
||||
fmt::format("{}{}{}{:03}", allianceString,
|
||||
frc::DriverStation::IsEnabled() ? "E" : "D",
|
||||
frc::DriverStation::IsAutonomous() ? "A" : "T",
|
||||
static_cast<int>(frc::Timer::GetMatchTime().value()));
|
||||
|
||||
arduino.WriteBulk(reinterpret_cast<uint8_t*>(string.data()), string.size());
|
||||
}
|
||||
|
||||
@@ -60,6 +60,7 @@ TEST_P(AllianceTest, Alliance) {
|
||||
case HAL_AllianceStationID_kBlue1:
|
||||
case HAL_AllianceStationID_kBlue2:
|
||||
case HAL_AllianceStationID_kBlue3:
|
||||
case HAL_AllianceStationID_kUnknown:
|
||||
isRed = false;
|
||||
break;
|
||||
case HAL_AllianceStationID_kRed1:
|
||||
@@ -76,7 +77,8 @@ INSTANTIATE_TEST_SUITE_P(
|
||||
testing::Values<HAL_AllianceStationID>(
|
||||
HAL_AllianceStationID_kRed1, HAL_AllianceStationID_kRed2,
|
||||
HAL_AllianceStationID_kRed3, HAL_AllianceStationID_kBlue1,
|
||||
HAL_AllianceStationID_kBlue2, HAL_AllianceStationID_kBlue3),
|
||||
HAL_AllianceStationID_kBlue2, HAL_AllianceStationID_kBlue3,
|
||||
HAL_AllianceStationID_kUnknown),
|
||||
[](const testing::TestParamInfo<AllianceTest::ParamType>& info) {
|
||||
switch (info.param) {
|
||||
case HAL_AllianceStationID_kBlue1:
|
||||
@@ -91,6 +93,8 @@ INSTANTIATE_TEST_SUITE_P(
|
||||
return std::string{"Red2"};
|
||||
case HAL_AllianceStationID_kRed3:
|
||||
return std::string{"Red3"};
|
||||
case HAL_AllianceStationID_kUnknown:
|
||||
return std::string{"Unknown"};
|
||||
}
|
||||
return std::string{"Error"};
|
||||
});
|
||||
|
||||
@@ -74,6 +74,9 @@ TEST_P(AllianceTest, Alliance) {
|
||||
case HAL_AllianceStationID_kRed3:
|
||||
expected = 'R';
|
||||
break;
|
||||
case HAL_AllianceStationID_kUnknown:
|
||||
expected = 'U';
|
||||
break;
|
||||
}
|
||||
EXPECT_EQ(expected, gString.at(0));
|
||||
}
|
||||
@@ -83,7 +86,8 @@ INSTANTIATE_TEST_SUITE_P(
|
||||
testing::Values<HAL_AllianceStationID>(
|
||||
HAL_AllianceStationID_kRed1, HAL_AllianceStationID_kRed2,
|
||||
HAL_AllianceStationID_kRed3, HAL_AllianceStationID_kBlue1,
|
||||
HAL_AllianceStationID_kBlue2, HAL_AllianceStationID_kBlue3),
|
||||
HAL_AllianceStationID_kBlue2, HAL_AllianceStationID_kBlue3,
|
||||
HAL_AllianceStationID_kUnknown),
|
||||
[](const testing::TestParamInfo<AllianceTest::ParamType>& info) {
|
||||
switch (info.param) {
|
||||
case HAL_AllianceStationID_kBlue1:
|
||||
@@ -98,6 +102,8 @@ INSTANTIATE_TEST_SUITE_P(
|
||||
return std::string{"Red2"};
|
||||
case HAL_AllianceStationID_kRed3:
|
||||
return std::string{"Red3"};
|
||||
case HAL_AllianceStationID_kUnknown:
|
||||
return std::string{"Unknown"};
|
||||
}
|
||||
return std::string{"Error"};
|
||||
});
|
||||
|
||||
@@ -22,6 +22,9 @@ import edu.wpi.first.util.datalog.DataLog;
|
||||
import edu.wpi.first.util.datalog.FloatArrayLogEntry;
|
||||
import edu.wpi.first.util.datalog.IntegerArrayLogEntry;
|
||||
import java.nio.ByteBuffer;
|
||||
import java.util.Map;
|
||||
import java.util.Optional;
|
||||
import java.util.OptionalInt;
|
||||
import java.util.concurrent.locks.ReentrantLock;
|
||||
|
||||
/** Provide access to the network communication data to / from the Driver Station. */
|
||||
@@ -67,8 +70,7 @@ public final class DriverStation {
|
||||
/** The robot alliance that the robot is a part of. */
|
||||
public enum Alliance {
|
||||
Red,
|
||||
Blue,
|
||||
Invalid
|
||||
Blue
|
||||
}
|
||||
|
||||
public enum MatchType {
|
||||
@@ -1086,6 +1088,26 @@ public final class DriverStation {
|
||||
}
|
||||
}
|
||||
|
||||
private static Map<AllianceStationID, Optional<Alliance>> m_allianceMap =
|
||||
Map.of(
|
||||
AllianceStationID.Unknown, Optional.empty(),
|
||||
AllianceStationID.Red1, Optional.of(Alliance.Red),
|
||||
AllianceStationID.Red2, Optional.of(Alliance.Red),
|
||||
AllianceStationID.Red3, Optional.of(Alliance.Red),
|
||||
AllianceStationID.Blue1, Optional.of(Alliance.Blue),
|
||||
AllianceStationID.Blue2, Optional.of(Alliance.Blue),
|
||||
AllianceStationID.Blue3, Optional.of(Alliance.Blue));
|
||||
|
||||
private static Map<AllianceStationID, OptionalInt> m_stationMap =
|
||||
Map.of(
|
||||
AllianceStationID.Unknown, OptionalInt.empty(),
|
||||
AllianceStationID.Red1, OptionalInt.of(1),
|
||||
AllianceStationID.Red2, OptionalInt.of(2),
|
||||
AllianceStationID.Red3, OptionalInt.of(3),
|
||||
AllianceStationID.Blue1, OptionalInt.of(1),
|
||||
AllianceStationID.Blue2, OptionalInt.of(2),
|
||||
AllianceStationID.Blue3, OptionalInt.of(3));
|
||||
|
||||
/**
|
||||
* Get the current alliance from the FMS.
|
||||
*
|
||||
@@ -1093,26 +1115,13 @@ public final class DriverStation {
|
||||
*
|
||||
* @return the current alliance
|
||||
*/
|
||||
public static Alliance getAlliance() {
|
||||
public static Optional<Alliance> getAlliance() {
|
||||
AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation();
|
||||
if (allianceStationID == null) {
|
||||
return Alliance.Invalid;
|
||||
allianceStationID = AllianceStationID.Unknown;
|
||||
}
|
||||
|
||||
switch (allianceStationID) {
|
||||
case Red1:
|
||||
case Red2:
|
||||
case Red3:
|
||||
return Alliance.Red;
|
||||
|
||||
case Blue1:
|
||||
case Blue2:
|
||||
case Blue3:
|
||||
return Alliance.Blue;
|
||||
|
||||
default:
|
||||
return Alliance.Invalid;
|
||||
}
|
||||
return m_allianceMap.get(allianceStationID);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -1122,27 +1131,25 @@ public final class DriverStation {
|
||||
*
|
||||
* @return the location of the team's driver station controls: 1, 2, or 3
|
||||
*/
|
||||
public static int getLocation() {
|
||||
public static OptionalInt getLocation() {
|
||||
AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation();
|
||||
if (allianceStationID == null) {
|
||||
return 0;
|
||||
allianceStationID = AllianceStationID.Unknown;
|
||||
}
|
||||
switch (allianceStationID) {
|
||||
case Red1:
|
||||
case Blue1:
|
||||
return 1;
|
||||
|
||||
case Red2:
|
||||
case Blue2:
|
||||
return 2;
|
||||
return m_stationMap.get(allianceStationID);
|
||||
}
|
||||
|
||||
case Blue3:
|
||||
case Red3:
|
||||
return 3;
|
||||
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
/**
|
||||
* Gets the raw alliance station of the teams driver station.
|
||||
*
|
||||
* <p>This returns the raw low level value. Prefer getLocation or getAlliance unless necessary for
|
||||
* performance.
|
||||
*
|
||||
* @return The raw alliance station id.
|
||||
*/
|
||||
public static AllianceStationID getRawAllianceStation() {
|
||||
return DriverStationJNI.getAllianceStation();
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -229,20 +229,22 @@ public final class DriverStationSim {
|
||||
*/
|
||||
public static AllianceStationID getAllianceStationId() {
|
||||
switch (DriverStationDataJNI.getAllianceStationId()) {
|
||||
case 0:
|
||||
case DriverStationJNI.kUnknownAllianceStation:
|
||||
return AllianceStationID.Unknown;
|
||||
case DriverStationJNI.kRed1AllianceStation:
|
||||
return AllianceStationID.Red1;
|
||||
case 1:
|
||||
case DriverStationJNI.kRed2AllianceStation:
|
||||
return AllianceStationID.Red2;
|
||||
case 2:
|
||||
case DriverStationJNI.kRed3AllianceStation:
|
||||
return AllianceStationID.Red3;
|
||||
case 3:
|
||||
case DriverStationJNI.kBlue1AllianceStation:
|
||||
return AllianceStationID.Blue1;
|
||||
case 4:
|
||||
case DriverStationJNI.kBlue2AllianceStation:
|
||||
return AllianceStationID.Blue2;
|
||||
case 5:
|
||||
case DriverStationJNI.kBlue3AllianceStation:
|
||||
return AllianceStationID.Blue3;
|
||||
default:
|
||||
return null;
|
||||
return AllianceStationID.Unknown;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -254,23 +256,26 @@ public final class DriverStationSim {
|
||||
public static void setAllianceStationId(AllianceStationID allianceStationId) {
|
||||
int allianceStation;
|
||||
switch (allianceStationId) {
|
||||
case Unknown:
|
||||
allianceStation = DriverStationJNI.kUnknownAllianceStation;
|
||||
break;
|
||||
case Red1:
|
||||
allianceStation = 0;
|
||||
allianceStation = DriverStationJNI.kRed1AllianceStation;
|
||||
break;
|
||||
case Red2:
|
||||
allianceStation = 1;
|
||||
allianceStation = DriverStationJNI.kRed2AllianceStation;
|
||||
break;
|
||||
case Red3:
|
||||
allianceStation = 2;
|
||||
allianceStation = DriverStationJNI.kRed3AllianceStation;
|
||||
break;
|
||||
case Blue1:
|
||||
allianceStation = 3;
|
||||
allianceStation = DriverStationJNI.kBlue1AllianceStation;
|
||||
break;
|
||||
case Blue2:
|
||||
allianceStation = 4;
|
||||
allianceStation = DriverStationJNI.kBlue2AllianceStation;
|
||||
break;
|
||||
case Blue3:
|
||||
allianceStation = 5;
|
||||
allianceStation = DriverStationJNI.kBlue3AllianceStation;
|
||||
break;
|
||||
default:
|
||||
return;
|
||||
|
||||
@@ -134,13 +134,23 @@ class DriverStationSimTest {
|
||||
DriverStationSim.setAllianceStationId(allianceStation);
|
||||
|
||||
try (CallbackStore cb = DriverStationSim.registerAllianceStationIdCallback(callback, false)) {
|
||||
// Unknown
|
||||
allianceStation = AllianceStationID.Unknown;
|
||||
DriverStationSim.setAllianceStationId(allianceStation);
|
||||
DriverStationSim.notifyNewData();
|
||||
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
|
||||
assertFalse(DriverStation.getAlliance().isPresent());
|
||||
assertFalse(DriverStation.getLocation().isPresent());
|
||||
assertTrue(callback.wasTriggered());
|
||||
assertEquals(allianceStation.ordinal(), callback.getSetValue());
|
||||
|
||||
// B1
|
||||
allianceStation = AllianceStationID.Blue1;
|
||||
DriverStationSim.setAllianceStationId(allianceStation);
|
||||
DriverStationSim.notifyNewData();
|
||||
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
|
||||
assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance());
|
||||
assertEquals(1, DriverStation.getLocation());
|
||||
assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance().get());
|
||||
assertEquals(1, DriverStation.getLocation().getAsInt());
|
||||
assertTrue(callback.wasTriggered());
|
||||
assertEquals(allianceStation.ordinal(), callback.getSetValue());
|
||||
|
||||
@@ -149,8 +159,8 @@ class DriverStationSimTest {
|
||||
DriverStationSim.setAllianceStationId(allianceStation);
|
||||
DriverStationSim.notifyNewData();
|
||||
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
|
||||
assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance());
|
||||
assertEquals(2, DriverStation.getLocation());
|
||||
assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance().get());
|
||||
assertEquals(2, DriverStation.getLocation().getAsInt());
|
||||
assertTrue(callback.wasTriggered());
|
||||
assertEquals(allianceStation.ordinal(), callback.getSetValue());
|
||||
|
||||
@@ -159,8 +169,8 @@ class DriverStationSimTest {
|
||||
DriverStationSim.setAllianceStationId(allianceStation);
|
||||
DriverStationSim.notifyNewData();
|
||||
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
|
||||
assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance());
|
||||
assertEquals(3, DriverStation.getLocation());
|
||||
assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance().get());
|
||||
assertEquals(3, DriverStation.getLocation().getAsInt());
|
||||
assertTrue(callback.wasTriggered());
|
||||
assertEquals(allianceStation.ordinal(), callback.getSetValue());
|
||||
|
||||
@@ -169,8 +179,8 @@ class DriverStationSimTest {
|
||||
DriverStationSim.setAllianceStationId(allianceStation);
|
||||
DriverStationSim.notifyNewData();
|
||||
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
|
||||
assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance());
|
||||
assertEquals(1, DriverStation.getLocation());
|
||||
assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance().get());
|
||||
assertEquals(1, DriverStation.getLocation().getAsInt());
|
||||
assertTrue(callback.wasTriggered());
|
||||
assertEquals(allianceStation.ordinal(), callback.getSetValue());
|
||||
|
||||
@@ -179,8 +189,8 @@ class DriverStationSimTest {
|
||||
DriverStationSim.setAllianceStationId(allianceStation);
|
||||
DriverStationSim.notifyNewData();
|
||||
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
|
||||
assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance());
|
||||
assertEquals(2, DriverStation.getLocation());
|
||||
assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance().get());
|
||||
assertEquals(2, DriverStation.getLocation().getAsInt());
|
||||
assertTrue(callback.wasTriggered());
|
||||
assertEquals(allianceStation.ordinal(), callback.getSetValue());
|
||||
|
||||
@@ -189,8 +199,8 @@ class DriverStationSimTest {
|
||||
DriverStationSim.setAllianceStationId(allianceStation);
|
||||
DriverStationSim.notifyNewData();
|
||||
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
|
||||
assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance());
|
||||
assertEquals(3, DriverStation.getLocation());
|
||||
assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance().get());
|
||||
assertEquals(3, DriverStation.getLocation().getAsInt());
|
||||
assertTrue(callback.wasTriggered());
|
||||
assertEquals(allianceStation.ordinal(), callback.getSetValue());
|
||||
}
|
||||
|
||||
@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.examples.digitalcommunication;
|
||||
import edu.wpi.first.wpilibj.DigitalOutput;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import java.util.Optional;
|
||||
|
||||
/**
|
||||
* This is a sample program demonstrating how to communicate to a light controller from the robot
|
||||
@@ -26,8 +27,14 @@ public class Robot extends TimedRobot {
|
||||
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
boolean setAlliance = false;
|
||||
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
|
||||
if (alliance.isPresent()) {
|
||||
setAlliance = alliance.get() == DriverStation.Alliance.Red;
|
||||
}
|
||||
|
||||
// pull alliance port high if on red alliance, pull low if on blue alliance
|
||||
m_allianceOutput.set(DriverStation.getAlliance() == DriverStation.Alliance.Red);
|
||||
m_allianceOutput.set(setAlliance);
|
||||
|
||||
// pull enabled port high if enabled, low if disabled
|
||||
m_enabledOutput.set(DriverStation.isEnabled());
|
||||
|
||||
@@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.I2C;
|
||||
import edu.wpi.first.wpilibj.I2C.Port;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import java.util.Optional;
|
||||
|
||||
/**
|
||||
* This is a sample program demonstrating how to communicate to a light controller from the robot
|
||||
@@ -48,8 +49,14 @@ public class Robot extends TimedRobot {
|
||||
// alliance, enabled in teleop mode, with 43 seconds left in the match.
|
||||
StringBuilder stateMessage = new StringBuilder(6);
|
||||
|
||||
String allianceString = "U";
|
||||
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
|
||||
if (alliance.isPresent()) {
|
||||
allianceString = alliance.get() == DriverStation.Alliance.Red ? "R" : "B";
|
||||
}
|
||||
|
||||
stateMessage
|
||||
.append(DriverStation.getAlliance() == DriverStation.Alliance.Red ? "R" : "B")
|
||||
.append(allianceString)
|
||||
.append(DriverStation.isEnabled() ? "E" : "D")
|
||||
.append(DriverStation.isAutonomous() ? "A" : "T")
|
||||
.append(String.format("%03d", (int) DriverStation.getMatchTime()));
|
||||
|
||||
@@ -73,6 +73,9 @@ class I2CCommunicationTest {
|
||||
|
||||
String str = assertTimeoutPreemptively(Duration.ofMillis(20L), () -> m_future.get());
|
||||
char expected = alliance.name().startsWith("Red") ? 'R' : 'B';
|
||||
if (alliance.name().startsWith("Unknown")) {
|
||||
expected = 'U';
|
||||
}
|
||||
|
||||
assertEquals(expected, str.charAt(0));
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user