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 manifest file in the resource
+ * directory.
+ */
+public class Robot extends IterativeRobot {
+
+ Command m_autonomousCommand;
+ public static OI oi;
+
+ // Initialize the subsystems
+ public static DriveTrain drivetrain = new DriveTrain();
+ public static Collector collector = new Collector();
+ public static Shooter shooter = new Shooter();
+ public static Pneumatics pneumatics = new Pneumatics();
+ public static Pivot pivot = new Pivot();
+
+ public SendableChooser NOTE: It doesn't wait for the claw to close since there is no sensor to
+ * detect that.
+ */
+public class CloseClaw extends InstantCommand {
+
+ public CloseClaw() {
+ requires(Robot.collector);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.collector.close();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java
new file mode 100644
index 0000000000..d10d0160f2
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
+
+/**
+ * Get the robot set to collect balls.
+ */
+public class Collect extends CommandGroup {
+ public Collect() {
+ addSequential(new SetCollectionSpeed(Collector.kForward));
+ addParallel(new CloseClaw());
+ addSequential(new SetPivotSetpoint(Pivot.kCollect));
+ addSequential(new WaitForBall());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java
new file mode 100644
index 0000000000..fbd5ea737c
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * Drive over the line and then shoot the ball. If the hot goal is not detected,
+ * it will wait briefly.
+ */
+public class DriveAndShootAutonomous extends CommandGroup {
+ public DriveAndShootAutonomous() {
+ addSequential(new CloseClaw());
+ addSequential(new WaitForPressure(), 2);
+ if (Robot.isReal()) {
+ // NOTE: Simulation doesn't currently have the concept of hot.
+ addSequential(new CheckForHotGoal(2));
+ }
+ addSequential(new SetPivotSetpoint(45));
+ addSequential(new DriveForward(8, 0.3));
+ addSequential(new Shoot());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java
new file mode 100644
index 0000000000..0a85c6cf7e
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * This command drives the robot over a given distance with simple proportional
+ * control This command will drive a given distance limiting to a maximum speed.
+ */
+public class DriveForward extends Command {
+ private double m_driveForwardSpeed;
+ private double m_distance;
+ private double m_error;
+ private static final double kTolerance = 0.1;
+ private static final double kP = -1.0 / 5.0;
+
+ public DriveForward() {
+ this(10, 0.5);
+ }
+
+ public DriveForward(double dist) {
+ this(dist, 0.5);
+ }
+
+ public DriveForward(double dist, double maxSpeed) {
+ requires(Robot.drivetrain);
+ m_distance = dist;
+ m_driveForwardSpeed = maxSpeed;
+ }
+
+ @Override
+ protected void initialize() {
+ Robot.drivetrain.getRightEncoder().reset();
+ setTimeout(2);
+ }
+
+ @Override
+ protected void execute() {
+ m_error = m_distance - Robot.drivetrain.getRightEncoder().getDistance();
+ if (m_driveForwardSpeed * kP * m_error >= m_driveForwardSpeed) {
+ Robot.drivetrain.tankDrive(m_driveForwardSpeed, m_driveForwardSpeed);
+ } else {
+ Robot.drivetrain.tankDrive(m_driveForwardSpeed * kP * m_error,
+ m_driveForwardSpeed * kP * m_error);
+ }
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return Math.abs(m_error) <= kTolerance || isTimedOut();
+ }
+
+ @Override
+ protected void end() {
+ Robot.drivetrain.stop();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java
new file mode 100644
index 0000000000..7dc5891de8
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * This command allows PS3 joystick to drive the robot. It is always running
+ * except when interrupted by another command.
+ */
+public class DriveWithJoystick extends Command {
+ public DriveWithJoystick() {
+ requires(Robot.drivetrain);
+ }
+
+ @Override
+ protected void execute() {
+ Robot.drivetrain.tankDrive(Robot.oi.getJoystick());
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return false;
+ }
+
+ @Override
+ protected void end() {
+ Robot.drivetrain.stop();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java
new file mode 100644
index 0000000000..b0f06e18c5
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.TimedCommand;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * Extend the shooter and then retract it after a second.
+ */
+public class ExtendShooter extends TimedCommand {
+ public ExtendShooter() {
+ super(1);
+ requires(Robot.shooter);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.shooter.extendBoth();
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ Robot.shooter.retractBoth();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java
new file mode 100644
index 0000000000..9e6af5e3f5
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
+
+/**
+ * Spit the ball out into the low goal assuming that the robot is in front of
+ * it.
+ */
+public class LowGoal extends CommandGroup {
+ public LowGoal() {
+ addSequential(new SetPivotSetpoint(Pivot.kLowGoal));
+ addSequential(new SetCollectionSpeed(Collector.kReverse));
+ addSequential(new ExtendShooter());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java
new file mode 100644
index 0000000000..4e4da5e72d
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * Opens the claw.
+ */
+public class OpenClaw extends Command {
+ public OpenClaw() {
+ requires(Robot.collector);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.collector.open();
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return Robot.collector.isOpen();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java
new file mode 100644
index 0000000000..5303b253cf
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * This command sets the collector rollers spinning at the given speed. Since
+ * there is no sensor for detecting speed, it finishes immediately. As a result,
+ * the spinners may still be adjusting their speed.
+ */
+public class SetCollectionSpeed extends InstantCommand {
+ private double m_speed;
+
+ public SetCollectionSpeed(double speed) {
+ requires(Robot.collector);
+ this.m_speed = speed;
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.collector.setSpeed(m_speed);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java
new file mode 100644
index 0000000000..019356b503
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * Moves the pivot to a given angle. This command finishes when it is within the
+ * tolerance, but leaves the PID loop running to maintain the position. Other
+ * commands using the pivot should make sure they disable PID!
+ */
+public class SetPivotSetpoint extends Command {
+ private double m_setpoint;
+
+ public SetPivotSetpoint(double setpoint) {
+ this.m_setpoint = setpoint;
+ requires(Robot.pivot);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.pivot.enable();
+ Robot.pivot.setSetpoint(m_setpoint);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return Robot.pivot.onTarget();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java
new file mode 100644
index 0000000000..9271f9fbef
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
+
+/**
+ * Shoot the ball at the current angle.
+ */
+public class Shoot extends CommandGroup {
+ public Shoot() {
+ addSequential(new WaitForPressure());
+ addSequential(new SetCollectionSpeed(Collector.kStop));
+ addSequential(new OpenClaw());
+ addSequential(new ExtendShooter());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java
new file mode 100644
index 0000000000..4c692dcf05
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * Wait until the collector senses that it has the ball. This command does
+ * nothing and is intended to be used in command groups to wait for this
+ * condition.
+ */
+public class WaitForBall extends Command {
+ public WaitForBall() {
+ requires(Robot.collector);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return Robot.collector.hasBall();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java
new file mode 100644
index 0000000000..979193ccff
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * Wait until the pneumatics are fully pressurized. This command does nothing
+ * and is intended to be used in command groups to wait for this condition.
+ */
+public class WaitForPressure extends Command {
+ public WaitForPressure() {
+ requires(Robot.pneumatics);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return Robot.pneumatics.isPressurized();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java
new file mode 100644
index 0000000000..704c904aaf
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+
+/**
+ * The Collector subsystem has one motor for the rollers, a limit switch for
+ * ball detection, a piston for opening and closing the claw, and a reed switch
+ * to check if the piston is open.
+ */
+public class Collector extends Subsystem {
+ // Constants for some useful speeds
+ public static final double kForward = 1;
+ public static final double kStop = 0;
+ public static final double kReverse = -1;
+
+ // Subsystem devices
+ private SpeedController m_rollerMotor = new Victor(6);
+ private DigitalInput m_ballDetector = new DigitalInput(10);
+ private DigitalInput m_openDetector = new DigitalInput(6);
+ private Solenoid m_piston = new Solenoid(1, 1);
+
+ public Collector() {
+ // Put everything to the LiveWindow for testing.
+ LiveWindow.addActuator("Collector", "Roller Motor", (Victor) m_rollerMotor);
+ LiveWindow.addSensor("Collector", "Ball Detector", m_ballDetector);
+ LiveWindow.addSensor("Collector", "Claw Open Detector", m_openDetector);
+ LiveWindow.addActuator("Collector", "Piston", m_piston);
+ }
+
+ /**
+ * Whether or not the robot has the ball.
+ *
+ * NOTE: The current simulation model uses the the lower part of the claw
+ * since the limit switch wasn't exported. At some point, this will be
+ * updated.
+ *
+ * @return Whether or not the robot has the ball.
+ */
+ public boolean hasBall() {
+ return m_ballDetector.get(); // TODO: prepend ! to reflect real robot
+ }
+
+ /**
+ * Set the speed to spin the collector rollers.
+ *
+ * @param speed The speed to spin the rollers.
+ */
+ public void setSpeed(double speed) {
+ m_rollerMotor.set(-speed);
+ }
+
+ /**
+ * Stop the rollers from spinning.
+ */
+ public void stop() {
+ m_rollerMotor.set(0);
+ }
+
+ /**
+ * Wether or not the claw is open.
+ *
+ * @return Whether or not the claw is open.
+ */
+ public boolean isOpen() {
+ return m_openDetector.get(); // TODO: prepend ! to reflect real robot
+ }
+
+ /**
+ * Open the claw up (For shooting).
+ */
+ public void open() {
+ m_piston.set(true);
+ }
+
+ /**
+ * Close the claw (For collecting and driving).
+ */
+ public void close() {
+ m_piston.set(false);
+ }
+
+ /**
+ * No default command.
+ */
+ @Override
+ protected void initDefaultCommand() {
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java
new file mode 100644
index 0000000000..fe10fcdbed
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java
@@ -0,0 +1,145 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.CounterBase.EncodingType;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveWithJoystick;
+
+/**
+ * The DriveTrain subsystem controls the robot's chassis and reads in
+ * information about it's speed and position.
+ */
+public class DriveTrain extends Subsystem {
+ // Subsystem devices
+ private SpeedController m_frontLeftCIM = new Victor(1);
+ private SpeedController m_frontRightCIM = new Victor(2);
+ private SpeedController m_rearLeftCIM = new Victor(3);
+ private SpeedController m_rearRightCIM = new Victor(4);
+ private SpeedControllerGroup m_leftCIMs = new SpeedControllerGroup(
+ m_frontLeftCIM, m_rearLeftCIM);
+ private SpeedControllerGroup m_rightCIMs = new SpeedControllerGroup(
+ m_frontRightCIM, m_rearRightCIM);
+ private DifferentialDrive m_drive;
+ private Encoder m_rightEncoder = new Encoder(1, 2, true, EncodingType.k4X);
+ private Encoder m_leftEncoder = new Encoder(3, 4, false, EncodingType.k4X);
+ private AnalogGyro m_gyro = new AnalogGyro(2);
+
+ public DriveTrain() {
+ // Configure drive motors
+ LiveWindow.addActuator("DriveTrain", "Front Left CIM",
+ (Victor) m_frontLeftCIM);
+ LiveWindow.addActuator("DriveTrain", "Front Right CIM",
+ (Victor) m_frontRightCIM);
+ LiveWindow.addActuator("DriveTrain", "Back Left CIM",
+ (Victor) m_rearLeftCIM);
+ LiveWindow.addActuator("DriveTrain", "Back Right CIM",
+ (Victor) m_rearRightCIM);
+
+ // Configure the DifferentialDrive to reflect the fact that all motors
+ // are wired backwards (right is inverted in DifferentialDrive).
+ m_leftCIMs.setInverted(true);
+ m_drive = new DifferentialDrive(m_leftCIMs, m_rightCIMs);
+ m_drive.setSafetyEnabled(true);
+ m_drive.setExpiration(0.1);
+ m_drive.setMaxOutput(1.0);
+
+ // Configure encoders
+ m_rightEncoder.setPIDSourceType(PIDSourceType.kDisplacement);
+ m_leftEncoder.setPIDSourceType(PIDSourceType.kDisplacement);
+
+ if (Robot.isReal()) { // Converts to feet
+ m_rightEncoder.setDistancePerPulse(0.0785398);
+ m_leftEncoder.setDistancePerPulse(0.0785398);
+ } else {
+ // Convert to feet 4in diameter wheels with 360 tick sim encoders
+ m_rightEncoder.setDistancePerPulse(
+ (4.0/* in */ * Math.PI) / (360.0 * 12.0/* in/ft */));
+ m_leftEncoder.setDistancePerPulse(
+ (4.0/* in */ * Math.PI) / (360.0 * 12.0/* in/ft */));
+ }
+
+ LiveWindow.addSensor("DriveTrain", "Right Encoder", m_rightEncoder);
+ LiveWindow.addSensor("DriveTrain", "Left Encoder", m_leftEncoder);
+
+ // Configure gyro
+ if (Robot.isReal()) {
+ m_gyro.setSensitivity(0.007); // TODO: Handle more gracefully?
+ }
+ LiveWindow.addSensor("DriveTrain", "Gyro", m_gyro);
+ }
+
+ /**
+ * When other commands aren't using the drivetrain, allow tank drive with
+ * the joystick.
+ */
+ @Override
+ public void initDefaultCommand() {
+ setDefaultCommand(new DriveWithJoystick());
+ }
+
+ /**
+ * Tank drive using a PS3 joystick.
+ *
+ * @param joy PS3 style joystick to use as the input for tank drive.
+ */
+ public void tankDrive(Joystick joy) {
+ m_drive.tankDrive(joy.getY(), joy.getRawAxis(4));
+ }
+
+ /**
+ * Tank drive using individual joystick axes.
+ *
+ * @param leftAxis Left sides value
+ * @param rightAxis Right sides value
+ */
+ public void tankDrive(double leftAxis, double rightAxis) {
+ m_drive.tankDrive(leftAxis, rightAxis);
+ }
+
+ /**
+ * Stop the drivetrain from moving.
+ */
+ public void stop() {
+ m_drive.tankDrive(0, 0);
+ }
+
+ /**
+ * The encoder getting the distance and speed of left side of the
+ * drivetrain.
+ */
+ public Encoder getLeftEncoder() {
+ return m_leftEncoder;
+ }
+
+ /**
+ * The encoder getting the distance and speed of right side of the
+ * drivetrain.
+ */
+ public Encoder getRightEncoder() {
+ return m_rightEncoder;
+ }
+
+ /**
+ * The current angle of the drivetrain as measured by the Gyro.
+ */
+ public double getAngle() {
+ return m_gyro.getAngle();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java
new file mode 100644
index 0000000000..ddaa2db753
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.command.PIDSubsystem;
+import edu.wpi.first.wpilibj.interfaces.Potentiometer;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * The Pivot subsystem contains the Van-door motor and the pot for PID control
+ * of angle of the pivot and claw.
+ */
+public class Pivot extends PIDSubsystem {
+ // Constants for some useful angles
+ public static final double kCollect = 105;
+ public static final double kLowGoal = 90;
+ public static final double kShoot = 45;
+ public static final double kShootNear = 30;
+
+ // Sensors for measuring the position of the pivot.
+ private DigitalInput m_upperLimitSwitch = new DigitalInput(13);
+ private DigitalInput m_lowerLimitSwitch = new DigitalInput(12);
+
+ // 0 degrees is vertical facing up.
+ // Angle increases the more forward the pivot goes.
+ private Potentiometer m_pot = new AnalogPotentiometer(1);
+
+ // Motor to move the pivot.
+ private SpeedController m_motor = new Victor(5);
+
+ public Pivot() {
+ super("Pivot", 7.0, 0.0, 8.0);
+ setAbsoluteTolerance(0.005);
+ getPIDController().setContinuous(false);
+ if (Robot.isSimulation()) { // PID is different in simulation.
+ getPIDController().setPID(0.5, 0.001, 2);
+ setAbsoluteTolerance(5);
+ }
+
+ // Put everything to the LiveWindow for testing.
+ LiveWindow.addSensor("Pivot", "Upper Limit Switch", m_upperLimitSwitch);
+ LiveWindow.addSensor("Pivot", "Lower Limit Switch", m_lowerLimitSwitch);
+ LiveWindow.addSensor("Pivot", "Pot", (AnalogPotentiometer) m_pot);
+ LiveWindow.addActuator("Pivot", "Motor", (Victor) m_motor);
+ LiveWindow.addActuator("Pivot", "PIDSubsystem Controller",
+ getPIDController());
+ }
+
+ /**
+ * No default command, if PID is enabled, the current setpoint will be
+ * maintained.
+ */
+ @Override
+ public void initDefaultCommand() {
+ }
+
+ /**
+ * The angle read in by the potentiometer.
+ */
+ @Override
+ protected double returnPIDInput() {
+ return m_pot.get();
+ }
+
+ /**
+ * Set the motor speed based off of the PID output.
+ */
+ @Override
+ protected void usePIDOutput(double output) {
+ m_motor.pidWrite(output);
+ }
+
+ /**
+ * If the pivot is at its upper limit.
+ */
+ public boolean isAtUpperLimit() {
+ // TODO: inverted from real robot (prefix with !)
+ return m_upperLimitSwitch.get();
+ }
+
+ /**
+ * If the pivot is at its lower limit.
+ */
+ public boolean isAtLowerLimit() {
+ // TODO: inverted from real robot (prefix with !)
+ return m_lowerLimitSwitch.get();
+ }
+
+ /**
+ * The current angle of the pivot.
+ */
+ public double getAngle() {
+ return m_pot.get();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java
new file mode 100644
index 0000000000..6ee1046b83
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * The Pneumatics subsystem contains a pressure sensor.
+ *
+ * NOTE: The simulator currently doesn't support the compressor or pressure
+ * sensors.
+ */
+public class Pneumatics extends Subsystem {
+ AnalogInput m_pressureSensor = new AnalogInput(3);
+
+ private static final double kMaxPressure = 2.55;
+
+ public Pneumatics() {
+ LiveWindow.addSensor("Pneumatics", "Pressure Sensor", m_pressureSensor);
+ }
+
+ /**
+ * No default command.
+ */
+ @Override
+ public void initDefaultCommand() {
+ }
+
+ /**
+ * Whether or not the system is fully pressurized.
+ */
+ public boolean isPressurized() {
+ if (Robot.isReal()) {
+ return kMaxPressure <= m_pressureSensor.getVoltage();
+ } else {
+ return true; // NOTE: Simulation always has full pressure
+ }
+ }
+
+ /**
+ * Puts the pressure on the SmartDashboard.
+ */
+ public void writePressure() {
+ SmartDashboard.putNumber("Pressure", m_pressureSensor.getVoltage());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java
new file mode 100644
index 0000000000..61d43fcea3
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java
@@ -0,0 +1,176 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+
+/**
+ * The Shooter subsystem handles shooting. The mechanism for shooting is
+ * slightly complicated because it has to pneumatic cylinders for shooting, and
+ * a third latch to allow the pressure to partially build up and reduce the
+ * effect of the airflow. For shorter shots, when full power isn't needed, only
+ * one cylinder fires.
+ *
+ * NOTE: Simulation currently approximates this as as single pneumatic
+ * cylinder and ignores the latch.
+ */
+public class Shooter extends Subsystem {
+ // Devices
+ DoubleSolenoid m_piston1 = new DoubleSolenoid(1, 3, 4);
+ DoubleSolenoid m_piston2 = new DoubleSolenoid(1, 5, 6);
+ Solenoid m_latchPiston = new Solenoid(1, 2);
+ DigitalInput m_piston1ReedSwitchFront = new DigitalInput(9);
+ DigitalInput m_piston1ReedSwitchBack = new DigitalInput(11);
+ //NOTE: currently ignored in simulation
+ DigitalInput m_hotGoalSensor = new DigitalInput(3);
+
+ public Shooter() {
+ // Put everything to the LiveWindow for testing.
+ LiveWindow.addSensor("Shooter", "Hot Goal Sensor", m_hotGoalSensor);
+ LiveWindow.addSensor("Shooter", "Piston1 Reed Switch Front ",
+ m_piston1ReedSwitchFront);
+ LiveWindow.addSensor("Shooter", "Piston1 Reed Switch Back ",
+ m_piston1ReedSwitchBack);
+ LiveWindow.addActuator("Shooter", "Latch Piston", m_latchPiston);
+ }
+
+ /**
+ * No default command.
+ */
+ @Override
+ public void initDefaultCommand() {
+ }
+
+ /**
+ * Extend both solenoids to shoot.
+ */
+ public void extendBoth() {
+ m_piston1.set(DoubleSolenoid.Value.kForward);
+ m_piston2.set(DoubleSolenoid.Value.kForward);
+ }
+
+ /**
+ * Retract both solenoids to prepare to shoot.
+ */
+ public void retractBoth() {
+ m_piston1.set(DoubleSolenoid.Value.kReverse);
+ m_piston2.set(DoubleSolenoid.Value.kReverse);
+ }
+
+ /**
+ * Extend solenoid 1 to shoot.
+ */
+ public void extend1() {
+ m_piston1.set(DoubleSolenoid.Value.kForward);
+ }
+
+ /**
+ * Retract solenoid 1 to prepare to shoot.
+ */
+ public void retract1() {
+ m_piston1.set(DoubleSolenoid.Value.kReverse);
+ }
+
+ /**
+ * Extend solenoid 2 to shoot.
+ */
+ public void extend2() {
+ m_piston2.set(DoubleSolenoid.Value.kReverse);
+ }
+
+ /**
+ * Retract solenoid 2 to prepare to shoot.
+ */
+ public void retract2() {
+ m_piston2.set(DoubleSolenoid.Value.kForward);
+ }
+
+ /**
+ * Turns off the piston1 double solenoid. This won't actuate anything
+ * because double solenoids preserve their state when turned off. This
+ * should be called in order to reduce the amount of time that the coils
+ * are powered.
+ */
+ public void off1() {
+ m_piston1.set(DoubleSolenoid.Value.kOff);
+ }
+
+ /**
+ * Turns off the piston2 double solenoid. This won't actuate anything
+ * because double solenoids preserve their state when turned off. This
+ * should be called in order to reduce the amount of time that the coils
+ * are powered.
+ */
+ public void off2() {
+ m_piston2.set(DoubleSolenoid.Value.kOff);
+ }
+
+ /**
+ * Release the latch so that we can shoot.
+ */
+ public void unlatch() {
+ m_latchPiston.set(true);
+ }
+
+ /**
+ * Latch so that pressure can build up and we aren't limited by air flow.
+ */
+ public void latch() {
+ m_latchPiston.set(false);
+ }
+
+ /**
+ * Toggles the latch postions.
+ */
+ public void toggleLatchPosition() {
+ m_latchPiston.set(!m_latchPiston.get());
+ }
+
+ /**
+ * Is Piston 1 extended (after shooting).
+ *
+ * @return Whether or not piston 1 is fully extended.
+ */
+ public boolean piston1IsExtended() {
+ return !m_piston1ReedSwitchFront.get();
+ }
+
+ /**
+ * Is Piston 1 retracted (before shooting).
+ *
+ * @return Whether or not piston 1 is fully retracted.
+ */
+ public boolean piston1IsRetracted() {
+ return !m_piston1ReedSwitchBack.get();
+ }
+
+ /**
+ * Turns off all double solenoids. Double solenoids hold their position when
+ * they are turned off. We should turn them off whenever possible to extend
+ * the life of the coils.
+ */
+ public void offBoth() {
+ m_piston1.set(DoubleSolenoid.Value.kOff);
+ m_piston2.set(DoubleSolenoid.Value.kOff);
+ }
+
+ /**
+ * Return whether the goal is hot as read by the banner sensor.
+ *
+ * NOTE: doesn't work in simulation.
+ *
+ * @return Whether or not the goal is hot
+ */
+ public boolean goalIsHot() {
+ return m_hotGoalSensor.get();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java
new file mode 100644
index 0000000000..4cbf98b363
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.pacgoat.triggers;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.buttons.Trigger;
+
+/**
+ * A custom button that is triggered when two buttons on a Joystick are
+ * simultaneously pressed.
+ */
+public class DoubleButton extends Trigger {
+ private Joystick m_joy;
+ private int m_button1;
+ private int m_button2;
+
+ public DoubleButton(Joystick joy, int button1, int button2) {
+ this.m_joy = joy;
+ this.m_button1 = button1;
+ this.m_button2 = button2;
+ }
+
+ @Override
+ public boolean get() {
+ return m_joy.getRawButton(m_button1) && m_joy.getRawButton(m_button2);
+ }
+}