Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Re-Add PacGoat java example (#802)

Browse Source
This commit is contained in:
sciencewhiz
2017-12-04 20:09:02 -08:00
committed by Peter Johnson
parent e9e407a87d
commit 6a00dc7976
23 changed files with 1304 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
wpilibjExamples/examples.xml
View File

@@ -251,6 +251,45 @@
<file source="examples/gearsbot/subsystems/Wrist.java" destination="src/$package-dir/subsystems/Wrist.java" />
</files>
</example>
<example>
<name>PacGoat</name>
<description>A fully functional example CommandBased program for FRC Team 190's 2014
robot. This code can run on your computer if it supports simulation.</description>
<tags>
<tag>Complete Robot</tag>
</tags>
<world>/usr/share/frcsim/worlds/PacGoat2014.world</world>
<packages>
<package>src/$package-dir</package>
<package>src/$package-dir/commands</package>
<package>src/$package-dir/subsystems</package>
<package>src/$package-dir/triggers</package>
</packages>
<files>
<file source="examples/pacgoat/OI.java" destination="src/$package-dir/OI.java" />
<file source="examples/pacgoat/Robot.java" destination="src/$package-dir/Robot.java" />
<file source="examples/pacgoat/commands/CheckForHotGoal.java" destination="src/$package-dir/commands/CheckForHotGoal.java" />
<file source="examples/pacgoat/commands/CloseClaw.java" destination="src/$package-dir/commands/CloseClaw.java" />
<file source="examples/pacgoat/commands/Collect.java" destination="src/$package-dir/commands/Collect.java" />
<file source="examples/pacgoat/commands/DriveAndShootAutonomous.java" destination="src/$package-dir/commands/DriveAndShootAutonomous.java" />
<file source="examples/pacgoat/commands/DriveForward.java" destination="src/$package-dir/commands/DriveForward.java" />
<file source="examples/pacgoat/commands/DriveWithJoystick.java" destination="src/$package-dir/commands/DriveWithJoystick.java" />
<file source="examples/pacgoat/commands/ExtendShooter.java" destination="src/$package-dir/commands/ExtendShooter.java" />
<file source="examples/pacgoat/commands/LowGoal.java" destination="src/$package-dir/commands/LowGoal.java" />
<file source="examples/pacgoat/commands/OpenClaw.java" destination="src/$package-dir/commands/OpenClaw.java" />
<file source="examples/pacgoat/commands/SetCollectionSpeed.java" destination="src/$package-dir/commands/SetCollectionSpeed.java" />
<file source="examples/pacgoat/commands/SetPivotSetpoint.java" destination="src/$package-dir/commands/SetPivotSetpoint.java" />
<file source="examples/pacgoat/commands/Shoot.java" destination="src/$package-dir/commands/Shoot.java" />
<file source="examples/pacgoat/commands/WaitForBall.java" destination="src/$package-dir/commands/WaitForBall.java" />
<file source="examples/pacgoat/commands/WaitForPressure.java" destination="src/$package-dir/commands/WaitForPressure.java" />
<file source="examples/pacgoat/subsystems/Collector.java" destination="src/$package-dir/subsystems/Collector.java" />
<file source="examples/pacgoat/subsystems/DriveTrain.java" destination="src/$package-dir/subsystems/DriveTrain.java" />
<file source="examples/pacgoat/subsystems/Pivot.java" destination="src/$package-dir/subsystems/Pivot.java" />
<file source="examples/pacgoat/subsystems/Pneumatics.java" destination="src/$package-dir/subsystems/Pneumatics.java" />
<file source="examples/pacgoat/subsystems/Shooter.java" destination="src/$package-dir/subsystems/Shooter.java" />
<file source="examples/pacgoat/triggers/DoubleButton.java" destination="src/$package-dir/triggers/DoubleButton.java" />
</files>
</example>
<example>
<name>Simple Vision</name>
<description>Demonstrate the use of the CameraServer class to stream from a USB Webcam

57
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java Normal file
View File

@@ -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;
}
}

131
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java Normal file
View File

@@ -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.
*
* <p>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<Command> 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<Command>();
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());
}
}

30
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java Normal file
View File

@@ -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();
}
}

31
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java Normal file
View File

@@ -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.
*
* <p>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();
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java Normal file
View File

@@ -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());
}
}

30
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java Normal file
View File

@@ -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());
}
}

65
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java Normal file
View File

@@ -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();
}
}

37
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java Normal file
View File

@@ -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();
}
}

34
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java Normal file
View File

@@ -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();
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java Normal file
View File

@@ -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());
}
}

33
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java Normal file
View File

@@ -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();
}
}

32
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java Normal file
View File

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

39
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java Normal file
View File

@@ -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();
}
}

24
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java Normal file
View File

@@ -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());
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java Normal file
View File

@@ -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();
}
}

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java Normal file
View File

@@ -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();
}
}

100
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java Normal file
View File

@@ -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.
*
* <p>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() {
}
}

145
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java Normal file
View File

@@ -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();
}
}

106
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java Normal file
View File

@@ -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();
}
}

56
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java Normal file
View File

@@ -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.
*
* <p>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());
}
}

176
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java Normal file
View File

@@ -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.
*
* <p>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.
*
* <p>NOTE: doesn't work in simulation.
*
* @return Whether or not the goal is hot
*/
public boolean goalIsHot() {
return m_hotGoalSensor.get();
}
}

32
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java Normal file
View File

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