diff --git a/romiVendordep/.styleguide b/romiVendordep/.styleguide new file mode 100644 index 0000000000..7c8d7a70fb --- /dev/null +++ b/romiVendordep/.styleguide @@ -0,0 +1,31 @@ +cppHeaderFileInclude { + \.h$ + \.hpp$ + \.inc$ +} + +cppSrcFileInclude { + \.cpp$ +} + +repoRootNameOverride { + wpilib +} + +includeOtherLibs { + ^cameraserver/ + ^cscore + ^fmt/ + ^frc/ + ^gtest/ + ^hal/ + ^imgui + ^mockdata/ + ^networktables/ + ^ntcore + ^opencv2/ + ^support/ + ^units/ + ^vision/ + ^wpi/ +} diff --git a/romiVendordep/CMakeLists.txt b/romiVendordep/CMakeLists.txt new file mode 100644 index 0000000000..6ec0a2b36c --- /dev/null +++ b/romiVendordep/CMakeLists.txt @@ -0,0 +1,59 @@ +project(romiVendordep) + +include(SubDirList) +include(CompileWarnings) +include(AddTest) + +if (WITH_JAVA) + find_package(Java REQUIRED) + include(UseJava) + set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") + + file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) + add_jar(romiVendordep_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME romiVendordep) + + get_property(ROMIVENDORDEP_JAR_FILE TARGET romiVendordep_jar PROPERTY JAR_FILE) + install(FILES ${ROMIVENDORDEP_JAR_FILE} DESTINATION "${java_lib_dest}") + + set_property(TARGET romiVendordep_jar PROPERTY FOLDER "java") + + if (WITH_FLAT_INSTALL) + set (romiVendordep_config_dir ${wpilib_dest}) + else() + set (romiVendordep_config_dir share/romiVendordep) + endif() + + install(FILES romiVendordep-config.cmake DESTINATION ${romiVendordep_config_dir}) +endif() + +file(GLOB_RECURSE romiVendordep_native_src src/main/native/cpp/*.cpp) +add_library(romiVendordep ${romiVendordep_native_src}) +set_target_properties(romiVendordep PROPERTIES DEBUG_POSTFIX "d") +set_property(TARGET romiVendordep PROPERTY FOLDER "libraries") + +target_compile_features(romiVendordep PUBLIC cxx_std_20) +wpilib_target_warnings(romiVendordep) +target_link_libraries(romiVendordep wpilibc) + +target_include_directories(romiVendordep PUBLIC + $ + $) + +install(TARGETS romiVendordep EXPORT romiVendordep DESTINATION "${main_lib_dest}") +install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/romiVendordep") + +if (FLAT_INSTALL_WPILIB) + set(romiVendordep_config_dir ${wpilib_dest}) + else() + set(romiVendordep_config_dir share/romiVendordep) + endif() + + configure_file(romiVendordep-config.cmake.in ${WPILIB_BINARY_DIR}/romiVendordep-config.cmake) + install(FILES ${WPILIB_BINARY_DIR}/romiVendordep-config.cmake DESTINATION ${romiVendordep_config_dir}) + install(EXPORT romiVendordep DESTINATION ${romiVendordep_config_dir}) + + if (WITH_TESTS) + wpilib_add_test(romiVendordep src/test/native/cpp) + target_include_directories(romiVendordep_test PRIVATE src/test/native/include) + target_link_libraries(romiVendordep_test romiVendordep gmock_main) + endif() diff --git a/romiVendordep/README.md b/romiVendordep/README.md new file mode 100644 index 0000000000..e970a6c52f --- /dev/null +++ b/romiVendordep/README.md @@ -0,0 +1,2 @@ +# romi-vendordep +WPILib Romi Vendordep diff --git a/romiVendordep/RomiVendordep.json b/romiVendordep/RomiVendordep.json new file mode 100644 index 0000000000..1a4da2a1a8 --- /dev/null +++ b/romiVendordep/RomiVendordep.json @@ -0,0 +1,36 @@ +{ + "fileName": "RomiVendordep.json", + "name": "Romi-Vendordep", + "version": "1.0.0", + "uuid": "1010372a-b446-46f4-b229-61e53a26a7dc", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.romiVendordep", + "artifactId": "romiVendordep-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.romiVendordep", + "artifactId": "romiVendordep-cpp", + "version": "vendordep", + "libName": "romiVendordep", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/romiVendordep/build.gradle b/romiVendordep/build.gradle new file mode 100644 index 0000000000..d2f0c0d0f7 --- /dev/null +++ b/romiVendordep/build.gradle @@ -0,0 +1,98 @@ +ext { + nativeName = 'romiVendordep' + devMain = 'edu.wpi.first.wpilibj.romi.DevMain' +} + +evaluationDependsOn(":ntcore") +evaluationDependsOn(":cscore") +evaluationDependsOn(":hal") +evaluationDependsOn(":wpimath") +evaluationDependsOn(":wpilibc") +evaluationDependsOn(":cameraserver") +evaluationDependsOn(":wpilibj") + +apply from: "${rootDir}/shared/javacpp/setupBuild.gradle" + +dependencies { + implementation project(":wpiutil") + implementation project(":wpinet") + implementation project(":ntcore") + implementation project(":cscore") + implementation project(":hal") + implementation project(":wpimath") + implementation project(":wpilibj") +} + +nativeUtils.exportsConfigs { + // Main library is just default empty. This will export everything + romiVendordep { + x64ExcludeSymbols = [ + '_CT??_R0?AV_System_error', + '_CT??_R0?AVexception', + '_CT??_R0?AVfailure', + '_CT??_R0?AVruntime_error', + '_CT??_R0?AVsystem_error', + '_CTA5?AVfailure', + '_TI5?AVfailure', + '_CT??_R0?AVout_of_range', + '_CTA3?AVout_of_range', + '_TI3?AVout_of_range', + '_CT??_R0?AVbad_cast' + ] + } +} + +model { + components {} + binaries { + all { + if (!it.buildable || !(it instanceof NativeBinarySpec)) { + return + } + lib project: ":wpilibc", library: "wpilibc", linkage: "shared" + project(":ntcore").addNtcoreDependency(it, "shared") + project(":hal").addHalDependency(it, "shared") + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' + lib project: ':wpinet', library: 'wpinet', linkage: 'shared' + lib project: ':wpimath', library: 'wpimath', linkage: 'shared' + + if (it.component.name == "${nativeName}Dev") { + project(":ntcore").addNtcoreJniDependency(it) + lib project: ":wpinet", library: "wpinetJNIShared", linkage: "shared" + lib project: ":wpiutil", library: "wpiutilJNIShared", linkage: "shared" + lib project: ":wpimath", library: "wpimathJNIShared", linkage: "shared" + project(":hal").addHalJniDependency(it) + } + + if (it instanceof GoogleTestTestSuiteBinarySpec) { + nativeUtils.useRequiredLibrary(it, "opencv_shared") + lib project: ":cscore", library: "cscore", linkage: "shared" + } + } + } + tasks { + def c = $.components + def found = false + def systemArch = getCurrentArch() + c.each { + if (it in NativeExecutableSpec && it.name == "${nativeName}Dev") { + it.binaries.each { + if (!found) { + def arch = it.targetPlatform.name + if (arch == systemArch) { + def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + "lib" + found = true + } + } + } + } + } + } +} + +test { + testLogging { + outputs.upToDateWhen {false} + showStandardStreams = true + } +} diff --git a/romiVendordep/romiVendordep-config.cmake.in b/romiVendordep/romiVendordep-config.cmake.in new file mode 100644 index 0000000000..3711d9cd6d --- /dev/null +++ b/romiVendordep/romiVendordep-config.cmake.in @@ -0,0 +1,11 @@ +include(CMakeFindDependencyMacro) + @WPIUTIL_DEP_REPLACE@ + @NTCORE_DEP_REPLACE@ + @CSCORE_DEP_REPLACE@ + @CAMERASERVER_DEP_REPLACE@ + @HAL_DEP_REPLACE@ + @WPILIBC_DEP_REPLACE@ + @WPIMATH_DEP_REPLACE@ + + @FILENAME_DEP_REPLACE@ + include(${SELF_DIR}/romiVendordep.cmake) diff --git a/romiVendordep/src/dev/java/edu/wpi/first/wpilibj/romi/DevMain.java b/romiVendordep/src/dev/java/edu/wpi/first/wpilibj/romi/DevMain.java new file mode 100644 index 0000000000..846263ed5e --- /dev/null +++ b/romiVendordep/src/dev/java/edu/wpi/first/wpilibj/romi/DevMain.java @@ -0,0 +1,21 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.romi; + +import edu.wpi.first.hal.HALUtil; +import edu.wpi.first.networktables.NetworkTablesJNI; +import edu.wpi.first.util.RuntimeDetector; + +public final class DevMain { + /** Main entry point. */ + public static void main(String[] args) { + System.out.println("Hello World!"); + System.out.println(RuntimeDetector.getPlatformPath()); + System.out.println(NetworkTablesJNI.now()); + System.out.println(HALUtil.getHALRuntimeType()); + } + + private DevMain() {} +} diff --git a/romiVendordep/src/dev/native/cpp/main.cpp b/romiVendordep/src/dev/native/cpp/main.cpp new file mode 100644 index 0000000000..a3e363efca --- /dev/null +++ b/romiVendordep/src/dev/native/cpp/main.cpp @@ -0,0 +1,5 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +int main() {} diff --git a/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/OnBoardIO.java b/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/OnBoardIO.java new file mode 100644 index 0000000000..02dcc2a137 --- /dev/null +++ b/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/OnBoardIO.java @@ -0,0 +1,150 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.romi; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DigitalOutput; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; + +/** + * This class represents the onboard IO of the Romi reference robot. This includes the pushbuttons + * and LEDs. + * + *

DIO 0 - Button A (input only) DIO 1 - Button B (input) or Green LED (output) DIO 2 - Button C + * (input) or Red LED (output) DIO 3 - Yellow LED (output only) + */ +public class OnBoardIO { + private final DigitalInput m_buttonA = new DigitalInput(0); + private final DigitalOutput m_yellowLed = new DigitalOutput(3); + + // DIO 1 + private final DigitalInput m_buttonB; + private final DigitalOutput m_greenLed; + + // DIO 2 + private final DigitalInput m_buttonC; + private final DigitalOutput m_redLed; + + private static final double MESSAGE_INTERVAL = 1.0; + private double m_nextMessageTime; + + public enum ChannelMode { + INPUT, + OUTPUT + } + + /** + * Constructor. + * + * @param dio1 Mode for DIO 1 (input = Button B, output = green LED) + * @param dio2 Mode for DIO 2 (input = Button C, output = red LED) + */ + public OnBoardIO(ChannelMode dio1, ChannelMode dio2) { + if (dio1 == ChannelMode.INPUT) { + m_buttonB = new DigitalInput(1); + m_greenLed = null; + } else { + m_greenLed = new DigitalOutput(1); + m_buttonB = null; + } + + if (dio2 == ChannelMode.INPUT) { + m_buttonC = new DigitalInput(2); + m_redLed = null; + } else { + m_redLed = new DigitalOutput(2); + m_buttonC = null; + } + } + + /** + * Gets if the A button is pressed. + * + * @return Whether or not Button A is pressed + */ + public boolean getButtonAPressed() { + return m_buttonA.get(); + } + + /** + * Gets if the B button is pressed. + * + * @return Whether or not Button B is pressed + */ + public boolean getButtonBPressed() { + if (m_buttonB != null) { + return m_buttonB.get(); + } + + double currentTime = Timer.getFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + DriverStation.reportError("Button B was not configured", true); + m_nextMessageTime = currentTime + MESSAGE_INTERVAL; + } + return false; + } + + /** + * Gets if the C button is pressed. + * + * @return Whether or not Button C is pressed + */ + public boolean getButtonCPressed() { + if (m_buttonC != null) { + return m_buttonC.get(); + } + + double currentTime = Timer.getFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + DriverStation.reportError("Button C was not configured", true); + m_nextMessageTime = currentTime + MESSAGE_INTERVAL; + } + return false; + } + + /** + * Sets the green LED. + * + * @param value Set whether or not to turn the Green LED on + */ + public void setGreenLed(boolean value) { + if (m_greenLed != null) { + m_greenLed.set(value); + } else { + double currentTime = Timer.getFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + DriverStation.reportError("Green LED was not configured", true); + m_nextMessageTime = currentTime + MESSAGE_INTERVAL; + } + } + } + + /** + * Sets the red LED. + * + * @param value Set whether or not to turn the Red LED on + */ + public void setRedLed(boolean value) { + if (m_redLed != null) { + m_redLed.set(value); + } else { + double currentTime = Timer.getFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + DriverStation.reportError("Red LED was not configured", true); + m_nextMessageTime = currentTime + MESSAGE_INTERVAL; + } + } + } + + /** + * Sets the yellow LED. + * + * @param value Set whether or not to turn the Yellow LED on + */ + public void setYellowLed(boolean value) { + m_yellowLed.set(value); + } +} diff --git a/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiGyro.java b/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiGyro.java new file mode 100644 index 0000000000..40eaefc20b --- /dev/null +++ b/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiGyro.java @@ -0,0 +1,147 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.romi; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimDouble; + +public class RomiGyro { + private final SimDevice m_simDevice; + private final SimDouble m_simRateX; + private final SimDouble m_simRateY; + private final SimDouble m_simRateZ; + private final SimDouble m_simAngleX; + private final SimDouble m_simAngleY; + private final SimDouble m_simAngleZ; + + private double m_angleXOffset; + private double m_angleYOffset; + private double m_angleZOffset; + + /** Create a new RomiGyro. */ + public RomiGyro() { + m_simDevice = SimDevice.create("Gyro:RomiGyro"); + if (m_simDevice != null) { + m_simDevice.createBoolean("init", Direction.kOutput, true); + m_simRateX = m_simDevice.createDouble("rate_x", Direction.kInput, 0.0); + m_simRateY = m_simDevice.createDouble("rate_y", Direction.kInput, 0.0); + m_simRateZ = m_simDevice.createDouble("rate_z", Direction.kInput, 0.0); + + m_simAngleX = m_simDevice.createDouble("angle_x", Direction.kInput, 0.0); + m_simAngleY = m_simDevice.createDouble("angle_y", Direction.kInput, 0.0); + m_simAngleZ = m_simDevice.createDouble("angle_z", Direction.kInput, 0.0); + } else { + m_simRateX = null; + m_simRateY = null; + m_simRateZ = null; + m_simAngleX = null; + m_simAngleY = null; + m_simAngleZ = null; + } + } + + /** + * Get the rate of turn in degrees-per-second around the X-axis. + * + * @return rate of turn in degrees-per-second + */ + public double getRateX() { + if (m_simRateX != null) { + return m_simRateX.get(); + } + + return 0.0; + } + + /** + * Get the rate of turn in degrees-per-second around the Y-axis. + * + * @return rate of turn in degrees-per-second + */ + public double getRateY() { + if (m_simRateY != null) { + return m_simRateY.get(); + } + + return 0.0; + } + + /** + * Get the rate of turn in degrees-per-second around the Z-axis. + * + * @return rate of turn in degrees-per-second + */ + public double getRateZ() { + if (m_simRateZ != null) { + return m_simRateZ.get(); + } + + return 0.0; + } + + /** + * Get the currently reported angle around the X-axis. + * + * @return current angle around X-axis in degrees + */ + public double getAngleX() { + if (m_simAngleX != null) { + return m_simAngleX.get() - m_angleXOffset; + } + + return 0.0; + } + + /** + * Get the currently reported angle around the X-axis. + * + * @return current angle around Y-axis in degrees + */ + public double getAngleY() { + if (m_simAngleY != null) { + return m_simAngleY.get() - m_angleYOffset; + } + + return 0.0; + } + + /** + * Get the currently reported angle around the Z-axis. + * + * @return current angle around Z-axis in degrees + */ + public double getAngleZ() { + if (m_simAngleZ != null) { + return m_simAngleZ.get() - m_angleZOffset; + } + + return 0.0; + } + + /** Reset the gyro angles to 0. */ + public void reset() { + if (m_simAngleX != null) { + m_angleXOffset = m_simAngleX.get(); + m_angleYOffset = m_simAngleY.get(); + m_angleZOffset = m_simAngleZ.get(); + } + } + + public double getAngle() { + return getAngleZ(); + } + + public double getRate() { + return getRateZ(); + } + + /** Close out the SimDevice. */ + public void close() { + if (m_simDevice != null) { + m_simDevice.close(); + } + } +} diff --git a/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java b/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java new file mode 100644 index 0000000000..93024f6a2c --- /dev/null +++ b/romiVendordep/src/main/java/edu/wpi/first/wpilibj/romi/RomiMotor.java @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.romi; + +import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController; + +/** + * RomiMotor. + * + *

A general use PWM motor controller representing the motors on a Romi robot + */ +public class RomiMotor extends PWMMotorController { + /** Common initialization code called by all constructors. */ + protected void initRomiMotor() { + m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X); + m_pwm.setSpeed(0.0); + m_pwm.setZeroLatch(); + } + + /** + * Constructor. + * + * @param channel The PWM channel that the RomiMotor is attached to. 0 is the left motor, 1 is the + * right. + */ + public RomiMotor(final int channel) { + super("Romi Motor", channel); + initRomiMotor(); + } +} diff --git a/romiVendordep/src/main/native/cpp/romi/OnBoardIO.cpp b/romiVendordep/src/main/native/cpp/romi/OnBoardIO.cpp new file mode 100644 index 0000000000..cefe9ff2e4 --- /dev/null +++ b/romiVendordep/src/main/native/cpp/romi/OnBoardIO.cpp @@ -0,0 +1,82 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include +#include + +using namespace frc; + +OnBoardIO::OnBoardIO(OnBoardIO::ChannelMode dio1, OnBoardIO::ChannelMode dio2) { + if (dio1 == ChannelMode::INPUT) { + m_buttonB = std::make_unique(1); + } else { + m_greenLed = std::make_unique(1); + } + if (dio2 == ChannelMode::INPUT) { + m_buttonC = std::make_unique(2); + } else { + m_redLed = std::make_unique(2); + } +} + +bool OnBoardIO::GetButtonAPressed() { + return m_buttonA.Get(); +} + +bool OnBoardIO::GetButtonBPressed() { + if (m_buttonB) { + return m_buttonB->Get(); + } + + auto currentTime = frc::Timer::GetFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + FRC_ReportError(frc::err::Error, "{}", "Button B was not configured"); + m_nextMessageTime = currentTime + kMessageInterval; + } + return false; +} + +bool OnBoardIO::GetButtonCPressed() { + if (m_buttonC) { + return m_buttonC->Get(); + } + + auto currentTime = frc::Timer::GetFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + FRC_ReportError(frc::err::Error, "{}", "Button C was not configured"); + m_nextMessageTime = currentTime + kMessageInterval; + } + return false; +} + +void OnBoardIO::SetGreenLed(bool value) { + if (m_greenLed) { + m_greenLed->Set(value); + } else { + auto currentTime = frc::Timer::GetFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + FRC_ReportError(frc::err::Error, "{}", "Green LED was not configured"); + m_nextMessageTime = currentTime + kMessageInterval; + } + } +} + +void OnBoardIO::SetRedLed(bool value) { + if (m_redLed) { + m_redLed->Set(value); + } else { + auto currentTime = frc::Timer::GetFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + FRC_ReportError(frc::err::Error, "{}", "Red LED was not configured"); + m_nextMessageTime = currentTime + kMessageInterval; + } + } +} + +void OnBoardIO::SetYellowLed(bool value) { + m_yellowLed.Set(value); +} diff --git a/romiVendordep/src/main/native/cpp/romi/RomiGyro.cpp b/romiVendordep/src/main/native/cpp/romi/RomiGyro.cpp new file mode 100644 index 0000000000..c44cff9d82 --- /dev/null +++ b/romiVendordep/src/main/native/cpp/romi/RomiGyro.cpp @@ -0,0 +1,89 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +using namespace frc; + +RomiGyro::RomiGyro() : m_simDevice("Gyro:RomiGyro") { + if (m_simDevice) { + m_simDevice.CreateBoolean("init", hal::SimDevice::kOutput, true); + m_simRateX = + m_simDevice.CreateDouble("rate_x", hal::SimDevice::kInput, 0.0); + m_simRateY = + m_simDevice.CreateDouble("rate_y", hal::SimDevice::kInput, 0.0); + m_simRateZ = + m_simDevice.CreateDouble("rate_z", hal::SimDevice::kInput, 0.0); + m_simAngleX = + m_simDevice.CreateDouble("angle_x", hal::SimDevice::kInput, 0.0); + m_simAngleY = + m_simDevice.CreateDouble("angle_y", hal::SimDevice::kInput, 0.0); + m_simAngleZ = + m_simDevice.CreateDouble("angle_z", hal::SimDevice::kInput, 0.0); + } +} + +double RomiGyro::GetAngle() const { + return GetAngleZ(); +} + +double RomiGyro::GetRate() const { + return GetRateZ(); +} + +double RomiGyro::GetRateX() const { + if (m_simRateX) { + return m_simRateX.Get(); + } + + return 0.0; +} + +double RomiGyro::GetRateY() const { + if (m_simRateY) { + return m_simRateY.Get(); + } + + return 0.0; +} + +double RomiGyro::GetRateZ() const { + if (m_simRateZ) { + return m_simRateZ.Get(); + } + + return 0.0; +} + +double RomiGyro::GetAngleX() const { + if (m_simAngleX) { + return m_simAngleX.Get() - m_angleXOffset; + } + + return 0.0; +} + +double RomiGyro::GetAngleY() const { + if (m_simAngleY) { + return m_simAngleY.Get() - m_angleYOffset; + } + + return 0.0; +} + +double RomiGyro::GetAngleZ() const { + if (m_simAngleZ) { + return m_simAngleZ.Get() - m_angleZOffset; + } + + return 0.0; +} + +void RomiGyro::Reset() { + if (m_simAngleX) { + m_angleXOffset = m_simAngleX.Get(); + m_angleYOffset = m_simAngleY.Get(); + m_angleZOffset = m_simAngleZ.Get(); + } +} diff --git a/romiVendordep/src/main/native/cpp/romi/RomiMotor.cpp b/romiVendordep/src/main/native/cpp/romi/RomiMotor.cpp new file mode 100644 index 0000000000..729dc038d3 --- /dev/null +++ b/romiVendordep/src/main/native/cpp/romi/RomiMotor.cpp @@ -0,0 +1,13 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +using namespace frc; + +RomiMotor::RomiMotor(int channel) : PWMMotorController("Romi Motor", channel) { + m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X); + m_pwm.SetSpeed(0.0); + m_pwm.SetZeroLatch(); +} diff --git a/romiVendordep/src/main/native/include/frc/romi/OnBoardIO.h b/romiVendordep/src/main/native/include/frc/romi/OnBoardIO.h new file mode 100644 index 0000000000..8e08a35c71 --- /dev/null +++ b/romiVendordep/src/main/native/include/frc/romi/OnBoardIO.h @@ -0,0 +1,76 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include + +namespace frc { + +/** + * This class represents the onboard IO of the Romi + * reference robot. This includes the pushbuttons and + * LEDs. + * + *

DIO 0 - Button A (input only) + * DIO 1 - Button B (input) or Green LED (output) + * DIO 2 - Button C (input) or Red LED (output) + * DIO 3 - Yellow LED (output only) + */ +class OnBoardIO { + public: + enum ChannelMode { INPUT, OUTPUT }; + OnBoardIO(OnBoardIO::ChannelMode dio1, OnBoardIO::ChannelMode dio2); + + static constexpr auto kMessageInterval = 1_s; + units::second_t m_nextMessageTime = 0_s; + + /** + * Gets if the A button is pressed. + */ + bool GetButtonAPressed(); + + /** + * Gets if the B button is pressed. + */ + bool GetButtonBPressed(); + + /** + * Gets if the C button is pressed. + */ + bool GetButtonCPressed(); + + /** + * Sets the green LED. + */ + void SetGreenLed(bool value); + + /** + * Sets the red LED. + */ + void SetRedLed(bool value); + + /** + * Sets the yellow LED. + */ + void SetYellowLed(bool value); + + private: + frc::DigitalInput m_buttonA{0}; + frc::DigitalOutput m_yellowLed{3}; + + // DIO 1 + std::unique_ptr m_buttonB; + std::unique_ptr m_greenLed; + + // DIO 2 + std::unique_ptr m_buttonC; + std::unique_ptr m_redLed; +}; + +} // namespace frc diff --git a/romiVendordep/src/main/native/include/frc/romi/RomiGyro.h b/romiVendordep/src/main/native/include/frc/romi/RomiGyro.h new file mode 100644 index 0000000000..363daf3a31 --- /dev/null +++ b/romiVendordep/src/main/native/include/frc/romi/RomiGyro.h @@ -0,0 +1,91 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace frc { + +/** + * Use a rate gyro to return the robots heading relative to a starting position. + * + * This class is for the Romi onboard gyro, and will only work in + * simulation/Romi mode. Only one instance of a RomiGyro is supported. + */ +class RomiGyro { + public: + RomiGyro(); + + /** + * Return the actual angle in degrees that the robot is currently facing. + * + * The angle is based on integration of the returned rate form the gyro. + * The angle is continuous, that is, it will continue from 360->361 degrees. + * This allows algorithms that wouldn't want to see a discontinuity in the + * gyro output as it sweeps from 360 to 0 on the second time around. + * + * @return the current heading of the robot in degrees. + */ + double GetAngle() const; + + /** + * Return the rate of rotation of the gyro + * + * The rate is based on the most recent reading of the gyro. + * + * @return the current rate in degrees per second + */ + double GetRate() const; + + /** + * Gets the rate of turn in degrees-per-second around the X-axis + */ + double GetRateX() const; + + /** + * Gets the rate of turn in degrees-per-second around the Y-axis + */ + double GetRateY() const; + + /** + * Gets the rate of turn in degrees-per-second around the Z-axis + */ + double GetRateZ() const; + + /** + * Gets the currently reported angle around the X-axis + */ + double GetAngleX() const; + + /** + * Gets the currently reported angle around the X-axis + */ + double GetAngleY() const; + + /** + * Gets the currently reported angle around the X-axis + */ + double GetAngleZ() const; + + /** + * Resets the gyro + */ + void Reset(); + + private: + hal::SimDevice m_simDevice; + hal::SimDouble m_simRateX; + hal::SimDouble m_simRateY; + hal::SimDouble m_simRateZ; + hal::SimDouble m_simAngleX; + hal::SimDouble m_simAngleY; + hal::SimDouble m_simAngleZ; + + double m_angleXOffset = 0; + double m_angleYOffset = 0; + double m_angleZOffset = 0; +}; + +} // namespace frc diff --git a/romiVendordep/src/main/native/include/frc/romi/RomiMotor.h b/romiVendordep/src/main/native/include/frc/romi/RomiMotor.h new file mode 100644 index 0000000000..4bb925056b --- /dev/null +++ b/romiVendordep/src/main/native/include/frc/romi/RomiMotor.h @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace frc { + +/** + * RomiMotor + * + * A general use PWM motor controller representing the motors on a Romi robot + */ +class RomiMotor : public PWMMotorController { + public: + /** + * Constructor for a RomiMotor. + * + * @param channel The PWM channel that the RomiMotor is attached to. + * 0 is left, 1 is right + */ + explicit RomiMotor(int channel); + + RomiMotor(RomiMotor&&) = default; + RomiMotor& operator=(RomiMotor&&) = default; +}; + +} // namespace frc diff --git a/romiVendordep/src/test/native/cpp/main.cpp b/romiVendordep/src/test/native/cpp/main.cpp new file mode 100644 index 0000000000..2d710be58f --- /dev/null +++ b/romiVendordep/src/test/native/cpp/main.cpp @@ -0,0 +1,10 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/settings.gradle b/settings.gradle index 19cfba147d..38617022ae 100644 --- a/settings.gradle +++ b/settings.gradle @@ -49,6 +49,8 @@ include 'simulation:halsim_xrp' include 'cameraserver' include 'cameraserver:multiCameraServer' include 'wpilibNewCommands' +include 'romiVendordep' +include 'xrpVendordep' include 'myRobot' include 'docs' include 'msvcruntime' diff --git a/xrpVendordep/CMakeLists.txt b/xrpVendordep/CMakeLists.txt new file mode 100644 index 0000000000..9ba681b29b --- /dev/null +++ b/xrpVendordep/CMakeLists.txt @@ -0,0 +1,59 @@ +project(xrpVendordep) + +include(SubDirList) +include(CompileWarnings) +include(AddTest) + +if (WITH_JAVA) + find_package(Java REQUIRED) + include(UseJava) + set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") + + file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) + add_jar(xrpVendordep_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME xrpVendordep) + + get_property(xrpVendordep_JAR_FILE TARGET xrpVendordep_jar PROPERTY JAR_FILE) + install(FILES ${xrpVendordep_JAR_FILE} DESTINATION "${java_lib_dest}") + + set_property(TARGET xrpVendordep_jar PROPERTY FOLDER "java") + + if (WITH_FLAT_INSTALL) + set (xrpVendordep_config_dir ${wpilib_dest}) + else() + set (xrpVendordep_config_dir share/xrpVendordep) + endif() + + install(FILES xrpVendordep-config.cmake DESTINATION ${xrpVendordep_config_dir}) +endif() + +file(GLOB_RECURSE xrpVendordep_native_src src/main/native/cpp/*.cpp) +add_library(xrpVendordep ${xrpVendordep_native_src}) +set_target_properties(xrpVendordep PROPERTIES DEBUG_POSTFIX "d") +set_property(TARGET xrpVendordep PROPERTY FOLDER "libraries") + +target_compile_features(xrpVendordep PUBLIC cxx_std_20) +wpilib_target_warnings(xrpVendordep) +target_link_libraries(xrpVendordep wpilibc) + +target_include_directories(xrpVendordep PUBLIC + $ + $) + +install(TARGETS xrpVendordep EXPORT xrpVendordep DESTINATION "${main_lib_dest}") +install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/xrpVendordep") + +if (FLAT_INSTALL_WPILIB) + set(xrpVendordep_config_dir ${wpilib_dest}) + else() + set(xrpVendordep_config_dir share/xrpVendordep) + endif() + + configure_file(xrpVendordep-config.cmake.in ${WPILIB_BINARY_DIR}/xrpVendordep-config.cmake) + install(FILES ${WPILIB_BINARY_DIR}/xrpVendordep-config.cmake DESTINATION ${xrpVendordep_config_dir}) + install(EXPORT xrpVendordep DESTINATION ${xrpVendordep_config_dir}) + + if (WITH_TESTS) + wpilib_add_test(xrpVendordep src/test/native/cpp) + target_include_directories(xrpVendordep_test PRIVATE src/test/native/include) + target_link_libraries(xrpVendordep_test xrpVendordep gmock_main) + endif() diff --git a/xrpVendordep/README.md b/xrpVendordep/README.md new file mode 100644 index 0000000000..0b3688cfa1 --- /dev/null +++ b/xrpVendordep/README.md @@ -0,0 +1,2 @@ +# XRP-vendordep +WPILib XRP Vendordep diff --git a/xrpVendordep/XRPVendordep.json b/xrpVendordep/XRPVendordep.json new file mode 100644 index 0000000000..0bb1b20c6c --- /dev/null +++ b/xrpVendordep/XRPVendordep.json @@ -0,0 +1,36 @@ +{ + "fileName": "XRPVendordep.json", + "name": "XRP-Vendordep", + "version": "1.0.0", + "uuid": "1571a1a5-ed3f-4f07-b7eb-b2beb17394e0", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.xrpVendordep", + "artifactId": "xrpVendordep-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.xrpVendordep", + "artifactId": "xrpVendordep-cpp", + "version": "vendordep", + "libName": "xrpVendordep", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/xrpVendordep/build.gradle b/xrpVendordep/build.gradle new file mode 100644 index 0000000000..3ee9d0acef --- /dev/null +++ b/xrpVendordep/build.gradle @@ -0,0 +1,98 @@ +ext { + nativeName = 'xrpVendordep' + devMain = 'edu.wpi.first.wpilibj.xrp.DevMain' +} + +evaluationDependsOn(":ntcore") +evaluationDependsOn(":cscore") +evaluationDependsOn(":hal") +evaluationDependsOn(":wpimath") +evaluationDependsOn(":wpilibc") +evaluationDependsOn(":cameraserver") +evaluationDependsOn(":wpilibj") + +apply from: "${rootDir}/shared/javacpp/setupBuild.gradle" + +dependencies { + implementation project(":wpiutil") + implementation project(":wpinet") + implementation project(":ntcore") + implementation project(":cscore") + implementation project(":hal") + implementation project(":wpimath") + implementation project(":wpilibj") +} + +nativeUtils.exportsConfigs { + // Main library is just default empty. This will export everything + xrpVendordep { + x64ExcludeSymbols = [ + '_CT??_R0?AV_System_error', + '_CT??_R0?AVexception', + '_CT??_R0?AVfailure', + '_CT??_R0?AVruntime_error', + '_CT??_R0?AVsystem_error', + '_CTA5?AVfailure', + '_TI5?AVfailure', + '_CT??_R0?AVout_of_range', + '_CTA3?AVout_of_range', + '_TI3?AVout_of_range', + '_CT??_R0?AVbad_cast' + ] + } +} + +model { + components {} + binaries { + all { + if (!it.buildable || !(it instanceof NativeBinarySpec)) { + return + } + lib project: ":wpilibc", library: "wpilibc", linkage: "shared" + project(":ntcore").addNtcoreDependency(it, "shared") + project(":hal").addHalDependency(it, "shared") + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' + lib project: ':wpinet', library: 'wpinet', linkage: 'shared' + lib project: ':wpimath', library: 'wpimath', linkage: 'shared' + + if (it.component.name == "${nativeName}Dev") { + project(":ntcore").addNtcoreJniDependency(it) + lib project: ":wpinet", library: "wpinetJNIShared", linkage: "shared" + lib project: ":wpiutil", library: "wpiutilJNIShared", linkage: "shared" + lib project: ":wpimath", library: "wpimathJNIShared", linkage: "shared" + project(":hal").addHalJniDependency(it) + } + + if (it instanceof GoogleTestTestSuiteBinarySpec) { + nativeUtils.useRequiredLibrary(it, "opencv_shared") + lib project: ":cscore", library: "cscore", linkage: "shared" + } + } + } + tasks { + def c = $.components + def found = false + def systemArch = getCurrentArch() + c.each { + if (it in NativeExecutableSpec && it.name == "${nativeName}Dev") { + it.binaries.each { + if (!found) { + def arch = it.targetPlatform.name + if (arch == systemArch) { + def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + "lib" + found = true + } + } + } + } + } + } +} + +test { + testLogging { + outputs.upToDateWhen {false} + showStandardStreams = true + } +} diff --git a/xrpVendordep/src/dev/java/edu/wpi/first/wpilibj/xrp/DevMain.java b/xrpVendordep/src/dev/java/edu/wpi/first/wpilibj/xrp/DevMain.java new file mode 100644 index 0000000000..9b981c5666 --- /dev/null +++ b/xrpVendordep/src/dev/java/edu/wpi/first/wpilibj/xrp/DevMain.java @@ -0,0 +1,21 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.xrp; + +import edu.wpi.first.hal.HALUtil; +import edu.wpi.first.networktables.NetworkTablesJNI; +import edu.wpi.first.util.RuntimeDetector; + +public final class DevMain { + /** Main entry point. */ + public static void main(String[] args) { + System.out.println("Hello World!"); + System.out.println(RuntimeDetector.getPlatformPath()); + System.out.println(NetworkTablesJNI.now()); + System.out.println(HALUtil.getHALRuntimeType()); + } + + private DevMain() {} +} diff --git a/xrpVendordep/src/dev/native/cpp/main.cpp b/xrpVendordep/src/dev/native/cpp/main.cpp new file mode 100644 index 0000000000..a3e363efca --- /dev/null +++ b/xrpVendordep/src/dev/native/cpp/main.cpp @@ -0,0 +1,5 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +int main() {} diff --git a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPGyro.java b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPGyro.java new file mode 100644 index 0000000000..16d70d4f00 --- /dev/null +++ b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPGyro.java @@ -0,0 +1,147 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.xrp; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimDouble; + +public class XRPGyro { + private final SimDevice m_simDevice; + private final SimDouble m_simRateX; + private final SimDouble m_simRateY; + private final SimDouble m_simRateZ; + private final SimDouble m_simAngleX; + private final SimDouble m_simAngleY; + private final SimDouble m_simAngleZ; + + private double m_angleXOffset; + private double m_angleYOffset; + private double m_angleZOffset; + + /** Create a new XRPGyro. */ + public XRPGyro() { + m_simDevice = SimDevice.create("Gyro:XRPGyro"); + if (m_simDevice != null) { + m_simDevice.createBoolean("init", Direction.kOutput, true); + m_simRateX = m_simDevice.createDouble("rate_x", Direction.kInput, 0.0); + m_simRateY = m_simDevice.createDouble("rate_y", Direction.kInput, 0.0); + m_simRateZ = m_simDevice.createDouble("rate_z", Direction.kInput, 0.0); + + m_simAngleX = m_simDevice.createDouble("angle_x", Direction.kInput, 0.0); + m_simAngleY = m_simDevice.createDouble("angle_y", Direction.kInput, 0.0); + m_simAngleZ = m_simDevice.createDouble("angle_z", Direction.kInput, 0.0); + } else { + m_simRateX = null; + m_simRateY = null; + m_simRateZ = null; + m_simAngleX = null; + m_simAngleY = null; + m_simAngleZ = null; + } + } + + /** + * Get the rate of turn in degrees-per-second around the X-axis. + * + * @return rate of turn in degrees-per-second + */ + public double getRateX() { + if (m_simRateX != null) { + return m_simRateX.get(); + } + + return 0.0; + } + + /** + * Get the rate of turn in degrees-per-second around the Y-axis. + * + * @return rate of turn in degrees-per-second + */ + public double getRateY() { + if (m_simRateY != null) { + return m_simRateY.get(); + } + + return 0.0; + } + + /** + * Get the rate of turn in degrees-per-second around the Z-axis. + * + * @return rate of turn in degrees-per-second + */ + public double getRateZ() { + if (m_simRateZ != null) { + return m_simRateZ.get(); + } + + return 0.0; + } + + /** + * Get the currently reported angle around the X-axis. + * + * @return current angle around X-axis in degrees + */ + public double getAngleX() { + if (m_simAngleX != null) { + return m_simAngleX.get() - m_angleXOffset; + } + + return 0.0; + } + + /** + * Get the currently reported angle around the X-axis. + * + * @return current angle around Y-axis in degrees + */ + public double getAngleY() { + if (m_simAngleY != null) { + return m_simAngleY.get() - m_angleYOffset; + } + + return 0.0; + } + + /** + * Get the currently reported angle around the Z-axis. + * + * @return current angle around Z-axis in degrees + */ + public double getAngleZ() { + if (m_simAngleZ != null) { + return m_simAngleZ.get() - m_angleZOffset; + } + + return 0.0; + } + + /** Reset the gyro angles to 0. */ + public void reset() { + if (m_simAngleX != null) { + m_angleXOffset = m_simAngleX.get(); + m_angleYOffset = m_simAngleY.get(); + m_angleZOffset = m_simAngleZ.get(); + } + } + + public double getAngle() { + return getAngleZ(); + } + + public double getRate() { + return getRateZ(); + } + + /** Close out the SimDevice. */ + public void close() { + if (m_simDevice != null) { + m_simDevice.close(); + } + } +} diff --git a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPMotor.java b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPMotor.java new file mode 100644 index 0000000000..957f9e7a3c --- /dev/null +++ b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPMotor.java @@ -0,0 +1,109 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.xrp; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import java.util.HashMap; +import java.util.HashSet; + +/** + * XRPMotor. + * + *

A SimDevice based motor controller representing the motors on an XRP robot + */ +public class XRPMotor implements MotorController { + private static HashMap s_simDeviceNameMap = new HashMap<>(); + private static HashSet s_registeredDevices = new HashSet<>(); + + private static void checkDeviceAllocation(int deviceNum) { + if (!s_simDeviceNameMap.containsKey(deviceNum)) { + throw new IllegalArgumentException("Invalid XRPMotor device number. Should be 0-3"); + } + + if (s_registeredDevices.contains(deviceNum)) { + throw new IllegalArgumentException("XRPMotor " + deviceNum + " already allocated"); + } + + s_registeredDevices.add(deviceNum); + } + + static { + s_simDeviceNameMap.put(0, "motorL"); + s_simDeviceNameMap.put(1, "motorR"); + s_simDeviceNameMap.put(2, "motor3"); + s_simDeviceNameMap.put(3, "motor4"); + } + + private final SimDouble m_simSpeed; + private final SimBoolean m_simInverted; + + /** XRPMotor. */ + public XRPMotor(int deviceNum) { + checkDeviceAllocation(deviceNum); + + // We want this to appear on the WS messages as type: "XRPMotor", device: + String simDeviceName = "XRPMotor:" + s_simDeviceNameMap.get(deviceNum); + SimDevice xrpMotorSimDevice = SimDevice.create(simDeviceName); + + if (xrpMotorSimDevice != null) { + xrpMotorSimDevice.createBoolean("init", Direction.kOutput, true); + m_simInverted = xrpMotorSimDevice.createBoolean("inverted", Direction.kInput, false); + m_simSpeed = xrpMotorSimDevice.createDouble("speed", Direction.kOutput, 0.0); + } else { + m_simInverted = null; + m_simSpeed = null; + } + } + + @Override + public void set(double speed) { + if (m_simSpeed != null) { + boolean invert = false; + if (m_simInverted != null) { + invert = m_simInverted.get(); + } + + m_simSpeed.set(invert ? -speed : speed); + } + } + + @Override + public double get() { + if (m_simSpeed != null) { + return m_simSpeed.get(); + } + + return 0.0; + } + + @Override + public void setInverted(boolean isInverted) { + if (m_simInverted != null) { + m_simInverted.set(isInverted); + } + } + + @Override + public boolean getInverted() { + if (m_simInverted != null) { + return m_simInverted.get(); + } + return false; + } + + @Override + public void disable() { + set(0.0); + } + + @Override + public void stopMotor() { + set(0.0); + } +} diff --git a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPOnBoardIO.java b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPOnBoardIO.java new file mode 100644 index 0000000000..a37d417e0a --- /dev/null +++ b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPOnBoardIO.java @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.xrp; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DigitalOutput; + +/** + * This class represents the onboard IO of the XRP Reference Robot. This includes the USER + * pushbutton and LED + */ +public class XRPOnBoardIO { + private final DigitalInput m_button = new DigitalInput(0); + private final DigitalOutput m_led = new DigitalOutput(1); + + /** Constructor. */ + public XRPOnBoardIO() { + // No need to do anything else. Unlike the Romi, there are no other configurable + // I/O ports + } + + /** + * Gets if the USER button is pressed. + * + * @return True if the USER button is currently pressed. + */ + public boolean getUserButtonPressed() { + return m_button.get(); + } + + /** + * Sets the onboard LED. + * + * @param value True to activate LED, false otherwise. + */ + public void setLed(boolean value) { + m_led.set(value); + } +} diff --git a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java new file mode 100644 index 0000000000..ac7231fd22 --- /dev/null +++ b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java @@ -0,0 +1,123 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.xrp; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimDouble; +import java.util.HashMap; +import java.util.HashSet; + +/** + * XRPServo. + * + *

A SimDevice based servo + */ +public class XRPServo { + private static HashMap s_simDeviceNameMap = new HashMap<>(); + private static HashSet s_registeredDevices = new HashSet<>(); + + private static void checkDeviceAllocation(int deviceNum) { + if (!s_simDeviceNameMap.containsKey(deviceNum)) { + throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-5"); + } + + if (s_registeredDevices.contains(deviceNum)) { + throw new IllegalArgumentException("XRPServo " + deviceNum + " already allocated"); + } + + s_registeredDevices.add(deviceNum); + } + + static { + s_simDeviceNameMap.put(4, "servo1"); + s_simDeviceNameMap.put(5, "servo2"); + } + + private final SimDouble m_simPosition; + + /** XRPServo. */ + public XRPServo(int deviceNum) { + checkDeviceAllocation(deviceNum); + + // We want this to appear on WS as type: "XRPServo", device: + String simDeviceName = "XRPServo:" + s_simDeviceNameMap.get(deviceNum); + SimDevice xrpServoSimDevice = SimDevice.create(simDeviceName); + + if (xrpServoSimDevice != null) { + xrpServoSimDevice.createBoolean("init", Direction.kOutput, true); + // This should mimic PWM position [0.0, 1.0] + m_simPosition = xrpServoSimDevice.createDouble("position", Direction.kOutput, 0.5); + } else { + m_simPosition = null; + } + } + + /** + * Set the servo angle. + * + * @param angle Desired angle in degrees + */ + public void setAngle(double angle) { + if (angle < 0.0) { + angle = 0.0; + } + + if (angle > 180.0) { + angle = 180.0; + } + + double pos = angle / 180.0; + + if (m_simPosition != null) { + m_simPosition.set(pos); + } + } + + /** + * Get the servo angle. + * + * @return Current servo angle + */ + public double getAngle() { + if (m_simPosition != null) { + return m_simPosition.get() * 180.0; + } + + return 90.0; + } + + /** + * Set the servo position. + * + * @param pos Desired position (Between 0.0 and 1.0) + */ + public void setPosition(double pos) { + if (pos < 0.0) { + pos = 0.0; + } + + if (pos > 1.0) { + pos = 1.0; + } + + if (m_simPosition != null) { + m_simPosition.set(pos); + } + } + + /** + * Get the servo position. + * + * @return Current servo position + */ + public double getPosition() { + if (m_simPosition != null) { + return m_simPosition.get(); + } + + return 0.5; + } +} diff --git a/xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp b/xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp new file mode 100644 index 0000000000..46bf6180f5 --- /dev/null +++ b/xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp @@ -0,0 +1,89 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/xrp/XRPGyro.h" + +using namespace frc; + +XRPGyro::XRPGyro() : m_simDevice("Gyro:XRPGyro") { + if (m_simDevice) { + m_simDevice.CreateBoolean("init", hal::SimDevice::kOutput, true); + m_simRateX = + m_simDevice.CreateDouble("rate_x", hal::SimDevice::kInput, 0.0); + m_simRateY = + m_simDevice.CreateDouble("rate_y", hal::SimDevice::kInput, 0.0); + m_simRateZ = + m_simDevice.CreateDouble("rate_z", hal::SimDevice::kInput, 0.0); + m_simAngleX = + m_simDevice.CreateDouble("angle_x", hal::SimDevice::kInput, 0.0); + m_simAngleY = + m_simDevice.CreateDouble("angle_y", hal::SimDevice::kInput, 0.0); + m_simAngleZ = + m_simDevice.CreateDouble("angle_z", hal::SimDevice::kInput, 0.0); + } +} + +double XRPGyro::GetAngle() const { + return GetAngleZ(); +} + +double XRPGyro::GetRate() const { + return GetRateZ(); +} + +double XRPGyro::GetRateX() const { + if (m_simRateX) { + return m_simRateX.Get(); + } + + return 0.0; +} + +double XRPGyro::GetRateY() const { + if (m_simRateY) { + return m_simRateY.Get(); + } + + return 0.0; +} + +double XRPGyro::GetRateZ() const { + if (m_simRateZ) { + return m_simRateZ.Get(); + } + + return 0.0; +} + +double XRPGyro::GetAngleX() const { + if (m_simAngleX) { + return m_simAngleX.Get() - m_angleXOffset; + } + + return 0.0; +} + +double XRPGyro::GetAngleY() const { + if (m_simAngleY) { + return m_simAngleY.Get() - m_angleYOffset; + } + + return 0.0; +} + +double XRPGyro::GetAngleZ() const { + if (m_simAngleZ) { + return m_simAngleZ.Get() - m_angleZOffset; + } + + return 0.0; +} + +void XRPGyro::Reset() { + if (m_simAngleX) { + m_angleXOffset = m_simAngleX.Get(); + m_angleYOffset = m_simAngleY.Get(); + m_angleZOffset = m_simAngleZ.Get(); + } +} diff --git a/xrpVendordep/src/main/native/cpp/xrp/XRPMotor.cpp b/xrpVendordep/src/main/native/cpp/xrp/XRPMotor.cpp new file mode 100644 index 0000000000..02d1d58791 --- /dev/null +++ b/xrpVendordep/src/main/native/cpp/xrp/XRPMotor.cpp @@ -0,0 +1,88 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/xrp/XRPMotor.h" + +#include + +using namespace frc; + +std::map XRPMotor::s_simDeviceMap = { + {0, "motorL"}, {1, "motorR"}, {2, "motor3"}, {3, "motor4"}}; + +std::set XRPMotor::s_registeredDevices = {}; + +void XRPMotor::CheckDeviceAllocation(int deviceNum) { + if (s_simDeviceMap.count(deviceNum) == 0) { + throw FRC_MakeError(frc::err::ChannelIndexOutOfRange, "Channel {}", + deviceNum); + } + + if (s_registeredDevices.count(deviceNum) > 0) { + throw FRC_MakeError(frc::err::ResourceAlreadyAllocated, "Channel {}", + deviceNum); + } + + s_registeredDevices.insert(deviceNum); +} + +XRPMotor::XRPMotor(int deviceNum) { + CheckDeviceAllocation(deviceNum); + + m_deviceName = "XRPMotor:" + s_simDeviceMap[deviceNum]; + m_simDevice = hal::SimDevice(m_deviceName.c_str()); + + if (m_simDevice) { + m_simDevice.CreateBoolean("init", hal::SimDevice::kOutput, true); + m_simInverted = + m_simDevice.CreateBoolean("inverted", hal::SimDevice::kInput, false); + m_simSpeed = + m_simDevice.CreateDouble("speed", hal::SimDevice::kOutput, 0.0); + } +} + +void XRPMotor::Set(double speed) { + if (m_simSpeed) { + bool invert = false; + if (m_simInverted) { + invert = m_simInverted.Get(); + } + + m_simSpeed.Set(invert ? -speed : speed); + } +} + +double XRPMotor::Get() const { + if (m_simSpeed) { + return m_simSpeed.Get(); + } + + return 0.0; +} + +void XRPMotor::SetInverted(bool isInverted) { + if (m_simInverted) { + m_simInverted.Set(isInverted); + } +} + +bool XRPMotor::GetInverted() const { + if (m_simInverted) { + return m_simInverted.Get(); + } + + return false; +} + +void XRPMotor::Disable() { + Set(0.0); +} + +void XRPMotor::StopMotor() { + Set(0.0); +} + +std::string XRPMotor::GetDescription() const { + return m_deviceName; +} diff --git a/xrpVendordep/src/main/native/cpp/xrp/XRPOnBoardIO.cpp b/xrpVendordep/src/main/native/cpp/xrp/XRPOnBoardIO.cpp new file mode 100644 index 0000000000..eb1ab6122b --- /dev/null +++ b/xrpVendordep/src/main/native/cpp/xrp/XRPOnBoardIO.cpp @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/xrp/XRPOnBoardIO.h" + +#include +#include +#include +#include + +using namespace frc; + +bool XRPOnBoardIO::GetUserButtonPressed() { + return m_userButton.Get(); +} + +void XRPOnBoardIO::SetLed(bool value) { + m_led.Set(value); +} diff --git a/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp b/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp new file mode 100644 index 0000000000..01635cdcc6 --- /dev/null +++ b/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp @@ -0,0 +1,87 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/xrp/XRPServo.h" + +#include + +using namespace frc; + +std::map XRPServo::s_simDeviceMap = {{4, "servo1"}, + {5, "servo2"}}; + +std::set XRPServo::s_registeredDevices = {}; + +void XRPServo::CheckDeviceAllocation(int deviceNum) { + if (s_simDeviceMap.count(deviceNum) == 0) { + throw FRC_MakeError(frc::err::ChannelIndexOutOfRange, "Channel {}", + deviceNum); + } + + if (s_registeredDevices.count(deviceNum) > 0) { + throw FRC_MakeError(frc::err::ResourceAlreadyAllocated, "Channel {}", + deviceNum); + } + + s_registeredDevices.insert(deviceNum); +} + +XRPServo::XRPServo(int deviceNum) { + CheckDeviceAllocation(deviceNum); + + m_deviceName = "XRPServo:" + s_simDeviceMap[deviceNum]; + m_simDevice = hal::SimDevice(m_deviceName.c_str()); + + if (m_simDevice) { + m_simDevice.CreateBoolean("init", hal::SimDevice::kOutput, true); + m_simPosition = + m_simDevice.CreateDouble("position", hal::SimDevice::kOutput, 0.5); + } +} + +void XRPServo::SetAngle(double angleDegrees) { + if (angleDegrees < 0.0) { + angleDegrees = 0.0; + } + + if (angleDegrees > 180.0) { + angleDegrees = 180.0; + } + + double pos = (angleDegrees / 180.0); + + if (m_simPosition) { + m_simPosition.Set(pos); + } +} + +double XRPServo::GetAngle() const { + if (m_simPosition) { + return m_simPosition.Get() * 180.0; + } + + return 90.0; +} + +void XRPServo::SetPosition(double pos) { + if (pos < 0.0) { + pos = 0.0; + } + + if (pos > 1.0) { + pos = 1.0; + } + + if (m_simPosition) { + m_simPosition.Set(pos); + } +} + +double XRPServo::GetPosition() const { + if (m_simPosition) { + return m_simPosition.Get(); + } + + return 0.5; +} diff --git a/xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h b/xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h new file mode 100644 index 0000000000..81b1610872 --- /dev/null +++ b/xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h @@ -0,0 +1,91 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace frc { + +/** + * Use a rate gyro to return the robots heading relative to a starting position. + * + * This class is for the XRP onboard gyro, and will only work in + * simulation/XRP mode. Only one instance of a XRPGyro is supported. + */ +class XRPGyro { + public: + XRPGyro(); + + /** + * Return the actual angle in degrees that the robot is currently facing. + * + * The angle is based on integration of the returned rate form the gyro. + * The angle is continuous, that is, it will continue from 360->361 degrees. + * This allows algorithms that wouldn't want to see a discontinuity in the + * gyro output as it sweeps from 360 to 0 on the second time around. + * + * @return the current heading of the robot in degrees. + */ + double GetAngle() const; + + /** + * Return the rate of rotation of the gyro + * + * The rate is based on the most recent reading of the gyro. + * + * @return the current rate in degrees per second + */ + double GetRate() const; + + /** + * Gets the rate of turn in degrees-per-second around the X-axis + */ + double GetRateX() const; + + /** + * Gets the rate of turn in degrees-per-second around the Y-axis + */ + double GetRateY() const; + + /** + * Gets the rate of turn in degrees-per-second around the Z-axis + */ + double GetRateZ() const; + + /** + * Gets the currently reported angle around the X-axis + */ + double GetAngleX() const; + + /** + * Gets the currently reported angle around the X-axis + */ + double GetAngleY() const; + + /** + * Gets the currently reported angle around the X-axis + */ + double GetAngleZ() const; + + /** + * Resets the gyro + */ + void Reset(); + + private: + hal::SimDevice m_simDevice; + hal::SimDouble m_simRateX; + hal::SimDouble m_simRateY; + hal::SimDouble m_simRateZ; + hal::SimDouble m_simAngleX; + hal::SimDouble m_simAngleY; + hal::SimDouble m_simAngleZ; + + double m_angleXOffset = 0; + double m_angleYOffset = 0; + double m_angleZOffset = 0; +}; + +} // namespace frc diff --git a/xrpVendordep/src/main/native/include/frc/xrp/XRPMotor.h b/xrpVendordep/src/main/native/include/frc/xrp/XRPMotor.h new file mode 100644 index 0000000000..c174cbf7e3 --- /dev/null +++ b/xrpVendordep/src/main/native/include/frc/xrp/XRPMotor.h @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include +#include +#include + +#include + +namespace frc { + +class XRPMotor : public frc::MotorController, public frc::MotorSafety { + public: + explicit XRPMotor(int deviceNum); + + void Set(double value) override; + double Get() const override; + + void SetInverted(bool isInverted) override; + bool GetInverted() const override; + + void Disable() override; + + void StopMotor() override; + std::string GetDescription() const override; + + private: + hal::SimDevice m_simDevice; + hal::SimDouble m_simSpeed; + hal::SimBoolean m_simInverted; + + std::string m_deviceName; + + static std::map s_simDeviceMap; + static std::set s_registeredDevices; + + static void CheckDeviceAllocation(int deviceNum); +}; + +} // namespace frc diff --git a/xrpVendordep/src/main/native/include/frc/xrp/XRPOnBoardIO.h b/xrpVendordep/src/main/native/include/frc/xrp/XRPOnBoardIO.h new file mode 100644 index 0000000000..4b97870f2c --- /dev/null +++ b/xrpVendordep/src/main/native/include/frc/xrp/XRPOnBoardIO.h @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include + +#include + +namespace frc { + +/** + * This class represents the onboard IO of the XRP + * reference robot. This the USER push button and + * LED. + * + *

DIO 0 - USER Button (input only) + * DIO 1 - LED (output only) + */ +class XRPOnBoardIO { + public: + XRPOnBoardIO() {} // No need to do anything. No configurable IO + + static constexpr auto kMessageInterval = 1_s; + units::second_t m_nextMessageTime = 0_s; + + /** + * Gets if the USER button is pressed. + */ + bool GetUserButtonPressed(); + + /** + * Sets the yellow LED. + */ + void SetLed(bool value); + + private: + frc::DigitalInput m_userButton{0}; + frc::DigitalOutput m_led{1}; +}; + +} // namespace frc diff --git a/xrpVendordep/src/main/native/include/frc/xrp/XRPServo.h b/xrpVendordep/src/main/native/include/frc/xrp/XRPServo.h new file mode 100644 index 0000000000..f452a5f514 --- /dev/null +++ b/xrpVendordep/src/main/native/include/frc/xrp/XRPServo.h @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include + +#include + +namespace frc { + +class XRPServo { + public: + explicit XRPServo(int deviceNum); + + void SetAngle(double angleDegrees); + double GetAngle() const; + + void SetPosition(double position); + double GetPosition() const; + + private: + hal::SimDevice m_simDevice; + hal::SimDouble m_simPosition; + + std::string m_deviceName; + + static std::map s_simDeviceMap; + static std::set s_registeredDevices; + + static void CheckDeviceAllocation(int deviceNum); +}; + +} // namespace frc diff --git a/xrpVendordep/src/test/native/cpp/main.cpp b/xrpVendordep/src/test/native/cpp/main.cpp new file mode 100644 index 0000000000..2d710be58f --- /dev/null +++ b/xrpVendordep/src/test/native/cpp/main.cpp @@ -0,0 +1,10 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/xrpVendordep/xrpVendordep-config.cmake.in b/xrpVendordep/xrpVendordep-config.cmake.in new file mode 100644 index 0000000000..ad06e4ce2e --- /dev/null +++ b/xrpVendordep/xrpVendordep-config.cmake.in @@ -0,0 +1,11 @@ +include(CMakeFindDependencyMacro) + @WPIUTIL_DEP_REPLACE@ + @NTCORE_DEP_REPLACE@ + @CSCORE_DEP_REPLACE@ + @CAMERASERVER_DEP_REPLACE@ + @HAL_DEP_REPLACE@ + @WPILIBC_DEP_REPLACE@ + @WPIMATH_DEP_REPLACE@ + + @FILENAME_DEP_REPLACE@ + include(${SELF_DIR}/xrpVendordep.cmake)