From 6a00dc7976515f03549ec0b5482f8683ad56157e Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Mon, 4 Dec 2017 20:09:02 -0800 Subject: [PATCH] Re-Add PacGoat java example (#802) --- wpilibjExamples/examples.xml | 39 ++++ .../first/wpilibj/examples/pacgoat/OI.java | 57 ++++++ .../first/wpilibj/examples/pacgoat/Robot.java | 131 +++++++++++++ .../pacgoat/commands/CheckForHotGoal.java | 30 +++ .../examples/pacgoat/commands/CloseClaw.java | 31 +++ .../examples/pacgoat/commands/Collect.java | 25 +++ .../commands/DriveAndShootAutonomous.java | 30 +++ .../pacgoat/commands/DriveForward.java | 65 +++++++ .../pacgoat/commands/DriveWithJoystick.java | 37 ++++ .../pacgoat/commands/ExtendShooter.java | 34 ++++ .../examples/pacgoat/commands/LowGoal.java | 25 +++ .../examples/pacgoat/commands/OpenClaw.java | 33 ++++ .../pacgoat/commands/SetCollectionSpeed.java | 32 ++++ .../pacgoat/commands/SetPivotSetpoint.java | 39 ++++ .../examples/pacgoat/commands/Shoot.java | 24 +++ .../pacgoat/commands/WaitForBall.java | 29 +++ .../pacgoat/commands/WaitForPressure.java | 28 +++ .../pacgoat/subsystems/Collector.java | 100 ++++++++++ .../pacgoat/subsystems/DriveTrain.java | 145 +++++++++++++++ .../examples/pacgoat/subsystems/Pivot.java | 106 +++++++++++ .../pacgoat/subsystems/Pneumatics.java | 56 ++++++ .../examples/pacgoat/subsystems/Shooter.java | 176 ++++++++++++++++++ .../pacgoat/triggers/DoubleButton.java | 32 ++++ 23 files changed, 1304 insertions(+) create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java diff --git a/wpilibjExamples/examples.xml b/wpilibjExamples/examples.xml index ca8d8c66a6..d8988adf1b 100755 --- a/wpilibjExamples/examples.xml +++ b/wpilibjExamples/examples.xml @@ -251,6 +251,45 @@ + + PacGoat + A fully functional example CommandBased program for FRC Team 190's 2014 + robot. This code can run on your computer if it supports simulation. + + Complete Robot + + /usr/share/frcsim/worlds/PacGoat2014.world + + src/$package-dir + src/$package-dir/commands + src/$package-dir/subsystems + src/$package-dir/triggers + + + + + + + + + + + + + + + + + + + + + + + + + + Simple Vision Demonstrate the use of the CameraServer class to stream from a USB Webcam diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java new file mode 100644 index 0000000000..183e897596 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import edu.wpi.first.wpilibj.examples.pacgoat.commands.Collect; +import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveForward; +import edu.wpi.first.wpilibj.examples.pacgoat.commands.LowGoal; +import edu.wpi.first.wpilibj.examples.pacgoat.commands.SetCollectionSpeed; +import edu.wpi.first.wpilibj.examples.pacgoat.commands.SetPivotSetpoint; +import edu.wpi.first.wpilibj.examples.pacgoat.commands.Shoot; +import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector; +import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot; +import edu.wpi.first.wpilibj.examples.pacgoat.triggers.DoubleButton; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.buttons.JoystickButton; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * The operator interface of the robot, it has been simplified from the real + * robot to allow control with a single PS3 joystick. As a result, not all + * functionality from the real robot is available. + */ +public class OI { + public Joystick m_joystick = new Joystick(0); + + public OI() { + new JoystickButton(m_joystick, 12).whenPressed(new LowGoal()); + new JoystickButton(m_joystick, 10).whenPressed(new Collect()); + + new JoystickButton(m_joystick, 11).whenPressed( + new SetPivotSetpoint(Pivot.kShoot)); + new JoystickButton(m_joystick, 9).whenPressed( + new SetPivotSetpoint(Pivot.kShootNear)); + + new DoubleButton(m_joystick, 2, 3).whenActive(new Shoot()); + + // SmartDashboard Buttons + SmartDashboard.putData("Drive Forward", new DriveForward(2.25)); + SmartDashboard.putData("Drive Backward", new DriveForward(-2.25)); + SmartDashboard.putData("Start Rollers", + new SetCollectionSpeed(Collector.kForward)); + SmartDashboard.putData("Stop Rollers", + new SetCollectionSpeed(Collector.kStop)); + SmartDashboard.putData("Reverse Rollers", + new SetCollectionSpeed(Collector.kReverse)); + } + + public Joystick getJoystick() { + return m_joystick; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java new file mode 100644 index 0000000000..202adfdb9b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java @@ -0,0 +1,131 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveAndShootAutonomous; +import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveForward; +import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector; +import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.DriveTrain; +import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot; +import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pneumatics; +import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Shooter; + +/** + * This is the main class for running the PacGoat code. + * + *

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 m_autoChooser; + + // This function is run when the robot is first started up and should be + // used for any initialization code. + @Override + public void robotInit() { + SmartDashboard.putData(drivetrain); + SmartDashboard.putData(collector); + SmartDashboard.putData(shooter); + SmartDashboard.putData(pneumatics); + SmartDashboard.putData(pivot); + + // This MUST be here. If the OI creates Commands (which it very likely + // will), constructing it during the construction of CommandBase (from + // which commands extend), subsystems are not guaranteed to be + // yet. Thus, their requires() statements may grab null pointers. Bad + // news. Don't move it. + oi = new OI(); + + // instantiate the command used for the autonomous period + m_autoChooser = new SendableChooser(); + m_autoChooser.addDefault("Drive and Shoot", new DriveAndShootAutonomous()); + m_autoChooser.addObject("Drive Forward", new DriveForward()); + SmartDashboard.putData("Auto Mode", m_autoChooser); + } + + @Override + public void autonomousInit() { + m_autonomousCommand = (Command) m_autoChooser.getSelected(); + m_autonomousCommand.start(); + } + + // This function is called periodically during autonomous + @Override + public void autonomousPeriodic() { + Scheduler.getInstance().run(); + log(); + } + + @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(); + log(); + } + + // This function called periodically during test mode + @Override + public void testPeriodic() { + LiveWindow.run(); + } + + @Override + public void disabledInit() { + Robot.shooter.unlatch(); + } + + // This function is called periodically while disabled + @Override + public void disabledPeriodic() { + log(); + } + + /** + * Log interesting values to the SmartDashboard. + */ + private void log() { + Robot.pneumatics.writePressure(); + SmartDashboard.putNumber("Pivot Pot Value", Robot.pivot.getAngle()); + SmartDashboard.putNumber("Left Distance", + drivetrain.getLeftEncoder().getDistance()); + SmartDashboard.putNumber("Right Distance", + drivetrain.getRightEncoder().getDistance()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java new file mode 100644 index 0000000000..fad9ddc970 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.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.Command; + +import edu.wpi.first.wpilibj.examples.pacgoat.Robot; + +/** + * This command looks for the hot goal and waits until it's detected or timed + * out. The timeout is because it's better to shoot and get some autonomous + * points than get none. When called sequentially, this command will block until + * the hot goal is detected or until it is timed out. + */ +public class CheckForHotGoal extends Command { + public CheckForHotGoal(double time) { + setTimeout(time); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return isTimedOut() || Robot.shooter.goalIsHot(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java new file mode 100644 index 0000000000..7ee69f8850 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +/** + * Close the claw. + * + *

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); + } +}