Squashed 'third_party/allwpilib_2019/' changes from 936627bd9..a3f7420da
e20d96ea4 Use __has_include for WPILib.h (#2164)
a76d006a0 Update native-utils to 2020.7.2 (#2161)
24c031d69 Increase SPI auto byte count to allow 32 bytes to be sent (#2163)
6b4eecf5f Add hidden functions to get the SPI system and SPI DMA (#2162)
ccdd0fbdb Add TrapezoidProfile external PID examples (#2131)
5c6b8a0f4 Replace std::is_pod_v with std::is_standard_layout_v (#2159)
67d2fed68 Add DutyCycleEncoder channel constructor (#2158)
d8f11eb14 Hardcode channels for LSB weight (#2153)
b2ae75acd Add way to disable "no extensions found" message (#2134)
4f951789f Build testbench tests online inorder to improve speed (#2144)
005c4c5be Try catch around task dependencies to fix loading in editor (#2156)
34f6b3f4c Fix C++ RamseteCommand param doxygen (#2157)
f7a93713f Fix up templated TrapezoidProfile classes (#2151)
8c2ff94d7 Rename MathUtils to MathUtil for consistency with other util classes (#2155)
d003ec2dc Update to 2020v9 image (#2154)
8e7cc3fe7 Add user-friendly SimDeviceSim wrappers (#2150)
6c8f6cf47 Fix bug in cubic and quintic hermetic spline generation (#2139)
e37ecd33a Sim GUI: Add support for LED displays (#2138)
57c5523d6 Fix documentation in RamseteCommand (#2149)
7b9c6ebc2 Fix wpiutilJNIShared linkage typo in wpilibj (#2143)
9a515c80f Template C++ LinearFilter to work with unit types (#2142)
5b73c17f2 Remove encoder velocities methods in DifferentialDriveOdometry (#2147)
b8c102426 Fix PS3Eye VID and PID (#2146)
2622c6c29 Add default values for b and zeta in RamseteController (#2145)
f66ae5999 Add HSV helpers to AddressableLED (#2135)
5e97c81d8 Add MedianFilter class for moving-window median (#2136)
f79b7a058 Remove unnecessary constructor arg for LinearFilter's circular buffers (#2140)
e49494830 Replace Jenkins with Azure agent (#1914)
b67d049ac Check status of PDP CAN reads (#2126)
70102a60b SendableRegistry.foreachLiveWindow: Prevent concurrent modification (#2129)
6dcd2b0e2 Improve various subsystem APIs (#2130)
ce3973435 HAL_CAN_ReceiveMessage: return MessageNotFound if no callbacks registered (#2133)
3fcfc8ea7 Fix double disable segfaulting interrupts (#2132)
6ceafe3cd Fix class reference for logger (#2123)
b058dcf64 Catch exceptions generated by OpenCV in cscore JNI (#2118)
0b9307fdf Remove unused parts of .styleguide files (#2119)
39be812b2 Fix C++ ArmFeedforward (#2120)
21e957ee4 Add DifferentialDrive voltage constraint (#2075)
e0bc97f66 Add ProfiledPIDSubsystem example (#2076)
3df44c874 Remove Rotation2d.h wpi/math include (#2117)
a58dbec8a Add holonomic follower examples (#2052)
9a8067465 Fix incomplete .styleguide (#2113)
ffa4b907c Fix C++ floating point literal formatting (#2114)
3d1ca856b Fix missing typename and return type (#2115)
5f85457a9 Add usage reporting for AddressableLED (#2108)
4ebae1712 Enforce leading/trailing zeros in Java numeric constants (#2105)
fa85fbfc1 Template C++ TrapezoidProfile and ProfiledPIDController on units (#2109)
f62e23f1a Add Doxygen for new HAL interfaces (#2110)
5443fdabc Directly construct PWM port from HAL, avoid wpilib PWM object (#2106)
c0e36df9d Standardize on PWMVictorSPX in examples (#2104)
8c4d9f541 Add TrapezoidProfileSubsystem (#2077)
45201d15f Add encoder distance overload to DifferentialDriveOdometry (#2096)
845aba33f Make feedforward classes constexpr (#2103)
500c43fb8 Add examples for DMA, DutyCycle, DutyCycleEncoder and AddressableLED (#2100)
589162811 Use DifferentialDriveWheelSpeeds in RamseteCommand ctor (#2091)
b37b68daa Add JRE deployment to MyRobot Deploy (#2099)
0e83c65d2 Fix small logic error in ParallelDeadlineGroup (#2095)
6f6c6da9f Updates to addressable LED (#2098)
1894219ef Fix devmain package in commands (#2097)
77a9949bb Add AddressableLED simulation GUI support
a4c9e4ec2 Add AddressableLED simulation support
8ed205907 Add AddressableLED (#2092)
59507b12d Bump to 2020 v7 image (#2086)
5d39bf806 Make halsimgui::DrawLEDs() values parameter const (#2088)
841ef91c0 Use gyro angle instead of robot angle for odometry (#2081)
1b66ead49 Use standard constant for pi instead of 3.14 (#2084)
db2c3dddd Use DMA Global Number for DMA Index (#2085)
82b2170fe Add DMA support to HAL and WPILibC (#2080)
8280b7e3a Add DutyCycleEncoder and AnalogEncoder (#2040)
551096006 Use kNumSystems for DutyCycle count in Ports (#2083)
df1065218 Remove release configs of deploy in MyRobot (#2082)
bf5388393 Add deploy options to myRobot (#2079)
b7bc1ea74 Update to 2020v6 image (#2078)
708009cd2 Update to gradle 6.0 (#2074)
3cce61b89 Add SmartDashboard::GetEntry function in C++ (#2064)
565e1f3e7 Fix spelling in MecanumDriveOdometry docs (#2072)
1853f7b6b Add timing window to simulation GUI
c5a049712 Add simulation pause/resume/step support
f5446c740 Add Notifier HALSIM access
3e049e02f Add name to HAL Notifier
2da64d15f Make usage reporting enums type match (#2069)
f04d95e50 Make FRCUsageReporting.h C-compatible (#2070)
d748c67a5 Generate docs for command libraries and fix doclint enable (#2071)
55a7f2b4a Add template for old command-based style (#2031)
486fa9c69 Add XboxController examples for arcade and tank drive (#2058)
e3dd1c5d7 Fix small bug in SplineHelper (#2061)
7dc7c71b5 Add feedforward components (#2045)
5f33d6af1 Fix ProfiledPIDSubsystem parameter name (#2017)
94843adb8 Standardize documentation of Speed Controllers bounds (#2043)
9bcff37b9 Add HAL specific version of wpi_setError (#2055)
326aecc9a Add error message for CAN Write Overrun (#2062)
00228678d Add requirements param to more Command APIs (#2059)
ff39a96ce Make DigitalOutput a DigitalSource (#2054)
5ccad2e8a Fix frc2::Timer returning incorrect timestamp values (#2057)
629e95776 Add VendorDeps JSON files for command libraries (#2048)
6858a57f7 Make notifier command tests a lot more lenient (#2056)
0ebe32823 Fix MyRobotStatic accidentally linking to shared command libs (#2046)
384d00f9e Fix various duty cycle bugs (#2047)
1f6850adf Add CAN Manufacturer for Studica (#2050)
7508aada9 Add ability to end startCompetition() main loop (#2032)
f5b4be16d Old C++ Command: Make GetName et al public (#2042)
e6f5c93ab Clean up new C++ commands (#2027)
39f46ceab Don't allow rising and falling values to be read from AnalogTrigger (#2039)
d93aa2b6b Add missing lock in FRCDriverStation (#2034)
114ddaf81 Fix duplicate encoders in examples (#2033)
f22d0961e Sim GUI: Add duty cycle support
3262c2bad Sim GUI: Use new multi-channel PDP getter function
96d40192a Revert accidental change to MyRobot.cpp (#2029)
ed30d5d40 Add JSON support for Trajectories (#2025)
2b6811edd Fix null pointer dereference in C++ CommandScheduler (#2023)
1d695a166 Add FPGA Duty Cycle support (#1987)
509819d83 Split the two command implementations into separate libraries (#2012)
2ad15cae1 Add multi PDP getter and sim PCM/PDP multi arg functions (#2014)
931b8ceef Add new usage reporting types from 2020v5 (#2026)
0b3821eba Change files with CRLF line endings to LF (#2022)
6f159d142 Add way to atomically check for new data, and wait otherwise (#2015)
a769f1f22 Fix bug in RamseteCommand (using degrees instead of radians) (#2020)
c5186d815 Clean up PIDCommand (#2010)
9ebd23d61 Add setVoltage method to SpeedController (#1997)
f6e311ef8 Fix SplineHelper bug (#2018)
f33bd9f05 Fix NPE in RamseteCommand (#2019)
1c1e0c9a6 Add HAL_SetAllSolenoids to sim (#2004)
ea9bb651a Remove accidental OpenCV link from wpilibc shared library (#2013)
cc0742518 Change command decorators to return implementation (#2007)
16b34cce2 Remove IterativeRobot templates (#2011)
669127e49 Update intellisense to work with Beta 2020 code (#2008)
9dc30797e Fix usage reporting indices (#2009)
f6b844ea3 Move HAL Interrupt struct to anonymous namespace (#2003)
a72f80991 Add extern C to DigitalGlitchFilterJNI (#2002)
916596cb0 Fix invalid examples json, add validator (#2001)
5509a8e96 Use constexpr for all example constants
0be6b6475 Use constexpr for DifferentialDriveKinematics
Change-Id: I1416646cbab487ad8021830215766d8ec7f24ddc
git-subtree-dir: third_party/allwpilib_2019
git-subtree-split: a3f7420dab7a104c27a0c3bf0872c999c98fd9a9
diff --git a/wpilibjExamples/build.gradle b/wpilibjExamples/build.gradle
index 7b65086..651d8e5 100644
--- a/wpilibjExamples/build.gradle
+++ b/wpilibjExamples/build.gradle
@@ -11,13 +11,15 @@
dependencies {
- compile project(':wpilibj')
+ implementation project(':wpilibj')
- compile project(':hal')
- compile project(':wpiutil')
- compile project(':ntcore')
- compile project(':cscore')
- compile project(':cameraserver')
+ implementation project(':hal')
+ implementation project(':wpiutil')
+ implementation project(':ntcore')
+ implementation project(':cscore')
+ implementation project(':cameraserver')
+ implementation project(':wpilibOldCommands')
+ implementation project(':wpilibNewCommands')
}
if (!project.hasProperty('skipPMD')) {
@@ -48,4 +50,7 @@
commandFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/commands/commands.json")
}
+ext {
+ isCppCommands = false
+}
apply from: "${rootDir}/shared/examplecheck.gradle"
diff --git a/wpilibjExamples/publish.gradle b/wpilibjExamples/publish.gradle
index a2c0746..a5ad1c6 100644
--- a/wpilibjExamples/publish.gradle
+++ b/wpilibjExamples/publish.gradle
@@ -12,8 +12,8 @@
def outputsFolder = file("$project.buildDir/outputs")
task javaExamplesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = examplesZipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = examplesZipBaseName
from(licenseFile) {
into '/'
@@ -25,8 +25,8 @@
}
task javaTemplatesZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = templatesZipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = templatesZipBaseName
from(licenseFile) {
into '/'
@@ -38,8 +38,8 @@
}
task javaCommandsZip(type: Zip) {
- destinationDir = outputsFolder
- baseName = commandsZipBaseName
+ destinationDirectory = outputsFolder
+ archiveBaseName = commandsZipBaseName
from(licenseFile) {
into '/'
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java
index d7f6e26..2feaca2 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java
@@ -1,29 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.iterative;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
- */
-public final class Main {
- private Main() {
- }
-
- /**
- * Main initialization function. Do not perform any initialization here.
- *
- * <p>If you change your main robot class, change the parameter type.
- */
- public static void main(String... args) {
- RobotBase.startRobot(Robot::new);
- }
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.addressableled;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+ private Main() {
+ }
+
+ /**
+ * Main initialization function. Do not perform any initialization here.
+ *
+ * <p>If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
new file mode 100644
index 0000000..b89365b
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.addressableled;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+public class Robot extends TimedRobot {
+ private AddressableLED m_led;
+ private AddressableLEDBuffer m_ledBuffer;
+ // Store what the last hue of the first pixel is
+ private int m_rainbowFirstPixelHue;
+
+ @Override
+ public void robotInit() {
+ // PWM port 0
+ // Must be a PWM header, not MXP or DIO
+ m_led = new AddressableLED(0);
+
+ // Reuse buffer
+ // Default to a length of 60, start empty output
+ // Length is expensive to set, so only set it once, then just update data
+ m_ledBuffer = new AddressableLEDBuffer(60);
+ m_led.setLength(m_ledBuffer.getLength());
+
+ // Set the data
+ m_led.setData(m_ledBuffer);
+ m_led.start();
+ }
+
+ @Override
+ public void robotPeriodic() {
+ // Fill the buffer with a rainbow
+ rainbow();
+ // Set the LEDs
+ m_led.setData(m_ledBuffer);
+ }
+
+ private void rainbow() {
+ // For every pixel
+ for (var i = 0; i < m_ledBuffer.getLength(); i++) {
+ // Calculate the hue - hue is easier for rainbows because the color
+ // shape is a circle so only one value needs to precess
+ final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_ledBuffer.getLength())) % 180;
+ // Set the value
+ m_ledBuffer.setHSV(i, hue, 255, 128);
+ }
+ // Increase by to make the rainbow "move"
+ m_rainbowFirstPixelHue += 3;
+ // Check bounds
+ m_rainbowFirstPixelHue %= 180;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java
index d7f6e26..fb5c2ab 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.iterative;
+package edu.wpi.first.wpilibj.examples.arcadedrivexboxcontroller;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
new file mode 100644
index 0000000..915d77e
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.arcadedrivexboxcontroller;
+
+import edu.wpi.first.wpilibj.GenericHID.Hand;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with split arcade steering and an Xbox controller.
+ */
+public class Robot extends TimedRobot {
+ private final PWMVictorSPX m_leftMotor = new PWMVictorSPX(0);
+ private final PWMVictorSPX m_rightMotor = new PWMVictorSPX(1);
+ private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final XboxController m_driverController = new XboxController(0);
+
+ @Override
+ public void teleopPeriodic() {
+ // Drive with split arcade drive.
+ // That means that the Y axis of the left stick moves forward
+ // and backward, and the X of the right stick turns left and right.
+ m_robotDrive.arcadeDrive(m_driverController.getY(Hand.kLeft),
+ m_driverController.getX(Hand.kRight));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java
new file mode 100644
index 0000000..fda1290
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armbot;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+ public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final boolean kLeftEncoderReversed = false;
+ public static final boolean kRightEncoderReversed = true;
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterInches = 6;
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+ }
+
+ public static final class ArmConstants {
+ public static final int kMotorPort = 4;
+
+ public static final double kP = 1;
+
+ // These are fake gains; in actuality these must be determined individually for each robot
+ public static final double kSVolts = 1;
+ public static final double kCosVolts = 1;
+ public static final double kVVoltSecondPerRad = 0.5;
+ public static final double kAVoltSecondSquaredPerRad = 0.1;
+
+ public static final double kMaxVelocityRadPerSecond = 3;
+ public static final double kMaxAccelerationRadPerSecSquared = 10;
+
+ public static final int[] kEncoderPorts = new int[]{4, 5};
+ public static final int kEncoderPPR = 256;
+ public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR;
+
+ // The offset of the arm from the horizontal in its neutral position,
+ // measured from the horizontal
+ public static final double kArmOffsetRads = 0.5;
+ }
+
+ public static final class AutoConstants {
+ public static final double kAutoTimeoutSeconds = 12;
+ public static final double kAutoShootTimeSeconds = 7;
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
similarity index 76%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
index d7f6e26..681a38e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.iterative;
+package edu.wpi.first.wpilibj.examples.armbot;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
new file mode 100644
index 0000000..a171fc6
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armbot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
new file mode 100644
index 0000000..64f44d6
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armbot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+ private final ArmSubsystem m_robotArm = new ArmSubsystem();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive
+ .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link JoystickButton}.
+ */
+ private void configureButtonBindings() {
+
+ // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
+ new JoystickButton(m_driverController, Button.kA.value)
+ .whenPressed(() -> m_robotArm.setGoal(2), m_robotArm);
+
+ // Move the arm to neutral position when the 'B' button is pressed.
+ new JoystickButton(m_driverController, Button.kB.value)
+ .whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm);
+
+ // Drive at half speed when the bumper is held
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
+ .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return new InstantCommand();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
new file mode 100644
index 0000000..7d549e5
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armbot.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.controller.ArmFeedforward;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kAVoltSecondSquaredPerRad;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kArmOffsetRads;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kCosVolts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMaxAccelerationRadPerSecSquared;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMaxVelocityRadPerSecond;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMotorPort;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kP;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kSVolts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kVVoltSecondPerRad;
+
+/**
+ * A robot arm subsystem that moves with a motion profile.
+ */
+public class ArmSubsystem extends ProfiledPIDSubsystem {
+ private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort);
+ private final Encoder m_encoder = new Encoder(kEncoderPorts[0], kEncoderPorts[1]);
+ private final ArmFeedforward m_feedforward =
+ new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad);
+
+ /**
+ * Create a new ArmSubsystem.
+ */
+ public ArmSubsystem() {
+ super(new ProfiledPIDController(
+ kP,
+ 0,
+ 0,
+ new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond,
+ kMaxAccelerationRadPerSecSquared)));
+ m_encoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ // Start arm at rest in neutral position
+ setGoal(kArmOffsetRads);
+ }
+
+ @Override
+ public void useOutput(double output, TrapezoidProfile.State setpoint) {
+ // Calculate the feedforward from the sepoint
+ double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
+ // Add the feedforward to the PID output to get the motor output
+ m_motor.setVoltage(output + feedforward);
+ }
+
+ @Override
+ public double getMeasurement() {
+ return m_encoder.getDistance() + kArmOffsetRads;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..a22079e
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armbot.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final SpeedControllerGroup m_leftMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+ new PWMVictorSPX(kLeftMotor2Port));
+
+ // The motors on the right side of the drive.
+ private final SpeedControllerGroup m_rightMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+ new PWMVictorSPX(kRightMotor2Port));
+
+ // The robot's drive
+ private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+ // The left-side drive encoder
+ private final Encoder m_leftEncoder =
+ new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+ // The right-side drive encoder
+ private final Encoder m_rightEncoder =
+ new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ // Sets the distance per pulse for the encoders
+ m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ }
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public void arcadeDrive(double fwd, double rot) {
+ m_drive.arcadeDrive(fwd, rot);
+ }
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ /**
+ * Gets the average distance of the two encoders.
+ *
+ * @return the average of the two encoder readings
+ */
+ public double getAverageEncoderDistance() {
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
+ }
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ public Encoder getLeftEncoder() {
+ return m_leftEncoder;
+ }
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ public Encoder getRightEncoder() {
+ return m_rightEncoder;
+ }
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java
new file mode 100644
index 0000000..baa15b6
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armbotoffboard;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+ public static final int[] kRightEncoderPorts = new int[]{2, 3};
+ public static final boolean kLeftEncoderReversed = false;
+ public static final boolean kRightEncoderReversed = true;
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterInches = 6;
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+ }
+
+ public static final class ArmConstants {
+ public static final int kMotorPort = 4;
+
+ public static final double kP = 1;
+
+ // These are fake gains; in actuality these must be determined individually for each robot
+ public static final double kSVolts = 1;
+ public static final double kCosVolts = 1;
+ public static final double kVVoltSecondPerRad = 0.5;
+ public static final double kAVoltSecondSquaredPerRad = 0.1;
+
+ public static final double kMaxVelocityRadPerSecond = 3;
+ public static final double kMaxAccelerationRadPerSecSquared = 10;
+
+ // The offset of the arm from the horizontal in its neutral position,
+ // measured from the horizontal
+ public static final double kArmOffsetRads = 0.5;
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
new file mode 100644
index 0000000..0fa7dae
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armbotoffboard;
+
+import edu.wpi.first.wpilibj.SpeedController;
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor controller.
+ *
+ * <p>Has no actual functionality.
+ */
+public class ExampleSmartMotorController implements SpeedController {
+ public enum PIDMode {
+ kPosition,
+ kVelocity,
+ kMovementWitchcraft
+ }
+
+ /**
+ * Creates a new ExampleSmartMotorController.
+ *
+ * @param port The port for the controller.
+ */
+ @SuppressWarnings("PMD.UnusedFormalParameter")
+ public ExampleSmartMotorController(int port) {
+ }
+
+ /**
+ * Example method for setting the PID gains of the smart controller.
+ *
+ * @param kp The proportional gain.
+ * @param ki The integral gain.
+ * @param kd The derivative gain.
+ */
+ public void setPID(double kp, double ki, double kd) {
+ }
+
+ /**
+ * Example method for setting the setpoint of the smart controller in PID mode.
+ *
+ * @param mode The mode of the PID controller.
+ * @param setpoint The controller setpoint.
+ * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+ */
+ public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {
+ }
+
+ /**
+ * Places this motor controller in follower mode.
+ *
+ * @param master The master to follow.
+ */
+ public void follow(ExampleSmartMotorController master) {
+ }
+
+ /**
+ * Returns the encoder distance.
+ *
+ * @return The current encoder distance.
+ */
+ public double getEncoderDistance() {
+ return 0;
+ }
+
+ /**
+ * Returns the encoder rate.
+ *
+ * @return The current encoder rate.
+ */
+ public double getEncoderRate() {
+ return 0;
+ }
+
+ /**
+ * Resets the encoder to zero distance.
+ */
+ public void resetEncoder() {
+ }
+
+ @Override
+ public void set(double speed) {
+ }
+
+ @Override
+ public double get() {
+ return 0;
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+
+ }
+
+ @Override
+ public boolean getInverted() {
+ return false;
+ }
+
+ @Override
+ public void disable() {
+ }
+
+ @Override
+ public void stopMotor() {
+ }
+
+ @Override
+ public void pidWrite(double output) {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
similarity index 76%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
index d7f6e26..ff9e928 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.iterative;
+package edu.wpi.first.wpilibj.examples.armbotoffboard;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java
new file mode 100644
index 0000000..8a7f99b
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armbotoffboard;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
new file mode 100644
index 0000000..285bd58
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armbotoffboard;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+ private final ArmSubsystem m_robotArm = new ArmSubsystem();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive
+ .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link JoystickButton}.
+ */
+ private void configureButtonBindings() {
+
+ // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
+ new JoystickButton(m_driverController, Button.kA.value)
+ .whenPressed(() -> m_robotArm.setGoal(2), m_robotArm);
+
+ // Move the arm to neutral position when the 'B' button is pressed.
+ new JoystickButton(m_driverController, Button.kB.value)
+ .whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm);
+
+ // Drive at half speed when the bumper is held
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
+ .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return new InstantCommand();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
new file mode 100644
index 0000000..2d2698c
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
+
+import edu.wpi.first.wpilibj.controller.ArmFeedforward;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
+
+import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController;
+
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kAVoltSecondSquaredPerRad;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kArmOffsetRads;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kCosVolts;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMaxAccelerationRadPerSecSquared;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMaxVelocityRadPerSecond;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMotorPort;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kP;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kSVolts;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kVVoltSecondPerRad;
+
+/**
+ * A robot arm subsystem that moves with a motion profile.
+ */
+public class ArmSubsystem extends TrapezoidProfileSubsystem {
+ private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(kMotorPort);
+ private final ArmFeedforward m_feedforward =
+ new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad);
+
+ /**
+ * Create a new ArmSubsystem.
+ */
+ public ArmSubsystem() {
+ super(new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond,
+ kMaxAccelerationRadPerSecSquared),
+ kArmOffsetRads);
+ m_motor.setPID(kP, 0, 0);
+ }
+
+ @Override
+ public void useState(TrapezoidProfile.State setpoint) {
+ // Calculate the feedforward from the sepoint
+ double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
+ // Add the feedforward to the PID output to get the motor output
+ m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
+ setpoint.position,
+ feedforward / 12.0);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..974f514
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final SpeedControllerGroup m_leftMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+ new PWMVictorSPX(kLeftMotor2Port));
+
+ // The motors on the right side of the drive.
+ private final SpeedControllerGroup m_rightMotors =
+ new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+ new PWMVictorSPX(kRightMotor2Port));
+
+ // The robot's drive
+ private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+ // The left-side drive encoder
+ private final Encoder m_leftEncoder =
+ new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+ // The right-side drive encoder
+ private final Encoder m_rightEncoder =
+ new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ // Sets the distance per pulse for the encoders
+ m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ }
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public void arcadeDrive(double fwd, double rot) {
+ m_drive.arcadeDrive(fwd, rot);
+ }
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ /**
+ * Gets the average distance of the two encoders.
+ *
+ * @return the average of the two encoder readings
+ */
+ public double getAverageEncoderDistance() {
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
+ }
+
+ /**
+ * Gets the left drive encoder.
+ *
+ * @return the left drive encoder
+ */
+ public Encoder getLeftEncoder() {
+ return m_leftEncoder;
+ }
+
+ /**
+ * Gets the right drive encoder.
+ *
+ * @return the right drive encoder
+ */
+ public Encoder getRightEncoder() {
+ return m_rightEncoder;
+ }
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
index 9f33156..cc3d2e1 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
@@ -9,7 +9,8 @@
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -29,13 +30,13 @@
private static final double kWheelRadius = 0.0508; // meters
private static final int kEncoderResolution = 4096;
- private final Spark m_leftMaster = new Spark(1);
- private final Spark m_leftFollower = new Spark(2);
- private final Spark m_rightMaster = new Spark(3);
- private final Spark m_rightFollower = new Spark(4);
+ private final SpeedController m_leftMaster = new PWMVictorSPX(1);
+ private final SpeedController m_leftFollower = new PWMVictorSPX(2);
+ private final SpeedController m_rightMaster = new PWMVictorSPX(3);
+ private final SpeedController m_rightFollower = new PWMVictorSPX(4);
private final Encoder m_leftEncoder = new Encoder(0, 1);
- private final Encoder m_rightEncoder = new Encoder(0, 1);
+ private final Encoder m_rightEncoder = new Encoder(2, 3);
private final SpeedControllerGroup m_leftGroup
= new SpeedControllerGroup(m_leftMaster, m_leftFollower);
@@ -50,8 +51,7 @@
private final DifferentialDriveKinematics m_kinematics
= new DifferentialDriveKinematics(kTrackWidth);
- private final DifferentialDriveOdometry m_odometry
- = new DifferentialDriveOdometry(m_kinematics);
+ private final DifferentialDriveOdometry m_odometry;
/**
* Constructs a differential drive object.
@@ -65,6 +65,11 @@
// resolution.
m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+
+ m_odometry = new DifferentialDriveOdometry(getAngle());
}
/**
@@ -78,15 +83,6 @@
}
/**
- * Returns the current wheel speeds.
- *
- * @return The current wheel speeds.
- */
- public DifferentialDriveWheelSpeeds getCurrentSpeeds() {
- return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
- }
-
- /**
* Sets the desired wheel speeds.
*
* @param speeds The desired wheel speeds.
@@ -116,6 +112,6 @@
* Updates the field-relative position.
*/
public void updateOdometry() {
- m_odometry.update(getAngle(), getCurrentSpeeds());
+ m_odometry.update(getAngle(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java
new file mode 100644
index 0000000..5f4bbeb
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+ // These characterization values MUST be determined either experimentally or theoretically
+ // for *your* robot's drive.
+ // The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these
+ // values for your robot.
+ public static final double ksVolts = 1;
+ public static final double kvVoltSecondsPerMeter = 0.8;
+ public static final double kaVoltSecondsSquaredPerMeter = 0.15;
+
+ public static final double kp = 1;
+
+ public static final double kMaxSpeedMetersPerSecond = 3;
+ public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
new file mode 100644
index 0000000..6e19020
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
+
+import edu.wpi.first.wpilibj.SpeedController;
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor controller.
+ *
+ * <p>Has no actual functionality.
+ */
+public class ExampleSmartMotorController implements SpeedController {
+ public enum PIDMode {
+ kPosition,
+ kVelocity,
+ kMovementWitchcraft
+ }
+
+ /**
+ * Creates a new ExampleSmartMotorController.
+ *
+ * @param port The port for the controller.
+ */
+ @SuppressWarnings("PMD.UnusedFormalParameter")
+ public ExampleSmartMotorController(int port) {
+ }
+
+ /**
+ * Example method for setting the PID gains of the smart controller.
+ *
+ * @param kp The proportional gain.
+ * @param ki The integral gain.
+ * @param kd The derivative gain.
+ */
+ public void setPID(double kp, double ki, double kd) {
+ }
+
+ /**
+ * Example method for setting the setpoint of the smart controller in PID mode.
+ *
+ * @param mode The mode of the PID controller.
+ * @param setpoint The controller setpoint.
+ * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+ */
+ public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {
+ }
+
+ /**
+ * Places this motor controller in follower mode.
+ *
+ * @param master The master to follow.
+ */
+ public void follow(ExampleSmartMotorController master) {
+ }
+
+ /**
+ * Returns the encoder distance.
+ *
+ * @return The current encoder distance.
+ */
+ public double getEncoderDistance() {
+ return 0;
+ }
+
+ /**
+ * Returns the encoder rate.
+ *
+ * @return The current encoder rate.
+ */
+ public double getEncoderRate() {
+ return 0;
+ }
+
+ /**
+ * Resets the encoder to zero distance.
+ */
+ public void resetEncoder() {
+ }
+
+ @Override
+ public void set(double speed) {
+ }
+
+ @Override
+ public double get() {
+ return 0;
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+
+ }
+
+ @Override
+ public boolean getInverted() {
+ return false;
+ }
+
+ @Override
+ public void disable() {
+ }
+
+ @Override
+ public void stopMotor() {
+ }
+
+ @Override
+ public void pidWrite(double output) {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java
similarity index 75%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java
index d7f6e26..89ab553 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.iterative;
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java
new file mode 100644
index 0000000..e8ac285
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
new file mode 100644
index 0000000..a0eb86a
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands.DriveDistanceProfiled;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxSpeedMetersPerSecond;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive
+ .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+ * {@link JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds
+ new JoystickButton(m_driverController, Button.kA.value)
+ .whenPressed(new DriveDistanceProfiled(3, m_robotDrive).withTimeout(10));
+
+ // Do the same thing as above when the 'B' button is pressed, but defined inline
+ new JoystickButton(m_driverController, Button.kB.value)
+ .whenPressed(
+ new TrapezoidProfileCommand(
+ new TrapezoidProfile(
+ // Limit the max acceleration and velocity
+ new TrapezoidProfile.Constraints(kMaxSpeedMetersPerSecond,
+ kMaxAccelerationMetersPerSecondSquared),
+ // End at desired position in meters; implicitly starts at 0
+ new TrapezoidProfile.State(3, 0)),
+ // Pipe the profile state to the drive
+ setpointState -> m_robotDrive.setDriveStates(
+ setpointState,
+ setpointState),
+ // Require the drive
+ m_robotDrive)
+ .beforeStarting(m_robotDrive::resetEncoders)
+ .withTimeout(10));
+
+ // Drive at half speed when the bumper is held
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
+ .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return new InstantCommand();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
new file mode 100644
index 0000000..f89b36a
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
+
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxSpeedMetersPerSecond;
+
+/**
+ * Drives a set distance using a motion profile.
+ */
+public class DriveDistanceProfiled extends TrapezoidProfileCommand {
+ /**
+ * Creates a new DriveDistanceProfiled command.
+ *
+ * @param meters The distance to drive.
+ * @param drive The drive subsystem to use.
+ */
+ public DriveDistanceProfiled(double meters, DriveSubsystem drive) {
+ super(
+ new TrapezoidProfile(
+ // Limit the max acceleration and velocity
+ new TrapezoidProfile.Constraints(kMaxSpeedMetersPerSecond,
+ kMaxAccelerationMetersPerSecondSquared),
+ // End at desired position in meters; implicitly starts at 0
+ new TrapezoidProfile.State(meters, 0)),
+ // Pipe the profile state to the drive
+ setpointState -> drive.setDriveStates(setpointState, setpointState),
+ // Require the drive
+ drive);
+ // Reset drive encoders since we're starting at 0
+ drive.resetEncoders();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..c8b98f7
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems;
+
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController;
+
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kRightMotor2Port;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kaVoltSecondsSquaredPerMeter;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kp;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.ksVolts;
+import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kvVoltSecondsPerMeter;
+
+public class DriveSubsystem extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final ExampleSmartMotorController m_leftMaster =
+ new ExampleSmartMotorController(kLeftMotor1Port);
+
+ private final ExampleSmartMotorController m_leftSlave =
+ new ExampleSmartMotorController(kLeftMotor2Port);
+
+ private final ExampleSmartMotorController m_rightMaster =
+ new ExampleSmartMotorController(kRightMotor1Port);
+
+ private final ExampleSmartMotorController m_rightSlave =
+ new ExampleSmartMotorController(kRightMotor2Port);
+
+ private final SimpleMotorFeedforward m_feedforward =
+ new SimpleMotorFeedforward(ksVolts,
+ kvVoltSecondsPerMeter,
+ kaVoltSecondsSquaredPerMeter);
+
+ // The robot's drive
+ private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMaster, m_rightMaster);
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ m_leftSlave.follow(m_leftMaster);
+ m_rightSlave.follow(m_rightMaster);
+
+ m_leftMaster.setPID(kp, 0, 0);
+ m_rightMaster.setPID(kp, 0, 0);
+ }
+
+ /**
+ * Drives the robot using arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public void arcadeDrive(double fwd, double rot) {
+ m_drive.arcadeDrive(fwd, rot);
+ }
+
+ /**
+ * Attempts to follow the given drive states using offboard PID.
+ *
+ * @param left The left wheel state.
+ * @param right The right wheel state.
+ */
+ public void setDriveStates(TrapezoidProfile.State left, TrapezoidProfile.State right) {
+ m_leftMaster.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
+ left.position,
+ m_feedforward.calculate(left.velocity));
+ m_rightMaster.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
+ right.position,
+ m_feedforward.calculate(right.velocity));
+ }
+
+ /**
+ * Returns the left encoder distance.
+ *
+ * @return the left encoder distance
+ */
+ public double getLeftEncoderDistance() {
+ return m_leftMaster.getEncoderDistance();
+ }
+
+ /**
+ * Returns the right encoder distance.
+ *
+ * @return the right encoder distance
+ */
+ public double getRightEncoderDistance() {
+ return m_rightMaster.getEncoderDistance();
+ }
+
+ /**
+ * Resets the drive encoders.
+ */
+ public void resetEncoders() {
+ m_leftMaster.resetEncoder();
+ m_rightMaster.resetEncoder();
+ }
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java
similarity index 88%
rename from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
rename to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java
index d7f6e26..a051005 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java
@@ -1,29 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.iterative;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
- */
-public final class Main {
- private Main() {
- }
-
- /**
- * Main initialization function. Do not perform any initialization here.
- *
- * <p>If you change your main robot class, change the parameter type.
- */
- public static void main(String... args) {
- RobotBase.startRobot(Robot::new);
- }
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.dutycycleencoder;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+ private Main() {
+ }
+
+ /**
+ * Main initialization function. Do not perform any initialization here.
+ *
+ * <p>If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java
new file mode 100644
index 0000000..5755f72
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.dutycycleencoder;
+
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+@SuppressWarnings({"PMD.SingularField"})
+public class Robot extends TimedRobot {
+ private DutyCycleEncoder m_dutyCycleEncoder;
+
+ @Override
+ public void robotInit() {
+ m_dutyCycleEncoder = new DutyCycleEncoder(0);
+
+ // Set to 0.5 units per rotation
+ m_dutyCycleEncoder.setDistancePerRotation(0.5);
+ }
+
+ @Override
+ public void robotPeriodic() {
+ // Connected can be checked, and uses the frequency of the encoder
+ boolean connected = m_dutyCycleEncoder.isConnected();
+
+ // Duty Cycle Frequency in Hz
+ int frequency = m_dutyCycleEncoder.getFrequency();
+
+ // Output of encoder
+ double output = m_dutyCycleEncoder.get();
+
+ // Output scaled by DistancePerPulse
+ double distance = m_dutyCycleEncoder.getDistance();
+
+ SmartDashboard.putBoolean("Connected", connected);
+ SmartDashboard.putNumber("Frequency", frequency);
+ SmartDashboard.putNumber("Output", output);
+ SmartDashboard.putNumber("Distance", distance);
+ }
+
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java
index d7f6e26..a7cbfc0 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java
@@ -1,29 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.iterative;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
- */
-public final class Main {
- private Main() {
- }
-
- /**
- * Main initialization function. Do not perform any initialization here.
- *
- * <p>If you change your main robot class, change the parameter type.
- */
- public static void main(String... args) {
- RobotBase.startRobot(Robot::new);
- }
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.dutycycleinput;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+ private Main() {
+ }
+
+ /**
+ * Main initialization function. Do not perform any initialization here.
+ *
+ * <p>If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java
new file mode 100644
index 0000000..657dc9b
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.dutycycleinput;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DutyCycle;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+@SuppressWarnings({"PMD.SingularField"})
+public class Robot extends TimedRobot {
+ private DigitalInput m_input;
+ private DutyCycle m_dutyCycle;
+
+ @Override
+ public void robotInit() {
+ m_input = new DigitalInput(0);
+ m_dutyCycle = new DutyCycle(m_input);
+ }
+
+ @Override
+ public void robotPeriodic() {
+ // Duty Cycle Frequency in Hz
+ int frequency = m_dutyCycle.getFrequency();
+
+ // Output of duty cycle, ranging from 0 to 1
+ // 1 is fully on, 0 is fully off
+ double output = m_dutyCycle.getOutput();
+
+ SmartDashboard.putNumber("Frequency", frequency);
+ SmartDashboard.putNumber("Duty Cycle", output);
+ }
+
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
index a48909f..5d3d546 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
@@ -9,7 +9,8 @@
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
@@ -19,7 +20,7 @@
private final Joystick m_joystick = new Joystick(1);
private final Encoder m_encoder = new Encoder(1, 2);
- private final Spark m_motor = new Spark(1);
+ private final SpeedController m_motor = new PWMVictorSPX(1);
// Create a PID controller whose setpoint's change is subject to maximum
// velocity and acceleration constraints.
@@ -30,7 +31,7 @@
@Override
public void robotInit() {
- m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+ m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
}
@Override
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
new file mode 100644
index 0000000..d6a84ef
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
+
+import edu.wpi.first.wpilibj.SpeedController;
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor controller.
+ *
+ * <p>Has no actual functionality.
+ */
+public class ExampleSmartMotorController implements SpeedController {
+ public enum PIDMode {
+ kPosition,
+ kVelocity,
+ kMovementWitchcraft
+ }
+
+ /**
+ * Creates a new ExampleSmartMotorController.
+ *
+ * @param port The port for the controller.
+ */
+ @SuppressWarnings("PMD.UnusedFormalParameter")
+ public ExampleSmartMotorController(int port) {
+ }
+
+ /**
+ * Example method for setting the PID gains of the smart controller.
+ *
+ * @param kp The proportional gain.
+ * @param ki The integral gain.
+ * @param kd The derivative gain.
+ */
+ public void setPID(double kp, double ki, double kd) {
+ }
+
+ /**
+ * Example method for setting the setpoint of the smart controller in PID mode.
+ *
+ * @param mode The mode of the PID controller.
+ * @param setpoint The controller setpoint.
+ * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+ */
+ public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {
+ }
+
+ /**
+ * Places this motor controller in follower mode.
+ *
+ * @param master The master to follow.
+ */
+ public void follow(ExampleSmartMotorController master) {
+ }
+
+ /**
+ * Returns the encoder distance.
+ *
+ * @return The current encoder distance.
+ */
+ public double getEncoderDistance() {
+ return 0;
+ }
+
+ /**
+ * Returns the encoder rate.
+ *
+ * @return The current encoder rate.
+ */
+ public double getEncoderRate() {
+ return 0;
+ }
+
+ /**
+ * Resets the encoder to zero distance.
+ */
+ public void resetEncoder() {
+ }
+
+ @Override
+ public void set(double speed) {
+ }
+
+ @Override
+ public double get() {
+ return 0;
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+
+ }
+
+ @Override
+ public boolean getInverted() {
+ return false;
+ }
+
+ @Override
+ public void disable() {
+ }
+
+ @Override
+ public void stopMotor() {
+ }
+
+ @Override
+ public void pidWrite(double output) {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
index fddd417..918d3d3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
@@ -7,20 +7,18 @@
package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
-import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
public class Robot extends TimedRobot {
private static double kDt = 0.02;
private final Joystick m_joystick = new Joystick(1);
- private final Encoder m_encoder = new Encoder(1, 2);
- private final Spark m_motor = new Spark(1);
- private final PIDController m_controller = new PIDController(1.3, 0.0, 0.7, kDt);
+ private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);
+ // Note: These gains are fake, and will have to be tuned for your robot.
+ private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1.5);
private final TrapezoidProfile.Constraints m_constraints =
new TrapezoidProfile.Constraints(1.75, 0.75);
@@ -29,7 +27,8 @@
@Override
public void robotInit() {
- m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+ // Note: These gains are fake, and will have to be tuned for your robot.
+ m_motor.setPID(1.3, 0.0, 0.7);
}
@Override
@@ -49,10 +48,8 @@
// toward the goal while obeying the constraints.
m_setpoint = profile.calculate(kDt);
- double output = m_controller.calculate(m_encoder.getDistance(),
- m_setpoint.position);
-
- // Run controller with profiled setpoint and update motor output
- m_motor.set(output);
+ // Send setpoint to offboard controller PID
+ m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position,
+ m_feedforward.calculate(m_setpoint.velocity) / 12.0);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
index 4bb26ea..ef34fe5 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
@@ -54,7 +54,7 @@
* attached to a 3 inch diameter (1.5inch radius) wheel,
* and that we want to measure distance in inches.
*/
- m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+ m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
/*
* Defines the lowest rate at which the encoder will
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
index 39840f7..6dd4178 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
@@ -7,7 +7,8 @@
],
"foldername": "gettingstarted",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Tank Drive",
@@ -20,7 +21,8 @@
],
"foldername": "tankdrive",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Arcade Drive",
@@ -30,7 +32,8 @@
],
"foldername": "arcadedrive",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Mecanum Drive",
@@ -43,7 +46,8 @@
],
"foldername": "mecanumdrive",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "PDP CAN Monitoring",
@@ -55,7 +59,8 @@
],
"foldername": "canpdp",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Solenoids",
@@ -68,7 +73,8 @@
],
"foldername": "solenoid",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Encoder",
@@ -80,7 +86,8 @@
],
"foldername": "encoder",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Relay",
@@ -92,7 +99,8 @@
],
"foldername": "relay",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Ultrasonic",
@@ -104,7 +112,8 @@
],
"foldername": "ultrasonic",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Ultrasonic PID",
@@ -116,7 +125,8 @@
],
"foldername": "ultrasonicpid",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Potentiometer PID",
@@ -129,7 +139,8 @@
],
"foldername": "potentiometerpid",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Elevator with trapezoid profiled PID",
@@ -142,7 +153,8 @@
],
"foldername": "elevatortrapezoidprofile",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Elevator with profiled PID controller",
@@ -155,7 +167,8 @@
],
"foldername": "elevatorprofiledpid",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Gyro",
@@ -168,7 +181,8 @@
],
"foldername": "gyro",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Gyro Mecanum",
@@ -181,7 +195,8 @@
],
"foldername": "gyromecanum",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "HID Rumble",
@@ -191,7 +206,8 @@
],
"foldername": "hidrumble",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Motor Controller",
@@ -203,7 +219,8 @@
],
"foldername": "motorcontrol",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Motor Control With Encoder",
@@ -217,8 +234,9 @@
"Complete List"
],
"foldername": "motorcontrolencoder",
- "gradlebase": "java"
- ,"mainclass": "Main"
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "GearsBot",
@@ -228,7 +246,8 @@
],
"foldername": "gearsbot",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "PacGoat",
@@ -238,7 +257,8 @@
],
"foldername": "pacgoat",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Simple Vision",
@@ -249,7 +269,8 @@
],
"foldername": "quickvision",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Intermediate Vision",
@@ -260,7 +281,8 @@
],
"foldername": "intermediatevision",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Axis Camera Sample",
@@ -270,7 +292,8 @@
],
"foldername": "axiscamera",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Shuffleboard Sample",
@@ -281,7 +304,8 @@
],
"foldername": "shuffleboard",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "'Traditional' Hatchbot",
@@ -292,7 +316,8 @@
],
"foldername": "hatchbottraditional",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "'Inlined' Hatchbot",
@@ -304,7 +329,8 @@
],
"foldername": "hatchbotinlined",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "Select Command Example",
@@ -314,7 +340,8 @@
],
"foldername": "selectcommand",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "Scheduler Event Logging",
@@ -325,7 +352,8 @@
],
"foldername": "schedulereventlogging",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "Frisbeebot",
@@ -336,7 +364,8 @@
],
"foldername": "frisbeebot",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "Gyro Drive Commands",
@@ -348,7 +377,8 @@
],
"foldername": "gyrodrivecommands",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "SwerveBot",
@@ -358,7 +388,8 @@
],
"foldername": "swervebot",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "MecanumBot",
@@ -368,7 +399,8 @@
],
"foldername": "mecanumbot",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
"name": "DifferentialDriveBot",
@@ -378,10 +410,11 @@
],
"foldername": "differentialdrivebot",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
},
{
- "name:": "RamseteCommand",
+ "name": "RamseteCommand",
"description": "An example command-based robot demonstrating the use of a RamseteCommand to follow a pregenerated trajectory.",
"tags": [
"RamseteCommand",
@@ -392,6 +425,131 @@
],
"foldername": "ramsetecommand",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "Arcade Drive Xbox Controller",
+ "description": "Demonstrates the use of the DifferentialDrive class to drive a robot with Arcade Drive and an Xbox Controller",
+ "tags": [
+ "Getting Started with Java"
+ ],
+ "foldername": "arcadedrivexboxcontroller",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 1
+ },
+ {
+ "name": "Tank Drive Xbox Controller",
+ "description": "Demonstrates the use of the DifferentialDrive class to drive a robot with Tank Drive and an Xbox Controller",
+ "tags": [
+ "Getting Started with Java"
+ ],
+ "foldername": "tankdrivexboxcontroller",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 1
+ },
+ {
+ "name": "Duty Cycle Encoder",
+ "description": "Demonstrates the use of the Duty Cycle Encoder class",
+ "tags": [
+ "Getting Started with Java"
+ ],
+ "foldername": "dutycycleencoder",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "Duty Cycle Input",
+ "description": "Demonstrates the use of the Duty Cycle class",
+ "tags": [
+ "Getting Started with Java"
+ ],
+ "foldername": "dutycycleinput",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "Addressable LED",
+ "description": "Demonstrates the use of the Addressable LED class",
+ "tags": [
+ "Getting Started with Java"
+ ],
+ "foldername": "addressableled",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "ArmBot",
+ "description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.",
+ "tags": [
+ "ArmBot",
+ "PID",
+ "Motion Profile"
+ ],
+ "foldername": "armbot",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "ArmBotOffboard",
+ "description": "An example command-based robot demonstrating the use of a TrapezoidProfileSubsystem to control an arm with an offboard PID.",
+ "tags": [
+ "ArmBotOffboard",
+ "PID",
+ "Motion Profile"
+ ],
+ "foldername": "armbotoffboard",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "DriveDistanceOffboard",
+ "description": "An example command-based robot demonstrating the use of a TrapezoidProfileCommand to drive a robot a set distance with offboard PID on the drive.",
+ "tags": [
+ "DriveDistance",
+ "PID",
+ "Motion Profile"
+ ],
+ "foldername": "drivedistanceoffboard",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "MecanumControllerCommand",
+ "description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.",
+ "tags": [
+ "MecanumControllerCommand",
+ "Mecanum",
+ "PID",
+ "Trajectory",
+ "Path following"
+ ],
+ "foldername": "mecanumcontrollercommand",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "SwerveControllerCommand",
+ "description": "An example command-based robot demonstrating the use of a SwerveControllerCommand to follow a pregenerated trajectory.",
+ "tags": [
+ "SwerveControllerCommand",
+ "Swerve",
+ "PID",
+ "Trajectory",
+ "Path following"
+ ],
+ "foldername": "swervecontrollercommand",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 2
}
]
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
index 384fb98..c8c422c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
@@ -40,7 +40,7 @@
public static final int kEncoderCPR = 1024;
public static final double kEncoderDistancePerPulse =
// Distance units will be rotations
- 1. / (double) kEncoderCPR;
+ 1.0 / (double) kEncoderCPR;
public static final int kShooterMotorPort = 4;
public static final int kFeederMotorPort = 5;
@@ -49,18 +49,19 @@
public static final double kShooterTargetRPS = 4000;
public static final double kShooterToleranceRPS = 50;
+ // These are not real PID gains, and will have to be tuned for your specific robot.
public static final double kP = 1;
public static final double kI = 0;
public static final double kD = 0;
// On a real robot the feedforward constants should be empirically determined; these are
// reasonable guesses.
- public static final double kSFractional = .05;
- public static final double kVFractional =
- // Should have value 1 at free speed...
- 1. / kShooterFreeRPS;
+ public static final double kSVolts = 0.05;
+ public static final double kVVoltSecondsPerRotation =
+ // Should have value 12V at free speed...
+ 12.0 / kShooterFreeRPS;
- public static final double kFeederSpeed = .5;
+ public static final double kFeederSpeed = 0.5;
}
public static final class AutoConstants {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
index ea6377a..69008ae 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
@@ -103,7 +103,7 @@
// Drive at half speed when the bumper is held
new JoystickButton(m_driverController, Button.kBumperRight.value)
- .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
index c9d8729..192fa30 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
@@ -78,7 +78,7 @@
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
- return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
index 5ebfb04..c870114 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
@@ -10,6 +10,7 @@
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kD;
@@ -20,17 +21,19 @@
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederSpeed;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kI;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kP;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kSFractional;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kSVolts;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterMotorPort;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterTargetRPS;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterToleranceRPS;
-import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kVFractional;
+import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kVVoltSecondsPerRotation;
public class ShooterSubsystem extends PIDSubsystem {
private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(kShooterMotorPort);
private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(kFeederMotorPort);
private final Encoder m_shooterEncoder =
new Encoder(kEncoderPorts[0], kEncoderPorts[1], kEncoderReversed);
+ private final SimpleMotorFeedforward m_shooterFeedforward
+ = new SimpleMotorFeedforward(kSVolts, kVVoltSecondsPerRotation);
/**
* The shooter subsystem for the robot.
@@ -39,17 +42,12 @@
super(new PIDController(kP, kI, kD));
getController().setTolerance(kShooterToleranceRPS);
m_shooterEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ setSetpoint(kShooterTargetRPS);
}
@Override
- public void useOutput(double output) {
- // Use a feedforward of the form kS + kV * velocity
- m_shooterMotor.set(output + kSFractional + kVFractional * kShooterTargetRPS);
- }
-
- @Override
- public double getSetpoint() {
- return kShooterTargetRPS;
+ public void useOutput(double output, double setpoint) {
+ m_shooterMotor.setVoltage(output + m_shooterFeedforward.calculate(setpoint));
}
@Override
@@ -68,12 +66,4 @@
public void stopFeeder() {
m_feederMotor.set(0);
}
-
- @Override
- public void disable() {
- super.disable();
- // Turn off motor when we disable, since useOutput(0) doesn't stop the motor due to our
- // feedforward
- m_shooterMotor.set(0);
- }
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
index fffd478..4f5c77f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
@@ -22,7 +22,6 @@
public class Elevator extends PIDSubsystem {
private final Victor m_motor;
private final AnalogPotentiometer m_pot;
- private double m_setpoint;
private static final double kP_real = 4;
private static final double kI_real = 0.07;
@@ -73,26 +72,7 @@
* Use the motor as the PID output. This method is automatically called by the subsystem.
*/
@Override
- public void useOutput(double output) {
+ public void useOutput(double output, double setpoint) {
m_motor.set(output);
}
-
- /**
- * Returns the setpoint used by the PIDController.
- *
- * @return The setpoint for the PIDController.
- */
- @Override
- public double getSetpoint() {
- return m_setpoint;
- }
-
- /**
- * Sets the setpoint used by the PIDController.
- *
- * @param setpoint The setpoint for the PIDController.
- */
- public void setSetpoint(double setpoint) {
- m_setpoint = setpoint;
- }
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
index ae65bed..dbd952e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
@@ -21,7 +21,6 @@
public class Wrist extends PIDSubsystem {
private final Victor m_motor;
private final AnalogPotentiometer m_pot;
- private double m_setpoint;
private static final double kP_real = 1;
private static final double kP_simulation = 0.05;
@@ -70,26 +69,7 @@
* Use the motor as the PID output. This method is automatically called by the subsystem.
*/
@Override
- public void useOutput(double output) {
+ public void useOutput(double output, double setpoint) {
m_motor.set(output);
}
-
- /**
- * Returns the setpoint used by the PIDController.
- *
- * @return The setpoint for the PIDController.
- */
- @Override
- public double getSetpoint() {
- return m_setpoint;
- }
-
- /**
- * Sets the setpoint used by the PIDController.
- *
- * @param setpoint The setpoint for the PIDController.
- */
- public void setSetpoint(double setpoint) {
- m_setpoint = setpoint;
- }
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
index 50afb7b..473d6e1 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
@@ -36,13 +36,16 @@
public static final boolean kGyroReversed = false;
public static final double kStabilizationP = 1;
- public static final double kStabilizationI = .5;
+ public static final double kStabilizationI = 0.5;
public static final double kStabilizationD = 0;
public static final double kTurnP = 1;
public static final double kTurnI = 0;
public static final double kTurnD = 0;
+ public static final double kMaxTurnRateDegPerS = 100;
+ public static final double kMaxTurnAccelerationDegPerSSquared = 300;
+
public static final double kTurnToleranceDeg = 5;
public static final double kTurnRateToleranceDegPerS = 10; // degrees per second
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
index 5a6d632..44b09b8 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
@@ -17,6 +17,7 @@
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngleProfiled;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
import static edu.wpi.first.wpilibj.XboxController.Button;
@@ -65,7 +66,7 @@
private void configureButtonBindings() {
// Drive at half speed when the right bumper is held
new JoystickButton(m_driverController, Button.kBumperRight.value)
- .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
// Stabilize robot to drive straight with gyro when left bumper is held
@@ -85,6 +86,10 @@
// Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout
new JoystickButton(m_driverController, Button.kX.value)
.whenPressed(new TurnToAngle(90, m_robotDrive).withTimeout(5));
+
+ // Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5 second timeout
+ new JoystickButton(m_driverController, Button.kA.value)
+ .whenPressed(new TurnToAngleProfiled(-90, m_robotDrive).withTimeout(5));
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
new file mode 100644
index 0000000..8fb4320
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
+
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
+
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kMaxTurnAccelerationDegPerSSquared;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kMaxTurnRateDegPerS;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnD;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnI;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnP;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnRateToleranceDegPerS;
+import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnToleranceDeg;
+
+/**
+ * A command that will turn the robot to the specified angle using a motion profile.
+ */
+public class TurnToAngleProfiled extends ProfiledPIDCommand {
+ /**
+ * Turns to robot to the specified angle using a motion profile.
+ *
+ * @param targetAngleDegrees The angle to turn to
+ * @param drive The drive subsystem to use
+ */
+ public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
+ super(
+ new ProfiledPIDController(kTurnP, kTurnI, kTurnD,
+ new TrapezoidProfile.Constraints(
+ kMaxTurnRateDegPerS,
+ kMaxTurnAccelerationDegPerSSquared
+ )),
+ // Close loop on heading
+ drive::getHeading,
+ // Set reference to target
+ targetAngleDegrees,
+ // Pipe output to turn robot
+ (output, setpoint) -> drive.arcadeDrive(0, output),
+ // Require the drive
+ drive);
+
+ // Set the controller to be continuous (because it is an angle controller)
+ getController().enableContinuousInput(-180, 180);
+ // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
+ // setpoint before it is considered as having reached the reference
+ getController().setTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
+ }
+
+ @Override
+ public boolean isFinished() {
+ // End when the controller is at the reference.
+ return getController().atGoal();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
index e91b8b3..a4f001e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
@@ -84,7 +84,7 @@
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
- return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
@@ -127,7 +127,7 @@
* @return the robot's heading in degrees, from 180 to 180
*/
public double getHeading() {
- return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1. : 1.);
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
}
/**
@@ -136,6 +136,6 @@
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
- return m_gyro.getRate() * (kGyroReversed ? -1. : 1.);
+ return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
index 27cb872..8029888 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
@@ -42,7 +42,7 @@
public static final class AutoConstants {
public static final double kAutoDriveDistanceInches = 60;
public static final double kAutoBackupDistanceInches = 20;
- public static final double kAutoDriveSpeed = .5;
+ public static final double kAutoDriveSpeed = 0.5;
}
public static final class OIConstants {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
index 9310d39..a5c7129 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
@@ -105,7 +105,7 @@
.whenPressed(new InstantCommand(m_hatchSubsystem::releaseHatch, m_hatchSubsystem));
// While holding the shoulder button, drive at half speed
new JoystickButton(m_driverController, Button.kBumperRight.value)
- .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
index 233cb85..5361e87 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
@@ -78,7 +78,7 @@
* @return the average of the TWO encoder readings
*/
public double getAverageEncoderDistance() {
- return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
index 1e9e060..2674b50 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
@@ -42,7 +42,7 @@
public static final class AutoConstants {
public static final double kAutoDriveDistanceInches = 60;
public static final double kAutoBackupDistanceInches = 20;
- public static final double kAutoDriveSpeed = .5;
+ public static final double kAutoDriveSpeed = 0.5;
}
public static final class OIConstants {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
index 396d40d..bd6c093 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
@@ -20,7 +20,7 @@
@Override
public void initialize() {
- m_drive.setMaxOutput(.5);
+ m_drive.setMaxOutput(0.5);
}
@Override
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
index 6cadc1f..a068b56 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
@@ -78,7 +78,7 @@
* @return the average of the TWO encoder readings
*/
public double getAverageEncoderDistance() {
- return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
index 29a6f49..6220807 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
@@ -9,7 +9,8 @@
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
@@ -26,15 +27,15 @@
public static final double kMaxSpeed = 3.0; // 3 meters per second
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
- private final Spark m_frontLeftMotor = new Spark(1);
- private final Spark m_frontRightMotor = new Spark(2);
- private final Spark m_backLeftMotor = new Spark(3);
- private final Spark m_backRightMotor = new Spark(4);
+ private final SpeedController m_frontLeftMotor = new PWMVictorSPX(1);
+ private final SpeedController m_frontRightMotor = new PWMVictorSPX(2);
+ private final SpeedController m_backLeftMotor = new PWMVictorSPX(3);
+ private final SpeedController m_backRightMotor = new PWMVictorSPX(4);
private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
- private final Encoder m_frontRightEncoder = new Encoder(0, 1);
- private final Encoder m_backLeftEncoder = new Encoder(0, 1);
- private final Encoder m_backRightEncoder = new Encoder(0, 1);
+ private final Encoder m_frontRightEncoder = new Encoder(2, 3);
+ private final Encoder m_backLeftEncoder = new Encoder(4, 5);
+ private final Encoder m_backRightEncoder = new Encoder(6, 7);
private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
@@ -52,7 +53,8 @@
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
);
- private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics);
+ private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
+ getAngle());
/**
* Constructs a MecanumDrive and resets the gyro.
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
new file mode 100644
index 0000000..88f904b
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
+
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kFrontLeftMotorPort = 0;
+ public static final int kRearLeftMotorPort = 1;
+ public static final int kFrontRightMotorPort = 2;
+ public static final int kRearRightMotorPort = 3;
+
+ public static final int[] kFrontLeftEncoderPorts = new int[]{0, 1};
+ public static final int[] kRearLeftEncoderPorts = new int[]{2, 3};
+ public static final int[] kFrontRightEncoderPorts = new int[]{4, 5};
+ public static final int[] kRearRightEncoderPorts = new int[]{5, 6};
+
+ public static final boolean kFrontLeftEncoderReversed = false;
+ public static final boolean kRearLeftEncoderReversed = true;
+ public static final boolean kFrontRightEncoderReversed = false;
+ public static final boolean kRearRightEncoderReversed = true;
+
+ public static final double kTrackWidth = 0.5;
+ // Distance between centers of right and left wheels on robot
+ public static final double kWheelBase = 0.7;
+ // Distance between centers of front and back wheels on robot
+
+ public static final MecanumDriveKinematics kDriveKinematics =
+ new MecanumDriveKinematics(
+ new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterMeters = 0.15;
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+
+ public static final boolean kGyroReversed = false;
+
+ // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+ // These characterization values MUST be determined either experimentally or theoretically
+ // for *your* robot's drive.
+ // The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these
+ // values for your robot.
+ public static final SimpleMotorFeedforward kFeedforward =
+ new SimpleMotorFeedforward(1, 0.8, 0.15);
+
+ // Example value only - as above, this must be tuned for your drive!
+ public static final double kPFrontLeftVel = 0.5;
+ public static final double kPRearLeftVel = 0.5;
+ public static final double kPFrontRightVel = 0.5;
+ public static final double kPRearRightVel = 0.5;
+
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+
+ }
+
+ public static final class AutoConstants {
+ public static final double kMaxSpeedMetersPerSecond = 3;
+ public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+ public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
+ public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
+
+ public static final double kPXController = 0.5;
+ public static final double kPYController = 0.5;
+ public static final double kPThetaController = 0.5;
+
+ //Constraint for the motion profilied robot angle controller
+ public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
+ new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond,
+ kMaxAngularSpeedRadiansPerSecondSquared);
+
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java
similarity index 75%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java
index d7f6e26..9670d3d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.iterative;
+package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java
new file mode 100644
index 0000000..6f2ccbf
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
new file mode 100644
index 0000000..54707e3
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
+
+import java.util.List;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.MecanumControllerCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPThetaController;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPXController;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPYController;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kThetaControllerConstraints;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kDriveKinematics;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFeedforward;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPFrontLeftVel;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPFrontRightVel;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPRearLeftVel;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPRearRightVel;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants.kDriverControllerPort;
+
+/*
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+@SuppressWarnings("PMD.ExcessiveImports")
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive.drive(
+ m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight),
+ m_driverController.getX(GenericHID.Hand.kLeft), false)));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+ * {@link JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // Drive at half speed when the right bumper is held
+ new JoystickButton(m_driverController, Button.kBumperRight.value)
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
+ .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+
+ }
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ // Create config for trajectory
+ TrajectoryConfig config =
+ new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
+ // Add kinematics to ensure max speed is actually obeyed
+ .setKinematics(kDriveKinematics);
+
+ // An example trajectory to follow. All units in meters.
+ Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
+ // Start at the origin facing the +X direction
+ new Pose2d(0, 0, new Rotation2d(0)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ List.of(
+ new Translation2d(1, 1),
+ new Translation2d(2, - 1)
+ ),
+ // End 3 meters straight ahead of where we started, facing forward
+ new Pose2d(3, 0, new Rotation2d(0)),
+ config
+ );
+
+ MecanumControllerCommand mecanumControllerCommand = new MecanumControllerCommand(
+ exampleTrajectory,
+ m_robotDrive::getPose,
+
+ kFeedforward,
+ kDriveKinematics,
+
+ //Position contollers
+ new PIDController(kPXController, 0, 0),
+ new PIDController(kPYController, 0, 0),
+ new ProfiledPIDController(kPThetaController, 0, 0, kThetaControllerConstraints),
+
+ //Needed for normalizing wheel speeds
+ kMaxSpeedMetersPerSecond,
+
+ //Velocity PID's
+ new PIDController(kPFrontLeftVel, 0, 0),
+ new PIDController(kPRearLeftVel, 0, 0),
+ new PIDController(kPFrontRightVel, 0, 0),
+ new PIDController(kPRearRightVel, 0, 0),
+
+ m_robotDrive::getCurrentWheelSpeeds,
+
+ m_robotDrive::setDriveSpeedControllersVolts, //Consumer for the output motor voltages
+
+ m_robotDrive
+ );
+
+ // Run path following command, then stop at the end.
+ return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..b68f770
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
@@ -0,0 +1,253 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems;
+
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.drive.MecanumDrive;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kDriveKinematics;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftMotorPort;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightMotorPort;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kGyroReversed;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftMotorPort;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightMotorPort;
+
+public class DriveSubsystem extends SubsystemBase {
+ private final PWMVictorSPX m_frontLeft = new PWMVictorSPX(kFrontLeftMotorPort);
+ private final PWMVictorSPX m_rearLeft = new PWMVictorSPX(kRearLeftMotorPort);
+ private final PWMVictorSPX m_frontRight = new PWMVictorSPX(kFrontRightMotorPort);
+ private final PWMVictorSPX m_rearRight = new PWMVictorSPX(kRearRightMotorPort);
+
+ private final MecanumDrive m_drive = new MecanumDrive(
+ m_frontLeft,
+ m_rearLeft,
+ m_frontRight,
+ m_rearRight);
+
+ // The front-left-side drive encoder
+ private final Encoder m_frontLeftEncoder =
+ new Encoder(kFrontLeftEncoderPorts[0], kFrontLeftEncoderPorts[1],
+ kFrontLeftEncoderReversed);
+
+ // The rear-left-side drive encoder
+ private final Encoder m_rearLeftEncoder =
+ new Encoder(kRearLeftEncoderPorts[0], kRearLeftEncoderPorts[1],
+ kRearLeftEncoderReversed);
+
+ // The front-right--side drive encoder
+ private final Encoder m_frontRightEncoder =
+ new Encoder(kFrontRightEncoderPorts[0], kFrontRightEncoderPorts[1],
+ kFrontRightEncoderReversed);
+
+ // The rear-right-side drive encoder
+ private final Encoder m_rearRightEncoder =
+ new Encoder(kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
+ kRearRightEncoderReversed);
+
+ // The gyro sensor
+ private final Gyro m_gyro = new ADXRS450_Gyro();
+
+ // Odometry class for tracking robot pose
+ MecanumDriveOdometry m_odometry =
+ new MecanumDriveOdometry(kDriveKinematics, getAngle());
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {
+ // Sets the distance per pulse for the encoders
+ m_frontLeftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rearLeftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_frontRightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ m_rearRightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+ }
+
+ /**
+ * Returns the angle of the robot as a Rotation2d.
+ *
+ * @return The angle of the robot.
+ */
+ public Rotation2d getAngle() {
+ // Negating the angle because WPILib gyros are CW positive.
+ return Rotation2d.fromDegrees(m_gyro.getAngle() * (kGyroReversed ? 1.0 : -1.0));
+ }
+
+ @Override
+ public void periodic() {
+ // Update the odometry in the periodic block
+ m_odometry.update(getAngle(),
+ new MecanumDriveWheelSpeeds(
+ m_frontLeftEncoder.getRate(),
+ m_rearLeftEncoder.getRate(),
+ m_frontRightEncoder.getRate(),
+ m_rearRightEncoder.getRate()));
+ }
+
+ /**
+ * Returns the currently-estimated pose of the robot.
+ *
+ * @return The pose.
+ */
+ public Pose2d getPose() {
+ return m_odometry.getPoseMeters();
+ }
+
+ /**
+ * Resets the odometry to the specified pose.
+ *
+ * @param pose The pose to which to set the odometry.
+ */
+ public void resetOdometry(Pose2d pose) {
+ m_odometry.resetPosition(pose, getAngle());
+ }
+
+ /**
+ * Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1] and the linear
+ * speeds have no effect on the angular speed.
+ *
+ * @param xSpeed Speed of the robot in the x direction (forward/backwards).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+ if ( fieldRelative ) {
+ m_drive.driveCartesian(ySpeed, xSpeed, rot, -m_gyro.getAngle());
+ } else {
+ m_drive.driveCartesian(ySpeed, xSpeed, rot);
+ }
+
+ }
+
+ /**
+ * Sets the front left drive SpeedController to a voltage.
+ */
+ public void setDriveSpeedControllersVolts(MecanumDriveMotorVoltages volts) {
+ m_frontLeft.setVoltage(volts.frontLeftVoltage);
+ m_rearLeft.setVoltage(volts.rearLeftVoltage);
+ m_frontRight.setVoltage(volts.frontRightVoltage);
+ m_rearRight.setVoltage(volts.rearRightVoltage);
+ }
+
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_frontLeftEncoder.reset();
+ m_rearLeftEncoder.reset();
+ m_frontRightEncoder.reset();
+ m_rearRightEncoder.reset();
+ }
+
+ /**
+ * Gets the front left drive encoder.
+ *
+ * @return the front left drive encoder
+ */
+
+ public Encoder getFrontLeftEncoder() {
+ return m_frontLeftEncoder;
+ }
+
+ /**
+ * Gets the rear left drive encoder.
+ *
+ * @return the rear left drive encoder
+ */
+
+ public Encoder getRearLeftEncoder() {
+ return m_rearLeftEncoder;
+ }
+
+ /**
+ * Gets the front right drive encoder.
+ *
+ * @return the front right drive encoder
+ */
+
+ public Encoder getFrontRightEncoder() {
+ return m_frontRightEncoder;
+ }
+ /**
+ * Gets the rear right drive encoder.
+ *
+ * @return the rear right encoder
+ */
+
+ public Encoder getRearRightEncoder() {
+ return m_rearRightEncoder;
+ }
+
+ /**
+ * Gets the current wheel speeds.
+ *
+ * @return the current wheel speeds in a MecanumDriveWheelSpeeds object.
+ */
+
+ public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
+ return new MecanumDriveWheelSpeeds(m_frontLeftEncoder.getRate(),
+ m_rearLeftEncoder.getRate(),
+ m_frontRightEncoder.getRate(),
+ m_rearRightEncoder.getRate());
+ }
+
+
+ /**
+ * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+ *
+ * @param maxOutput the maximum output to which the drive will be constrained
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_drive.setMaxOutput(maxOutput);
+ }
+
+ /**
+ * Zeroes the heading of the robot.
+ */
+ public void zeroHeading() {
+ m_gyro.reset();
+ }
+
+ /**
+ * Returns the heading of the robot.
+ *
+ * @return the robot's heading in degrees, from 180 to 180
+ */
+ public double getHeading() {
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
+ }
+
+ /**
+ * Returns the turn rate of the robot.
+ *
+ * @return The turn rate of the robot, in degrees per second
+ */
+ public double getTurnRate() {
+ return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
index f7aecdd..7d47655 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
@@ -8,7 +8,6 @@
package edu.wpi.first.wpilibj.examples.ramsetecommand;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -30,12 +29,12 @@
public static final boolean kLeftEncoderReversed = false;
public static final boolean kRightEncoderReversed = true;
- public static final double kTrackwidthMeters = .6;
+ public static final double kTrackwidthMeters = 0.6;
public static final DifferentialDriveKinematics kDriveKinematics =
new DifferentialDriveKinematics(kTrackwidthMeters);
public static final int kEncoderCPR = 1024;
- public static final double kWheelDiameterMeters = .15;
+ public static final double kWheelDiameterMeters = 0.15;
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
@@ -48,11 +47,11 @@
// The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these
// values for your robot.
public static final double ksVolts = 1;
- public static final double kvVoltSecondsPerMeter = .8;
- public static final double kaVoltSecondsSquaredPerMeter = .15;
+ public static final double kvVoltSecondsPerMeter = 0.8;
+ public static final double kaVoltSecondsSquaredPerMeter = 0.15;
// Example value only - as above, this must be tuned for your drive!
- public static final double kPDriveVel = .5;
+ public static final double kPDriveVel = 0.5;
}
public static final class OIConstants {
@@ -63,12 +62,8 @@
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
- public static final DifferentialDriveKinematicsConstraint kAutoPathConstraints =
- new DifferentialDriveKinematicsConstraint(DriveConstants.kDriveKinematics,
- kMaxSpeedMetersPerSecond);
-
// Reasonable baseline values for a RAMSETE follower in units of meters and seconds
public static final double kRamseteB = 2;
- public static final double kRamseteZeta = .7;
+ public static final double kRamseteZeta = 0.7;
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
index f8a3a25..c575e29 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
@@ -13,12 +13,14 @@
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -78,7 +80,7 @@
private void configureButtonBindings() {
// Drive at half speed when the right bumper is held
new JoystickButton(m_driverController, Button.kBumperRight.value)
- .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+ .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
.whenReleased(() -> m_robotDrive.setMaxOutput(1));
}
@@ -90,11 +92,23 @@
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
+
+ // Create a voltage constraint to ensure we don't accelerate too fast
+ var autoVoltageConstraint =
+ new DifferentialDriveVoltageConstraint(
+ new SimpleMotorFeedforward(ksVolts,
+ kvVoltSecondsPerMeter,
+ kaVoltSecondsSquaredPerMeter),
+ kDriveKinematics,
+ 10);
+
// Create config for trajectory
TrajectoryConfig config =
new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
// Add kinematics to ensure max speed is actually obeyed
- .setKinematics(kDriveKinematics);
+ .setKinematics(kDriveKinematics)
+ // Apply the voltage constraint
+ .addConstraint(autoVoltageConstraint);
// An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
@@ -115,20 +129,17 @@
exampleTrajectory,
m_robotDrive::getPose,
new RamseteController(kRamseteB, kRamseteZeta),
- ksVolts,
- kvVoltSecondsPerMeter,
- kaVoltSecondsSquaredPerMeter,
+ new SimpleMotorFeedforward(ksVolts, kvVoltSecondsPerMeter, kaVoltSecondsSquaredPerMeter),
kDriveKinematics,
- m_robotDrive.getLeftEncoder()::getRate,
- m_robotDrive.getRightEncoder()::getRate,
+ m_robotDrive::getWheelSpeeds,
new PIDController(kPDriveVel, 0, 0),
new PIDController(kPDriveVel, 0, 0),
- // RamseteCommand passes volts to the callback, so we have to rescale here
- (left, right) -> m_robotDrive.tankDrive(left / 12., right / 12.),
+ // RamseteCommand passes volts to the callback
+ m_robotDrive::tankDriveVolts,
m_robotDrive
);
// Run path following command, then stop at the end.
- return ramseteCommand.andThen(() -> m_robotDrive.tankDrive(0, 0));
+ return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
index 053fad9..ecb640c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
@@ -19,7 +19,6 @@
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kDriveKinematics;
import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kEncoderDistancePerPulse;
import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kGyroReversed;
import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderPorts;
@@ -57,7 +56,7 @@
private final Gyro m_gyro = new ADXRS450_Gyro();
// Odometry class for tracking robot pose
- DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(kDriveKinematics);
+ private final DifferentialDriveOdometry m_odometry;
/**
* Creates a new DriveSubsystem.
@@ -66,16 +65,16 @@
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+
+ resetEncoders();
+ m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
}
@Override
public void periodic() {
// Update the odometry in the periodic block
- m_odometry.update(new Rotation2d(getHeading()),
- new DifferentialDriveWheelSpeeds(
- m_leftEncoder.getRate(),
- m_rightEncoder.getRate()
- ));
+ m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(),
+ m_rightEncoder.getDistance());
}
/**
@@ -88,12 +87,22 @@
}
/**
+ * Returns the current wheel speeds of the robot.
+ *
+ * @return The current wheel speeds.
+ */
+ public DifferentialDriveWheelSpeeds getWheelSpeeds() {
+ return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
+ }
+
+ /**
* Resets the odometry to the specified pose.
*
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
- m_odometry.resetPosition(pose);
+ resetEncoders();
+ m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
}
/**
@@ -107,14 +116,14 @@
}
/**
- * Drives the robot using tank controls. Does not square inputs to enable composition with
- * external controllers.
+ * Controls the left and right sides of the drive directly with voltages.
*
- * @param left the commanded left output
- * @param right the commanded right output
+ * @param leftVolts the commanded left output
+ * @param rightVolts the commanded right output
*/
- public void tankDrive(double left, double right) {
- m_drive.tankDrive(left, right, false);
+ public void tankDriveVolts(double leftVolts, double rightVolts) {
+ m_leftMotors.setVoltage(leftVolts);
+ m_rightMotors.setVoltage(-rightVolts);
}
/**
@@ -131,7 +140,7 @@
* @return the average of the two encoder readings
*/
public double getAverageEncoderDistance() {
- return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
}
/**
@@ -174,7 +183,7 @@
* @return the robot's heading in degrees, from 180 to 180
*/
public double getHeading() {
- return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1. : 1.);
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
}
/**
@@ -183,6 +192,6 @@
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
- return m_gyro.getRate() * (kGyroReversed ? -1. : 1.);
+ return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
}
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
index 0ec09f0..828a94b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
@@ -37,7 +37,7 @@
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
);
- private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics);
+ private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics, getAngle());
public Drivetrain() {
m_gyro.reset();
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
index 58a0c8d..2969fd4 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
@@ -8,7 +8,8 @@
package edu.wpi.first.wpilibj.examples.swervebot;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -23,11 +24,11 @@
private static final double kModuleMaxAngularAcceleration
= 2 * Math.PI; // radians per second squared
- private final Spark m_driveMotor;
- private final Spark m_turningMotor;
+ private final SpeedController m_driveMotor;
+ private final SpeedController m_turningMotor;
private final Encoder m_driveEncoder = new Encoder(0, 1);
- private final Encoder m_turningEncoder = new Encoder(0, 1);
+ private final Encoder m_turningEncoder = new Encoder(2, 3);
private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
@@ -42,8 +43,8 @@
* @param turningMotorChannel ID for the turning motor.
*/
public SwerveModule(int driveMotorChannel, int turningMotorChannel) {
- m_driveMotor = new Spark(driveMotorChannel);
- m_turningMotor = new Spark(turningMotorChannel);
+ m_driveMotor = new PWMVictorSPX(driveMotorChannel);
+ m_turningMotor = new PWMVictorSPX(turningMotorChannel);
// Set the distance per pulse for the drive encoder. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
new file mode 100644
index 0000000..19fc601
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervecontrollercommand;
+
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kFrontLeftDriveMotorPort = 0;
+ public static final int kRearLeftDriveMotorPort = 2;
+ public static final int kFrontRightDriveMotorPort = 4;
+ public static final int kRearRightDriveMotorPort = 6;
+
+ public static final int kFrontLeftTurningMotorPort = 1;
+ public static final int kRearLeftTurningMotorPort = 3;
+ public static final int kFrontRightTurningMotorPort = 5;
+ public static final int kRearRightTurningMotorPort = 7;
+
+ public static final int[] kFrontLeftTurningEncoderPorts = new int[]{0, 1};
+ public static final int[] kRearLeftTurningEncoderPorts = new int[]{2, 3};
+ public static final int[] kFrontRightTurningEncoderPorts = new int[]{4, 5};
+ public static final int[] kRearRightTurningEncoderPorts = new int[]{5, 6};
+
+ public static final boolean kFrontLeftTurningEncoderReversed = false;
+ public static final boolean kRearLeftTurningEncoderReversed = true;
+ public static final boolean kFrontRightTurningEncoderReversed = false;
+ public static final boolean kRearRightTurningEncoderReversed = true;
+
+ public static final int[] kFrontLeftDriveEncoderPorts = new int[]{7, 8};
+ public static final int[] kRearLeftDriveEncoderPorts = new int[]{9, 10};
+ public static final int[] kFrontRightDriveEncoderPorts = new int[]{11, 12};
+ public static final int[] kRearRightDriveEncoderPorts = new int[]{13, 14};
+
+ public static final boolean kFrontLeftDriveEncoderReversed = false;
+ public static final boolean kRearLeftDriveEncoderReversed = true;
+ public static final boolean kFrontRightDriveEncoderReversed = false;
+ public static final boolean kRearRightDriveEncoderReversed = true;
+
+
+ public static final double kTrackWidth = 0.5;
+ //Distance between centers of right and left wheels on robot
+ public static final double kWheelBase = 0.7;
+ //Distance between front and back wheels on robot
+ public static final SwerveDriveKinematics kDriveKinematics =
+ new SwerveDriveKinematics(
+ new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+ public static final boolean kGyroReversed = false;
+
+ // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+ // These characterization values MUST be determined either experimentally or theoretically
+ // for *your* robot's drive.
+ // The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these
+ // values for your robot.
+ public static final double ksVolts = 1;
+ public static final double kvVoltSecondsPerMeter = 0.8;
+ public static final double kaVoltSecondsSquaredPerMeter = 0.15;
+
+ }
+
+ public static final class ModuleConstants {
+ public static final double kMaxModuleAngularSpeedRadiansPerSecond = 2 * Math.PI;
+ public static final double kMaxModuleAngularAccelerationRadiansPerSecondSquared = 2 * Math.PI;
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterMeters = 0.15;
+ public static final double kDriveEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+
+ public static final double kTurningEncoderDistancePerPulse =
+ // Assumes the encoders are on a 1:1 reduction with the module shaft.
+ (2 * Math.PI) / (double) kEncoderCPR;
+
+ public static final double kPModuleTurningController = 1;
+
+ public static final double kPModuleDriveController = 1;
+
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 1;
+
+ }
+
+ public static final class AutoConstants {
+ public static final double kMaxSpeedMetersPerSecond = 3;
+ public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+ public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
+ public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
+
+ public static final double kPXController = 1;
+ public static final double kPYController = 1;
+ public static final double kPThetaController = 1;
+
+ //Constraint for the motion profilied robot angle controller
+ public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
+ new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond,
+ kMaxAngularSpeedRadiansPerSecondSquared);
+
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java
similarity index 75%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java
index d7f6e26..5c8a1da 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java
@@ -1,18 +1,18 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.iterative;
+package edu.wpi.first.wpilibj.examples.swervecontrollercommand;
import edu.wpi.first.wpilibj.RobotBase;
/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
*/
public final class Main {
private Main() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java
new file mode 100644
index 0000000..3d41ba4
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervecontrollercommand;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private RobotContainer m_robotContainer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
new file mode 100644
index 0000000..d0fcc71
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervecontrollercommand;
+
+import java.util.List;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPThetaController;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPXController;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPYController;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kThetaControllerConstraints;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kDriveKinematics;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants.kDriverControllerPort;
+
+/*
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems
+ private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+ // The driver's controller
+ XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+
+ // Configure default commands
+ // Set the default drive command to split-stick arcade drive
+ m_robotDrive.setDefaultCommand(
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ new RunCommand(() -> m_robotDrive.drive(
+ m_driverController.getY(GenericHID.Hand.kLeft),
+ m_driverController.getX(GenericHID.Hand.kRight),
+ m_driverController.getX(GenericHID.Hand.kLeft), false)));
+
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+ * {@link JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ }
+
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ // Create config for trajectory
+ TrajectoryConfig config =
+ new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
+ // Add kinematics to ensure max speed is actually obeyed
+ .setKinematics(kDriveKinematics);
+
+ // An example trajectory to follow. All units in meters.
+ Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
+ // Start at the origin facing the +X direction
+ new Pose2d(0, 0, new Rotation2d(0)),
+ // Pass through these two interior waypoints, making an 's' curve path
+ List.of(
+ new Translation2d(1, 1),
+ new Translation2d(2, - 1)
+ ),
+ // End 3 meters straight ahead of where we started, facing forward
+ new Pose2d(3, 0, new Rotation2d(0)),
+ config
+ );
+
+ SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
+ exampleTrajectory,
+ m_robotDrive::getPose, //Functional interface to feed supplier
+ kDriveKinematics,
+
+ //Position controllers
+ new PIDController(kPXController, 0, 0),
+ new PIDController(kPYController, 0, 0),
+ new ProfiledPIDController(kPThetaController, 0, 0, kThetaControllerConstraints),
+
+ m_robotDrive::setModuleStates,
+
+ m_robotDrive
+
+ );
+
+ // Run path following command, then stop at the end.
+ return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..ba80a15
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
@@ -0,0 +1,200 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems;
+
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kDriveKinematics;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kGyroReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveMotorPort;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningMotorPort;
+
+@SuppressWarnings("PMD.ExcessiveImports")
+public class DriveSubsystem extends SubsystemBase {
+ //Robot swerve modules
+ private final SwerveModule m_frontLeft = new SwerveModule(kFrontLeftDriveMotorPort,
+ kFrontLeftTurningMotorPort,
+ kFrontLeftDriveEncoderPorts,
+ kFrontLeftTurningEncoderPorts,
+ kFrontLeftDriveEncoderReversed,
+ kFrontLeftTurningEncoderReversed);
+
+ private final SwerveModule m_rearLeft = new SwerveModule(kRearLeftDriveMotorPort,
+ kRearLeftTurningMotorPort,
+ kRearLeftDriveEncoderPorts,
+ kRearLeftTurningEncoderPorts,
+ kRearLeftDriveEncoderReversed,
+ kRearLeftTurningEncoderReversed);
+
+
+ private final SwerveModule m_frontRight = new SwerveModule(kFrontRightDriveMotorPort,
+ kFrontRightTurningMotorPort,
+ kFrontRightDriveEncoderPorts,
+ kFrontRightTurningEncoderPorts,
+ kFrontRightDriveEncoderReversed,
+ kFrontRightTurningEncoderReversed);
+
+ private final SwerveModule m_rearRight = new SwerveModule(kRearRightDriveMotorPort,
+ kRearRightTurningMotorPort,
+ kRearRightDriveEncoderPorts,
+ kRearRightTurningEncoderPorts,
+ kRearRightDriveEncoderReversed,
+ kRearRightTurningEncoderReversed);
+
+ // The gyro sensor
+ private final Gyro m_gyro = new ADXRS450_Gyro();
+
+ // Odometry class for tracking robot pose
+ SwerveDriveOdometry m_odometry =
+ new SwerveDriveOdometry(kDriveKinematics, getAngle());
+
+ /**
+ * Creates a new DriveSubsystem.
+ */
+ public DriveSubsystem() {}
+
+ /**
+ * Returns the angle of the robot as a Rotation2d.
+ *
+ * @return The angle of the robot.
+ */
+ public Rotation2d getAngle() {
+ // Negating the angle because WPILib gyros are CW positive.
+ return Rotation2d.fromDegrees(m_gyro.getAngle() * (kGyroReversed ? 1.0 : -1.0));
+ }
+
+ @Override
+ public void periodic() {
+ // Update the odometry in the periodic block
+ m_odometry.update(
+ new Rotation2d(getHeading()),
+ m_frontLeft.getState(),
+ m_rearLeft.getState(),
+ m_frontRight.getState(),
+ m_rearRight.getState());
+ }
+
+ /**
+ * Returns the currently-estimated pose of the robot.
+ *
+ * @return The pose.
+ */
+ public Pose2d getPose() {
+ return m_odometry.getPoseMeters();
+ }
+
+ /**
+ * Resets the odometry to the specified pose.
+ *
+ * @param pose The pose to which to set the odometry.
+ */
+ public void resetOdometry(Pose2d pose) {
+ m_odometry.resetPosition(pose, getAngle());
+ }
+
+ /**
+ * Method to drive the robot using joystick info.
+ *
+ * @param xSpeed Speed of the robot in the x direction (forward).
+ * @param ySpeed Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+ var swerveModuleStates = kDriveKinematics.toSwerveModuleStates(
+ fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot, getAngle())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot)
+ );
+ SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeedMetersPerSecond);
+ m_frontLeft.setDesiredState(swerveModuleStates[0]);
+ m_frontRight.setDesiredState(swerveModuleStates[1]);
+ m_rearLeft.setDesiredState(swerveModuleStates[2]);
+ m_rearRight.setDesiredState(swerveModuleStates[3]);
+ }
+
+ /**
+ * Sets the swerve ModuleStates.
+ *
+ * @param desiredStates The desired SwerveModule states.
+ */
+ public void setModuleStates(SwerveModuleState[] desiredStates) {
+ SwerveDriveKinematics.normalizeWheelSpeeds(desiredStates, kMaxSpeedMetersPerSecond);
+ m_frontLeft.setDesiredState(desiredStates[0]);
+ m_frontRight.setDesiredState(desiredStates[1]);
+ m_rearLeft.setDesiredState(desiredStates[2]);
+ m_rearRight.setDesiredState(desiredStates[3]);
+ }
+
+ /**
+ * Resets the drive encoders to currently read a position of 0.
+ */
+ public void resetEncoders() {
+ m_frontLeft.resetEncoders();
+ m_rearLeft.resetEncoders();
+ m_frontRight.resetEncoders();
+ m_rearRight.resetEncoders();
+ }
+
+ /**
+ * Zeroes the heading of the robot.
+ */
+ public void zeroHeading() {
+ m_gyro.reset();
+ }
+
+ /**
+ * Returns the heading of the robot.
+ *
+ * @return the robot's heading in degrees, from 180 to 180
+ */
+ public double getHeading() {
+ return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
+ }
+
+ /**
+ * Returns the turn rate of the robot.
+ *
+ * @return The turn rate of the robot, in degrees per second
+ */
+ public double getTurnRate() {
+ return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
new file mode 100644
index 0000000..bf33288
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kDriveEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kPModuleDriveController;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kPModuleTurningController;
+import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kTurningEncoderDistancePerPulse;
+
+public class SwerveModule {
+ private final Spark m_driveMotor;
+ private final Spark m_turningMotor;
+
+ private final Encoder m_driveEncoder;
+ private final Encoder m_turningEncoder;
+
+ private final PIDController m_drivePIDController =
+ new PIDController(kPModuleDriveController, 0, 0);
+
+ //Using a TrapezoidProfile PIDController to allow for smooth turning
+ private final ProfiledPIDController m_turningPIDController
+ = new ProfiledPIDController(kPModuleTurningController, 0, 0,
+ new TrapezoidProfile.Constraints(kMaxModuleAngularSpeedRadiansPerSecond,
+ kMaxModuleAngularAccelerationRadiansPerSecondSquared));
+
+ /**
+ * Constructs a SwerveModule.
+ *
+ * @param driveMotorChannel ID for the drive motor.
+ * @param turningMotorChannel ID for the turning motor.
+ */
+ public SwerveModule(int driveMotorChannel,
+ int turningMotorChannel,
+ int[] driveEncoderPorts,
+ int[] turningEncoderPorts,
+ boolean driveEncoderReversed,
+ boolean turningEncoderReversed) {
+
+ m_driveMotor = new Spark(driveMotorChannel);
+ m_turningMotor = new Spark(turningMotorChannel);
+
+ this.m_driveEncoder = new Encoder(driveEncoderPorts[0], driveEncoderPorts[1]);
+
+ this.m_turningEncoder = new Encoder(turningEncoderPorts[0], turningEncoderPorts[1]);
+
+ // Set the distance per pulse for the drive encoder. We can simply use the
+ // distance traveled for one rotation of the wheel divided by the encoder
+ // resolution.
+ m_driveEncoder.setDistancePerPulse(kDriveEncoderDistancePerPulse);
+
+ //Set whether drive encoder should be reversed or not
+ m_driveEncoder.setReverseDirection(driveEncoderReversed);
+
+ // Set the distance (in this case, angle) per pulse for the turning encoder.
+ // This is the the angle through an entire rotation (2 * wpi::math::pi)
+ // divided by the encoder resolution.
+ m_turningEncoder.setDistancePerPulse(kTurningEncoderDistancePerPulse);
+
+ //Set whether turning encoder should be reversed or not
+ m_turningEncoder.setReverseDirection(turningEncoderReversed);
+
+ // Limit the PID Controller's input range between -pi and pi and set the input
+ // to be continuous.
+ m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
+ }
+
+ /**
+ * Returns the current state of the module.
+ *
+ * @return The current state of the module.
+ */
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get()));
+ }
+
+ /**
+ * Sets the desired state for the module.
+ *
+ * @param state Desired state with speed and angle.
+ */
+ public void setDesiredState(SwerveModuleState state) {
+ // Calculate the drive output from the drive PID controller.
+ final var driveOutput = m_drivePIDController.calculate(
+ m_driveEncoder.getRate(), state.speedMetersPerSecond);
+
+ // Calculate the turning motor output from the turning PID controller.
+ final var turnOutput = m_turningPIDController.calculate(
+ m_turningEncoder.get(), state.angle.getRadians()
+ );
+
+ // Calculate the turning motor output from the turning PID controller.
+ m_driveMotor.set(driveOutput);
+ m_turningMotor.set(turnOutput);
+ }
+
+ /**
+ * Zeros all the SwerveModule encoders.
+ */
+
+ public void resetEncoders() {
+ m_driveEncoder.reset();
+ m_turningEncoder.reset();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java
index d7f6e26..3adcee9 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.iterative;
+package edu.wpi.first.wpilibj.examples.tankdrivexboxcontroller;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
new file mode 100644
index 0000000..f0a9e76
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.tankdrivexboxcontroller;
+
+import edu.wpi.first.wpilibj.GenericHID.Hand;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with tank steering and an Xbox controller.
+ */
+public class Robot extends TimedRobot {
+ private final PWMVictorSPX m_leftMotor = new PWMVictorSPX(0);
+ private final PWMVictorSPX m_rightMotor = new PWMVictorSPX(1);
+ private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+ private final XboxController m_driverController = new XboxController(0);
+
+ @Override
+ public void teleopPeriodic() {
+ // Drive with tank drive.
+ // That means that the Y axis of the left stick moves the left side
+ // of the robot forward and backward, and the Y axis of the right stick
+ // moves the right side of the robot forward and backward.
+ m_robotDrive.tankDrive(m_driverController.getY(Hand.kLeft),
+ m_driverController.getY(Hand.kRight));
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
index e03c66c..2cae288 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,6 +8,7 @@
package edu.wpi.first.wpilibj.examples.ultrasonic;
import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.MedianFilter;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -31,6 +32,9 @@
private static final int kRightMotorPort = 1;
private static final int kUltrasonicPort = 0;
+ // median filter to discard outliers; filters over 10 samples
+ private final MedianFilter m_filter = new MedianFilter(10);
+
private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
private final DifferentialDrive m_robotDrive
= new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
@@ -43,7 +47,9 @@
@Override
public void teleopPeriodic() {
// sensor returns a value from 0-4095 that is scaled to inches
- double currentDistance = m_ultrasonic.getValue() * kValueToInches;
+ // returned value is filtered with a rolling median filter, since ultrasonics
+ // tend to be quite noisy and susceptible to sudden outliers
+ double currentDistance = m_filter.calculate(m_ultrasonic.getValue()) * kValueToInches;
// convert distance error to a motor speed
double currentSpeed = (kHoldDistance - currentDistance) * kP;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
index 225f3ad..b023429 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
@@ -8,6 +8,7 @@
package edu.wpi.first.wpilibj.examples.ultrasonicpid;
import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.MedianFilter;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.controller.PIDController;
@@ -37,6 +38,9 @@
private static final int kRightMotorPort = 1;
private static final int kUltrasonicPort = 0;
+ // median filter to discard outliers; filters over 5 samples
+ private final MedianFilter m_filter = new MedianFilter(5);
+
private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
private final DifferentialDrive m_robotDrive
= new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
@@ -51,8 +55,10 @@
@Override
public void teleopPeriodic() {
+ // returned value is filtered with a rolling median filter, since ultrasonics
+ // tend to be quite noisy and susceptible to sudden outliers
double pidOutput
- = m_pidController.calculate(m_ultrasonic.getAverageVoltage());
+ = m_pidController.calculate(m_filter.calculate(m_ultrasonic.getVoltage()));
m_robotDrive.arcadeDrive(pidOutput, 0);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java
deleted file mode 100644
index 630be9a..0000000
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java
+++ /dev/null
@@ -1,99 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.iterative;
-
-import edu.wpi.first.wpilibj.IterativeRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the IterativeRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the build.gradle file in the
- * project.
- */
-@SuppressWarnings("deprecation")
-public class Robot extends IterativeRobot {
- private static final String kDefaultAuto = "Default";
- private static final String kCustomAuto = "My Auto";
- private String m_autoSelected;
- private final SendableChooser<String> m_chooser = new SendableChooser<>();
-
- /**
- * This function is run when the robot is first started up and should be
- * used for any initialization code.
- */
- @Override
- public void robotInit() {
- m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
- m_chooser.addOption("My Auto", kCustomAuto);
- SmartDashboard.putData("Auto choices", m_chooser);
- }
-
- /**
- * This function is called every robot packet, no matter the mode. Use
- * this for items like diagnostics that you want ran during disabled,
- * autonomous, teleoperated and test.
- *
- * <p>This runs after the mode specific periodic functions, but before
- * LiveWindow and SmartDashboard integrated updating.
- */
- @Override
- public void robotPeriodic() {
- }
-
- /**
- * This autonomous (along with the chooser code above) shows how to select
- * between different autonomous modes using the dashboard. The sendable
- * chooser code works with the Java SmartDashboard. If you prefer the
- * LabVIEW Dashboard, remove all of the chooser code and uncomment the
- * getString line to get the auto name from the text box below the Gyro
- *
- * <p>You can add additional auto modes by adding additional comparisons to
- * the switch structure below with additional strings. If using the
- * SendableChooser make sure to add them to the chooser code above as well.
- */
- @Override
- public void autonomousInit() {
- m_autoSelected = m_chooser.getSelected();
- // autoSelected = SmartDashboard.getString("Auto Selector",
- // defaultAuto);
- System.out.println("Auto selected: " + m_autoSelected);
- }
-
- /**
- * This function is called periodically during autonomous.
- */
- @Override
- public void autonomousPeriodic() {
- switch (m_autoSelected) {
- case kCustomAuto:
- // Put custom auto code here
- break;
- case kDefaultAuto:
- default:
- // Put default auto code here
- break;
- }
- }
-
- /**
- * This function is called periodically during operator control.
- */
- @Override
- public void teleopPeriodic() {
- }
-
- /**
- * This function is called periodically during test mode.
- */
- @Override
- public void testPeriodic() {
- }
-}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Main.java
similarity index 88%
copy from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
copy to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Main.java
index d7f6e26..67d8377 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Main.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Main.java
@@ -1,11 +1,11 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
-package edu.wpi.first.wpilibj.templates.iterative;
+package edu.wpi.first.wpilibj.templates.oldcommandbased;
import edu.wpi.first.wpilibj.RobotBase;
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/OI.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/OI.java
new file mode 100644
index 0000000..d4c7f91
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/OI.java
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.oldcommandbased;
+
+/**
+ * This class is the glue that binds the controls on the physical operator
+ * interface to the commands and command groups that allow control of the robot.
+ */
+public class OI {
+ //// CREATING BUTTONS
+ // One type of button is a joystick button which is any button on a
+ //// joystick.
+ // You create one by telling it which joystick it's on and which button
+ // number it is.
+ // Joystick stick = new Joystick(port);
+ // Button button = new JoystickButton(stick, buttonNumber);
+
+ // There are a few additional built in buttons you can use. Additionally,
+ // by subclassing Button you can create custom triggers and bind those to
+ // commands the same as any other Button.
+
+ //// TRIGGERING COMMANDS WITH BUTTONS
+ // Once you have a button, it's trivial to bind it to a button in one of
+ // three ways:
+
+ // Start the command when the button is pressed and let it run the command
+ // until it is finished as determined by it's isFinished method.
+ // button.whenPressed(new ExampleCommand());
+
+ // Run the command while the button is being held down and interrupt it once
+ // the button is released.
+ // button.whileHeld(new ExampleCommand());
+
+ // Start the command when the button is released and let it run the command
+ // until it is finished as determined by it's isFinished method.
+ // button.whenReleased(new ExampleCommand());
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Robot.java
new file mode 100644
index 0000000..8f000ac
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Robot.java
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.oldcommandbased;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.templates.oldcommandbased.commands.ExampleCommand;
+import edu.wpi.first.wpilibj.templates.oldcommandbased.subsystems.ExampleSubsystem;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the TimedRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ public static ExampleSubsystem m_subsystem = new ExampleSubsystem();
+ public static OI m_oi;
+
+ Command m_autonomousCommand;
+ SendableChooser<Command> m_chooser = new SendableChooser<>();
+
+ /**
+ * This function is run when the robot is first started up and should be
+ * used for any initialization code.
+ */
+ @Override
+ public void robotInit() {
+ m_oi = new OI();
+ m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
+ // chooser.addOption("My Auto", new MyAutoCommand());
+ SmartDashboard.putData("Auto mode", m_chooser);
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want ran during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ * You can use it to reset any subsystem information you want to clear when
+ * the robot is disabled.
+ */
+ @Override
+ public void disabledInit() {
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ Scheduler.getInstance().run();
+ }
+
+ /**
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable
+ * chooser code works with the Java SmartDashboard. If you prefer the
+ * LabVIEW Dashboard, remove all of the chooser code and uncomment the
+ * getString code to get the auto name from the text box below the Gyro
+ *
+ * <p>You can add additional auto modes by adding additional commands to the
+ * chooser code above (like the commented example) or additional comparisons
+ * to the switch structure below with additional strings & commands.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_chooser.getSelected();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.start();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ Scheduler.getInstance().run();
+ }
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+ Scheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/RobotMap.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/RobotMap.java
new file mode 100644
index 0000000..9f33b02
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/RobotMap.java
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.oldcommandbased;
+
+/**
+ * The RobotMap is a mapping from the ports sensors and actuators are wired into
+ * to a variable name. This provides flexibility changing wiring, makes checking
+ * the wiring easier and significantly reduces the number of magic numbers
+ * floating around.
+ */
+public class RobotMap {
+ // For example to map the left and right motors, you could define the
+ // following variables to use with your drivetrain subsystem.
+ // public static int leftMotor = 1;
+ // public static int rightMotor = 2;
+
+ // If you are using multiple modules, make sure to define both the port
+ // number and the module. For example you with a rangefinder:
+ // public static int rangefinderPort = 1;
+ // public static int rangefinderModule = 1;
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/commands/ExampleCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/commands/ExampleCommand.java
new file mode 100644
index 0000000..0d84987
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/commands/ExampleCommand.java
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.oldcommandbased.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.templates.oldcommandbased.Robot;
+
+/**
+ * An example command. You can replace me with your own command.
+ */
+public class ExampleCommand extends Command {
+ public ExampleCommand() {
+ // Use requires() here to declare subsystem dependencies
+ requires(Robot.m_subsystem);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/subsystems/ExampleSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/subsystems/ExampleSubsystem.java
new file mode 100644
index 0000000..f3d1de9
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/subsystems/ExampleSubsystem.java
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.oldcommandbased.subsystems;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ * An example subsystem. You can replace with me with your own subsystem.
+ */
+public class ExampleSubsystem extends Subsystem {
+ // Put methods for controlling this subsystem
+ // here. Call these from Commands.
+
+
+ @Override
+ protected void initDefaultCommand() {
+ // Set the default command for a subsystem here.
+ // setDefaultCommand(new MySpecialCommand());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
index 615af87..783f76b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
@@ -33,6 +33,8 @@
public void test() {
}
+ private volatile boolean m_exit;
+
@SuppressWarnings("PMD.CyclomaticComplexity")
@Override
public void startCompetition() {
@@ -41,7 +43,7 @@
// Tell the DS that the robot is ready to be enabled
HAL.observeUserProgramStarting();
- while (!Thread.currentThread().isInterrupted()) {
+ while (!Thread.currentThread().isInterrupted() && !m_exit) {
if (isDisabled()) {
m_ds.InDisabled(true);
disabled();
@@ -77,4 +79,9 @@
}
}
}
+
+ @Override
+ public void endCompetition() {
+ m_exit = true;
+ }
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
index e4dbc1d..e0ebce7 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
@@ -1,15 +1,5 @@
[
{
- "name": "Iterative Robot",
- "description": "Iterative style",
- "tags": [
- "Iterative"
- ],
- "foldername": "iterative",
- "gradlebase": "java",
- "mainclass": "Main"
- },
- {
"name": "Timed Robot",
"description": "Timed style",
"tags": [
@@ -17,7 +7,8 @@
],
"foldername": "timed",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Timed Skeleton (Advanced)",
@@ -27,7 +18,8 @@
],
"foldername": "timedskeleton",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "RobotBase Skeleton (Advanced)",
@@ -37,7 +29,8 @@
],
"foldername": "robotbaseskeleton",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 1
},
{
"name": "Command Robot",
@@ -47,6 +40,18 @@
],
"foldername": "commandbased",
"gradlebase": "java",
- "mainclass": "Main"
+ "mainclass": "Main",
+ "commandversion": 2
+ },
+ {
+ "name": "Old Command Robot",
+ "description": "Old-command style (deprecated)",
+ "tags": [
+ "Command"
+ ],
+ "foldername": "oldcommandbased",
+ "gradlebase": "java",
+ "mainclass": "Main",
+ "commandversion": 1
}
]