diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/GearsBot/src/org/usfirst/frc/team190/robot/subsystems/DriveTrain.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/GearsBot/src/org/usfirst/frc/team190/robot/subsystems/DriveTrain.java
index c6c749e480..cfb17663dc 100644
--- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/GearsBot/src/org/usfirst/frc/team190/robot/subsystems/DriveTrain.java
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/GearsBot/src/org/usfirst/frc/team190/robot/subsystems/DriveTrain.java
@@ -1,6 +1,6 @@
package $package.subsystems;
-import edu.wpi.first.wpilibj.AnalogChannel;
+import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick.AxisType;
import edu.wpi.first.wpilibj.Gyro;
@@ -24,7 +24,7 @@ public class DriveTrain extends Subsystem {
front_right_motor, back_right_motor;
private RobotDrive drive;
private Encoder left_encoder, right_encoder;
- private AnalogChannel rangefinder;
+ private AnalogInput rangefinder;
private Gyro gyro;
public DriveTrain() {
@@ -55,7 +55,7 @@ public class DriveTrain extends Subsystem {
left_encoder.start();
right_encoder.start();
- rangefinder = new AnalogChannel(6);
+ rangefinder = new AnalogInput(6);
gyro = new Gyro(1);
// Let's show everything on the LiveWindow
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/OI.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/OI.java
new file mode 100644
index 0000000000..d9e0049de3
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/OI.java
@@ -0,0 +1,48 @@
+package $package;
+
+import $package.commands.Collect;
+import $package.commands.DriveForward;
+import $package.commands.LowGoal;
+import $package.commands.SetCollectionSpeed;
+import $package.commands.SetPivotSetpoint;
+import $package.commands.Shoot;
+import $package.subsystems.Collector;
+import $package.subsystems.Pivot;
+import $package.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 joystick;
+
+ public OI() {
+ joystick = new Joystick(1);
+
+ new JoystickButton(joystick, 12).whenPressed(new LowGoal());
+ new JoystickButton(joystick, 10).whenPressed(new Collect());
+
+ new JoystickButton(joystick, 11).whenPressed(new SetPivotSetpoint(Pivot.SHOOT));
+ new JoystickButton(joystick, 9).whenPressed(new SetPivotSetpoint(Pivot.SHOOT_NEAR));
+
+ new DoubleButton(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.FORWARD));
+ SmartDashboard.putData("Stop Rollers", new SetCollectionSpeed(Collector.STOP));
+ SmartDashboard.putData("Reverse Rollers", new SetCollectionSpeed(Collector.REVERSE));
+ }
+
+ public Joystick getJoystick() {
+ return joystick;
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/Robot.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/Robot.java
new file mode 100644
index 0000000000..21f079e7be
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/Robot.java
@@ -0,0 +1,122 @@
+package $package;
+
+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 $package.commands.DriveAndShootAutonomous;
+import $package.commands.DriveForward;
+import $package.subsystems.Collector;
+import $package.subsystems.DriveTrain;
+import $package.subsystems.Pivot;
+import $package.subsystems.Pneumatics;
+import $package.subsystems.Shooter;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * 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 autonomousCommand;
+ public static OI oi;
+
+ public static DriveTrain drivetrain;
+ public static Collector collector;
+ public static Shooter shooter;
+ public static Pneumatics pneumatics;
+ public static Pivot pivot;;
+
+ public SendableChooser autoChooser;
+ public SendableChooser autonomousDirectionChooser;
+
+ // This function is run when the robot is first started up and should be
+ // used for any initialization code.
+ public void robotInit() {
+ // Initialize the subsystems
+ drivetrain = new DriveTrain();
+ collector = new Collector();
+ shooter = new Shooter();
+ pneumatics = new Pneumatics();
+ pivot = new Pivot();
+ 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
+ autoChooser = new SendableChooser();
+ autoChooser.addDefault("Drive and Shoot", new DriveAndShootAutonomous());
+ autoChooser.addObject("Drive Forward", new DriveForward());
+ SmartDashboard.putData("Auto Mode", autoChooser);
+
+ pneumatics.start(); // Pressurize the pneumatics.
+ }
+
+ public void autonomousInit() {
+ Command auto = (Command) autoChooser.getSelected();
+ auto.start();
+ }
+
+ // This function is called periodically during autonomous
+ public void autonomousPeriodic() {
+ Scheduler.getInstance().run();
+ log();
+ }
+
+ 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 (autonomousCommand != null) {
+ autonomousCommand.cancel();
+ }
+ }
+
+ // This function is called periodically during operator control
+ public void teleopPeriodic() {
+ Scheduler.getInstance().run();
+ log();
+ }
+
+ // This function called periodically during test mode
+ public void testPeriodic() {
+ LiveWindow.run();
+ }
+
+ public void disabledInit() {
+ Robot.shooter.unlatch();
+ }
+
+ // This function is called periodically while disabled
+ 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/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/CheckForHotGoal.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/CheckForHotGoal.java
new file mode 100644
index 0000000000..dda0096921
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/CheckForHotGoal.java
@@ -0,0 +1,37 @@
+package $package.commands;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * 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);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {}
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {}
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return isTimedOut() || Robot.shooter.goalIsHot();
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {}
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/CloseClaw.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/CloseClaw.java
new file mode 100644
index 0000000000..80219e7c14
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/CloseClaw.java
@@ -0,0 +1,39 @@
+package $package.commands;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Close the claw.
+ *
+ * NOTE: It doesn't wait for the claw to close since their is no sensor to
+ * detect that.
+ */
+public class CloseClaw extends Command {
+
+ public CloseClaw() {
+ requires(Robot.collector);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.collector.close();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return true;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {}
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {}
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/Collect.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/Collect.java
new file mode 100644
index 0000000000..96426946ba
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/Collect.java
@@ -0,0 +1,18 @@
+package $package.commands;
+
+import $package.subsystems.Collector;
+import $package.subsystems.Pivot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * Get the robot set to collect balls.
+ */
+public class Collect extends CommandGroup {
+ public Collect() {
+ addSequential(new SetCollectionSpeed(Collector.FORWARD));
+ addParallel(new CloseClaw());
+ addSequential(new SetPivotSetpoint(Pivot.COLLECT));
+ addSequential(new WaitForBall());
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/DriveAndShootAutonomous.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/DriveAndShootAutonomous.java
new file mode 100644
index 0000000000..ff159818f2
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/DriveAndShootAutonomous.java
@@ -0,0 +1,23 @@
+package $package.commands;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * 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 WaitForPressure(), 2);
+ if (Robot.isReal()) {
+ // NOTE: Simulation doesn't currently have the concept of hot.
+ addSequential(new CheckForHotGoal(2));
+ }
+ addSequential(new CloseClaw());
+ addSequential(new SetPivotSetpoint(45));
+ addSequential(new DriveForward(8, 0.4));
+ addSequential(new Shoot());
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/DriveForward.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/DriveForward.java
new file mode 100644
index 0000000000..57cce4474a
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/DriveForward.java
@@ -0,0 +1,58 @@
+package $package.commands;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * 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 driveForwardSpeed;
+ private double distance;
+ private final double tolerance = .1; // TODO: Was 5
+ private double error;
+ private 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);
+ distance = dist;
+ driveForwardSpeed = maxSpeed;
+ }
+
+ protected void initialize() {
+ Robot.drivetrain.getRightEncoder().reset();
+ setTimeout(2);
+ }
+
+ protected void execute() {
+ error = (distance - Robot.drivetrain.getRightEncoder().getDistance());
+ if (driveForwardSpeed * Kp * error >= driveForwardSpeed) {
+ Robot.drivetrain.tankDrive(driveForwardSpeed, driveForwardSpeed);
+ } else {
+ Robot.drivetrain.tankDrive(driveForwardSpeed * Kp * error,
+ driveForwardSpeed * Kp * error);
+ }
+ }
+
+ protected boolean isFinished() {
+ return (Math.abs(error) <= tolerance) || isTimedOut();
+ }
+
+ protected void end() {
+ Robot.drivetrain.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/DriveWithJoystick.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/DriveWithJoystick.java
new file mode 100644
index 0000000000..5fbaa5b432
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/DriveWithJoystick.java
@@ -0,0 +1,34 @@
+package $package.commands;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * 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);
+ }
+
+ protected void initialize() {
+ }
+
+ protected void execute() {
+ Robot.drivetrain.tankDrive(Robot.oi.getJoystick());
+ }
+
+ protected boolean isFinished() {
+ return false;
+ }
+
+ protected void end() {
+ Robot.drivetrain.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/ExtendShooter.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/ExtendShooter.java
new file mode 100644
index 0000000000..2d585b6921
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/ExtendShooter.java
@@ -0,0 +1,41 @@
+package $package.commands;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Extend the shooter and then retract it after a second.
+ */
+public class ExtendShooter extends Command {
+
+ public ExtendShooter() {
+ requires(Robot.shooter);
+ setTimeout(1);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.shooter.extendBoth();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return isTimedOut();
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.shooter.retractBoth();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/LowGoal.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/LowGoal.java
new file mode 100644
index 0000000000..5dd84f9566
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/LowGoal.java
@@ -0,0 +1,17 @@
+package $package.commands;
+
+import $package.subsystems.Collector;
+import $package.subsystems.Pivot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * 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.LOW_GOAL));
+ addSequential(new SetCollectionSpeed(Collector.REVERSE));
+ addSequential(new ExtendShooter());
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/OpenClaw.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/OpenClaw.java
new file mode 100644
index 0000000000..b4eb35bcfb
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/OpenClaw.java
@@ -0,0 +1,36 @@
+package $package.commands;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Opens the claw
+ */
+public class OpenClaw extends Command {
+
+ public OpenClaw() {
+ requires(Robot.collector);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.collector.open();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return Robot.collector.isOpen();
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {}
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {}
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/SetCollectionSpeed.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/SetCollectionSpeed.java
new file mode 100644
index 0000000000..ec39826bee
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/SetCollectionSpeed.java
@@ -0,0 +1,39 @@
+package $package.commands;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * 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 Command {
+ private double speed;
+
+ public SetCollectionSpeed(double speed) {
+ requires(Robot.collector);
+ this.speed = speed;
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.collector.setSpeed(speed);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {}
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return true;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {}
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {}
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/SetPivotSetpoint.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/SetPivotSetpoint.java
new file mode 100644
index 0000000000..4a1643eed6
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/SetPivotSetpoint.java
@@ -0,0 +1,44 @@
+/*
+ * To change this template, choose Tools | Templates
+ * and open the template in the editor.
+ */
+package $package.commands;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * 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 setpoint;
+
+ public SetPivotSetpoint(double setpoint) {
+ this.setpoint = setpoint;
+ requires(Robot.pivot);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.pivot.enable();
+ Robot.pivot.setSetpoint(setpoint);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {}
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return Robot.pivot.onTarget();
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {}
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {}
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/Shoot.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/Shoot.java
new file mode 100644
index 0000000000..d08200ef28
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/Shoot.java
@@ -0,0 +1,17 @@
+package $package.commands;
+
+import $package.subsystems.Collector;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * Shoot the ball at the current angle.
+ */
+public class Shoot extends CommandGroup {
+ public Shoot() {
+ addSequential(new WaitForPressure());
+ addSequential(new SetCollectionSpeed(Collector.STOP));
+ addSequential(new OpenClaw());
+ addSequential(new ExtendShooter());
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/WaitForBall.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/WaitForBall.java
new file mode 100644
index 0000000000..5684d26af9
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/WaitForBall.java
@@ -0,0 +1,35 @@
+package $package.commands;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * 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);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {}
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {}
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return Robot.collector.hasBall();
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {}
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {}
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/WaitForPressure.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/WaitForPressure.java
new file mode 100644
index 0000000000..f1b6ab9d75
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/commands/WaitForPressure.java
@@ -0,0 +1,34 @@
+package $package.commands;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * 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);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {}
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {}
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return Robot.pneumatics.isPressurized();
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {}
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {}
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/Collector.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/Collector.java
new file mode 100644
index 0000000000..898be2651d
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/Collector.java
@@ -0,0 +1,91 @@
+package $package.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 FORWARD = 1;
+ public static final double STOP = 0;
+ public static final double REVERSE = -1;
+
+ // Subsystem devices
+ private SpeedController rollerMotor;
+ private DigitalInput ballDetector;
+ private Solenoid piston;
+ private DigitalInput openDetector;
+
+ public Collector() {
+ // Configure devices
+ rollerMotor = new Victor(6);
+ ballDetector = new DigitalInput(10);
+ openDetector = new DigitalInput(6);
+ piston = new Solenoid(1, 1);
+
+ // Put everything to the LiveWindow for testing.
+ LiveWindow.addActuator("Collector", "Roller Motor", (Victor) rollerMotor);
+ LiveWindow.addSensor("Collector", "Ball Detector", ballDetector);
+ LiveWindow.addSensor("Collector", "Claw Open Detector", openDetector);
+ LiveWindow.addActuator("Collector", "Piston", piston);
+ }
+
+ /**
+ * 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 ballDetector.get(); // TODO: prepend ! to reflect real robot
+ }
+
+ /**
+ * @param speed The speed to spin the rollers.
+ */
+ public void setSpeed(double speed) {
+ rollerMotor.set(-speed);
+ }
+
+ /**
+ * Stop the rollers from spinning
+ */
+ public void stop() {
+ rollerMotor.set(0);
+ }
+
+ /**
+ * @return Whether or not the claw is open.
+ */
+ public boolean isOpen() {
+ return openDetector.get(); // TODO: prepend ! to reflect real robot
+ }
+
+ /**
+ * Open the claw up. (For shooting)
+ */
+ public void open() {
+ piston.set(true);
+ }
+
+ /**
+ * Close the claw. (For collecting and driving)
+ */
+ public void close() {
+ piston.set(false);
+ }
+
+ /**
+ * No default command.
+ */
+ @Override protected void initDefaultCommand() {}
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/DriveTrain.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/DriveTrain.java
new file mode 100644
index 0000000000..1b840a0114
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/DriveTrain.java
@@ -0,0 +1,129 @@
+package $package.subsystems;
+
+import $package.Robot;
+import $package.commands.DriveWithJoystick;
+
+import edu.wpi.first.wpilibj.CounterBase.EncodingType;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Gyro;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PIDSource.PIDSourceParameter;
+import edu.wpi.first.wpilibj.RobotDrive;
+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 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 frontLeftCIM, frontRightCIM;
+ private SpeedController backLeftCIM, backRightCIM;
+ private RobotDrive drive;
+ private Encoder rightEncoder, leftEncoder;
+ private Gyro gyro;
+
+ public DriveTrain() {
+ // Configure drive motors
+ frontLeftCIM = new Victor(1);
+ frontRightCIM = new Victor(2);
+ backLeftCIM = new Victor(3);
+ backRightCIM = new Victor(4);
+ LiveWindow.addActuator("DriveTrain", "Front Left CIM", (Victor) frontLeftCIM);
+ LiveWindow.addActuator("DriveTrain", "Front Right CIM", (Victor) frontRightCIM);
+ LiveWindow.addActuator("DriveTrain", "Back Left CIM", (Victor) backLeftCIM);
+ LiveWindow.addActuator("DriveTrain", "Back Right CIM", (Victor) backRightCIM);
+
+ // Configure the RobotDrive to reflect the fact that all our motors are
+ // wired backwards and our drivers sensitivity preferences.
+ drive = new RobotDrive(frontLeftCIM, backLeftCIM, frontRightCIM, backRightCIM);
+ drive.setSafetyEnabled(true);
+ drive.setExpiration(0.1);
+ drive.setSensitivity(0.5);
+ drive.setMaxOutput(1.0);
+ drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
+ drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
+ drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
+ drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
+
+ // Configure encoders
+ rightEncoder = new Encoder(1, 2, true, EncodingType.k4X);
+ leftEncoder = new Encoder(5, 6, false, EncodingType.k4X); // XXX: Module 2
+ rightEncoder.setPIDSourceParameter(PIDSourceParameter.kDistance);
+ leftEncoder.setPIDSourceParameter(PIDSourceParameter.kDistance);
+
+ if (Robot.isReal()) { // Converts to feet
+ rightEncoder.setDistancePerPulse(0.0785398);
+ leftEncoder.setDistancePerPulse(0.0785398);
+ } else { // Convert to feet 4in diameter wheels with 360 tick simulated encoders
+ rightEncoder.setDistancePerPulse((4.0/*in*/*Math.PI)/(360.0*12.0/*in/ft*/));
+ leftEncoder.setDistancePerPulse((4.0/*in*/*Math.PI)/(360.0*12.0/*in/ft*/));
+ }
+
+ rightEncoder.start();
+ leftEncoder.start();
+ LiveWindow.addSensor("DriveTrain", "Right Encoder", rightEncoder);
+ LiveWindow.addSensor("DriveTrain", "Left Encoder", leftEncoder);
+
+ // Configure gyro
+ gyro = new Gyro(2);
+ if (Robot.isReal()) {
+ gyro.setSensitivity(0.007); // TODO: Handle more gracefully?
+ }
+ LiveWindow.addSensor("DriveTrain", "Gyro", gyro);
+ }
+
+ /**
+ * When other commands aren't using the drivetrain, allow tank drive with
+ * the joystick.
+ */
+ public void initDefaultCommand() {
+ setDefaultCommand(new DriveWithJoystick());
+ }
+
+ /**
+ * @param joy PS3 style joystick to use as the input for tank drive.
+ */
+ public void tankDrive(Joystick joy) {
+ drive.tankDrive(joy.getY(), joy.getRawAxis(4));
+ }
+
+ /**
+ * @param leftAxis Left sides value
+ * @param rightAxis Right sides value
+ */
+ public void tankDrive(double leftAxis, double rightAxis) {
+ drive.tankDrive(leftAxis, rightAxis);
+ }
+
+ /**
+ * Stop the drivetrain from moving.
+ */
+ public void stop() {
+ drive.tankDrive(0, 0);
+ }
+
+ /**
+ * @return The encoder getting the distance and speed of left side of the drivetrain.
+ */
+ public Encoder getLeftEncoder() {
+ return leftEncoder;
+ }
+
+ /**
+ * @return The encoder getting the distance and speed of right side of the drivetrain.
+ */
+ public Encoder getRightEncoder() {
+ return rightEncoder;
+ }
+
+ /**
+ * @return The current angle of the drivetrain.
+ */
+ public double getAngle() {
+ return gyro.getAngle();
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/Pivot.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/Pivot.java
new file mode 100644
index 0000000000..728aafe62c
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/Pivot.java
@@ -0,0 +1,94 @@
+package $package.subsystems;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.*;
+import edu.wpi.first.wpilibj.command.PIDSubsystem;
+import edu.wpi.first.wpilibj.interfaces.Potentiometer;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+
+/**
+ * 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 COLLECT = 105;
+ public static final double LOW_GOAL = 90;
+ public static final double SHOOT = 45;
+ public static final double SHOOT_NEAR = 30;
+
+ // Subsystem devices
+ private DigitalInput upperLimitSwitch;
+ private DigitalInput lowerLimitSwitch;
+ private Potentiometer pot;
+ private SpeedController motor;
+
+ 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.00001, 4.5);
+ setAbsoluteTolerance(2.5);
+ }
+
+ // Motor to move the pivot.
+ motor = new Victor(5);
+
+ // Sensors for measuring the position of the pivot.
+ upperLimitSwitch = new DigitalInput(13);
+ lowerLimitSwitch = new DigitalInput(12);
+
+ // 0 degrees is vertical facing up.
+ // Angle increases the more forward the pivot goes.
+ pot = new AnalogPotentiometer(1);
+
+ // Put everything to the LiveWindow for testing.
+ LiveWindow.addSensor("Pivot", "Upper Limit Switch", upperLimitSwitch);
+ LiveWindow.addSensor("Pivot", "Lower Limit Switch", lowerLimitSwitch);
+ LiveWindow.addSensor("Pivot", "Pot", (AnalogPotentiometer) pot);
+ LiveWindow.addActuator("Pivot", "Motor", (Victor) motor);
+ LiveWindow.addActuator("Pivot", "PIDSubsystem Controller", getPIDController());
+ }
+
+ /**
+ * No default command, if PID is enabled, the current setpoint will be maintained.
+ */
+ public void initDefaultCommand() {}
+
+ /**
+ * @return The angle read in by the potentiometer
+ */
+ protected double returnPIDInput() {
+ return pot.get();
+ }
+
+ /**
+ * Set the motor speed based off of the PID output
+ */
+ protected void usePIDOutput(double output) {
+ motor.pidWrite(output);
+ }
+
+ /**
+ * @return If the pivot is at its upper limit.
+ */
+ public boolean isAtUpperLimit() {
+ return upperLimitSwitch.get(); // TODO: inverted from real robot (prefix with !)
+ }
+
+ /**
+ * @return If the pivot is at its lower limit.
+ */
+ public boolean isAtLowerLimit() {
+ return lowerLimitSwitch.get(); // TODO: inverted from real robot (prefix with !)
+ }
+
+ /**
+ * @return The current angle of the pivot.
+ */
+ public double getAngle() {
+ return pot.get();
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/Pneumatics.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/Pneumatics.java
new file mode 100644
index 0000000000..dd6c4a815d
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/Pneumatics.java
@@ -0,0 +1,61 @@
+package $package.subsystems;
+
+import $package.Robot;
+
+import edu.wpi.first.wpilibj.*;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * The Pneumatics subsystem contains the compressor and a pressure sensor.
+ *
+ * NOTE: The simulator currently doesn't support the compressor or pressure sensors.
+ */
+public class Pneumatics extends Subsystem {
+ AnalogInput pressureSensor;
+ Compressor compressor;
+
+ private static final double MAX_PRESSURE = 2.55;
+
+ public Pneumatics() {
+ pressureSensor = new AnalogInput(3);
+ if (Robot.isReal()) {
+ compressor = new Compressor();
+ }
+
+ LiveWindow.addSensor("Pneumatics", "Pressure Sensor", pressureSensor);
+ }
+
+ /**
+ * No default command
+ */
+ public void initDefaultCommand() {}
+
+ /**
+ * Start the compressor going. The compressor automatically starts and stops as it goes above and below maximum pressure.
+ */
+ public void start() {
+ if (Robot.isReal()) {
+ compressor.start();
+ }
+ }
+
+ /**
+ * @return Whether or not the system is fully pressurized.
+ */
+ public boolean isPressurized() {
+ if (Robot.isReal()) {
+ return MAX_PRESSURE <= pressureSensor.getVoltage();
+ } else {
+ return true; // NOTE: Simulation always has full pressure
+ }
+ }
+
+ /**
+ * Puts the pressure on the SmartDashboard.
+ */
+ public void writePressure() {
+ SmartDashboard.putNumber("Pressure", pressureSensor.getVoltage());
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/Shooter.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/Shooter.java
new file mode 100644
index 0000000000..67ffcda623
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/subsystems/Shooter.java
@@ -0,0 +1,166 @@
+package $package.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 piston1;
+ DoubleSolenoid piston2;
+ Solenoid latchPiston;
+ DigitalInput piston1ReedSwitchFront;
+ DigitalInput piston1ReedSwitchBack;
+ DigitalInput hotGoalSensor; // NOTE: Currently ignored in simulation
+
+ public Shooter() {
+ // Configure Devices
+ hotGoalSensor = new DigitalInput(3);
+ piston1 = new DoubleSolenoid(1, 3, 4);
+ piston2 = new DoubleSolenoid(1, 5, 6);
+ latchPiston = new Solenoid(1, 2);
+ piston1ReedSwitchFront = new DigitalInput(9);
+ piston1ReedSwitchBack = new DigitalInput(11);
+
+ // Put everything to the LiveWindow for testing.
+ LiveWindow.addSensor("Shooter", "Hot Goal Sensor", hotGoalSensor);
+ LiveWindow.addSensor("Shooter", "Piston1 Reed Switch Front ",
+ piston1ReedSwitchFront);
+ LiveWindow.addSensor("Shooter", "Piston1 Reed Switch Back ",
+ piston1ReedSwitchBack);
+ LiveWindow.addActuator("Shooter", "Latch Piston", latchPiston);
+ }
+
+ /**
+ * No default command.
+ */
+ public void initDefaultCommand() {}
+
+ /**
+ * Extend both solenoids to shoot.
+ */
+ public void extendBoth() {
+ piston1.set(DoubleSolenoid.Value.kForward);
+ piston2.set(DoubleSolenoid.Value.kForward);
+ }
+
+ /**
+ * Retract both solenoids to prepare to shoot.
+ */
+ public void retractBoth() {
+ piston1.set(DoubleSolenoid.Value.kReverse);
+ piston2.set(DoubleSolenoid.Value.kReverse);
+ }
+
+ /**
+ * Extend solenoid 1 to shoot.
+ */
+ public void extend1() {
+ piston1.set(DoubleSolenoid.Value.kForward);
+ }
+
+ /**
+ * Retract solenoid 1 to prepare to shoot.
+ */
+ public void retract1() {
+ piston1.set(DoubleSolenoid.Value.kReverse);
+ }
+
+ /**
+ * Extend solenoid 2 to shoot.
+ */
+ public void extend2() {
+ piston2.set(DoubleSolenoid.Value.kReverse);
+ }
+
+ /**
+ * Retract solenoid 2 to prepare to shoot.
+ */
+ public void retract2() {
+ 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() {
+ piston1.set(DoubleSolenoid.Value.kOff);
+ }
+
+ /**
+ * 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 off2() {
+ piston2.set(DoubleSolenoid.Value.kOff);
+ }
+
+ /**
+ * Release the latch so that we can shoot
+ */
+ public void unlatch() {
+ latchPiston.set(true);
+ }
+
+ /**
+ * Latch so that pressure can build up and we aren't limited by air flow.
+ */
+ public void latch() {
+ latchPiston.set(false);
+ }
+
+ /**
+ * Toggles the latch postions
+ */
+ public void toggleLatchPosition() {
+ latchPiston.set(!latchPiston.get());
+ }
+
+ /**
+ * @return Whether or not piston 1 is fully extended.
+ */
+ public boolean piston1IsExtended() {
+ return !piston1ReedSwitchFront.get();
+ }
+
+ /**
+ * @return Whether or not piston 1 is fully retracted.
+ */
+ public boolean piston1IsRetracted() {
+ return !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() {
+ piston1.set(DoubleSolenoid.Value.kOff);
+ piston2.set(DoubleSolenoid.Value.kOff);
+ }
+
+ /**
+ * @return Whether or not the goal is hot as read by the banner sensor
+ */
+ public boolean goalIsHot() {
+ return hotGoalSensor.get();
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/triggers/DoubleButton.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/triggers/DoubleButton.java
new file mode 100644
index 0000000000..773ae627e9
--- /dev/null
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/PacGoat/src/org/usfirst/frc/team190/pacgoat/triggers/DoubleButton.java
@@ -0,0 +1,23 @@
+package $package.triggers;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.buttons.*;
+
+/**
+ * A custom button that is triggered when two buttons on a Joystick are
+ * simultaneously pressed.
+ */
+public class DoubleButton extends Trigger {
+ private Joystick joy;
+ private int button1, button2;
+
+ public DoubleButton(Joystick joy, int button1, int button2) {
+ this.joy = joy;
+ this.button1 = button1;
+ this.button2 = button2;
+ }
+
+ public boolean get() {
+ return joy.getRawButton(button1) && joy.getRawButton(button2);
+ }
+}
diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml
index 54be5d3453..e8577ee373 100644
--- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml
+++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml
@@ -84,4 +84,66 @@
+
+
+ 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.
+
+ CommandBased Robot
+ Simulation
+ 2014 Season
+
+
+ src/$package-dir
+ src/$package-dir/commands
+ src/$package-dir/subsystems
+ src/$package-dir/triggers
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Node.java b/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Node.java
index c9a3b5bd92..90ff433378 100644
--- a/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Node.java
+++ b/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Node.java
@@ -57,9 +57,10 @@ public class Node implements Runnable, ServerCallback {
initializeConnection();
new Thread(this).start();
+ LOG.info("Serving on: "+server.host+":"+server.port);
}
- public Publisher advertise(String topic, T defaultMessage) {
+ public synchronized Publisher advertise(String topic, T defaultMessage) {
topic = fixTopic(topic);
LOG.info("ADV "+topic);
String type = defaultMessage.getDescriptorForType().getFullName();
@@ -76,7 +77,7 @@ public class Node implements Runnable, ServerCallback {
return pub;
}
- public Subscriber
+ public synchronized Subscriber
subscribe(String topic, T defaultMessage, SubscriberCallback cb) {
topic = fixTopic(topic);
LOG.info("SUB "+topic);
@@ -122,7 +123,7 @@ public class Node implements Runnable, ServerCallback {
}
}
- private void initializeConnection() throws IOException {
+ private synchronized void initializeConnection() throws IOException {
Packet initData = master.read();
if (!initData.getType().equals("version_init")) {
throw new IOException("Expected 'version_init' packet, got '"+initData.getType()+"'.");
@@ -148,7 +149,7 @@ public class Node implements Runnable, ServerCallback {
}
}
- public void processPacket(Packet packet) throws InvalidProtocolBufferException {
+ public synchronized void processPacket(Packet packet) throws InvalidProtocolBufferException {
if (packet.getType().equals("publisher_add")) {
PublisherRecord pub = new RemotePublisherRecord(Publish.parseFrom(packet.getSerializedData()));
@@ -158,6 +159,7 @@ public class Node implements Runnable, ServerCallback {
}
LOG.info("New Publisher: "+pub.getTopic());
+ LOG.fine("Publisher: "+Publish.parseFrom(packet.getSerializedData()));
publishers.put(pub.getTopic(), pub);
} else if (packet.getType().equals("publisher_subscribe") ||
packet.getType().equals("publisher_advertise")) {
@@ -169,6 +171,7 @@ public class Node implements Runnable, ServerCallback {
}
LOG.info("PUBSUB found for "+pub.getTopic());
+ LOG.fine("Publisher: "+Publish.parseFrom(packet.getSerializedData()));
subscriptions.get(pub.getTopic()).connect(pub);
} else if (packet.getType().equals("topic_namespace_add")) {
namespaces.add(GzString.String.parseFrom(packet.getSerializedData()).getData());
@@ -183,9 +186,10 @@ public class Node implements Runnable, ServerCallback {
@Override
public void handle(Connection conn) throws IOException {
+ LOG.fine("Handling new connection");
Packet msg = conn.read();
if (msg == null) {
- LOG.severe("Read null message.");
+ LOG.warning("Read null message.");
return;
}
@@ -196,6 +200,7 @@ public class Node implements Runnable, ServerCallback {
+ sub.getTopic());
return;
}
+ LOG.fine("New connection for topic="+sub.getTopic());
PublisherRecord pub = publishers.get(sub.getTopic());
if (!pub.getMsgType().equals(sub.getMsgType())) {
@@ -204,7 +209,7 @@ public class Node implements Runnable, ServerCallback {
return;
}
- LOG.info("PUB " + sub.getTopic());
+ LOG.info("CONN " + sub.getTopic());
pub.connect(conn);
} else {
LOG.warning("Unknown message type: " + msg.getType());
diff --git a/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Publisher.java b/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Publisher.java
index 35bbfff2b1..8bf973e793 100644
--- a/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Publisher.java
+++ b/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Publisher.java
@@ -3,6 +3,7 @@ package org.gazebosim.transport;
import java.io.IOException;
import java.util.LinkedList;
import java.util.List;
+import java.util.logging.Logger;
import com.google.protobuf.Message;
@@ -13,7 +14,9 @@ public class Publisher implements PublisherRecord {
private List listeners;
private boolean latching = false;
private T lastMsg = null;
-
+
+ private static final Logger LOG = Logger.getLogger("Gazebo Transport");
+
public Publisher(String topic, String msgType, String localHost, int localPort) {
this.topic = topic;
this.msgType = msgType;
@@ -24,16 +27,21 @@ public class Publisher implements PublisherRecord {
public synchronized void publish(T msg) {
lastMsg = msg;
+ List toRemove = new LinkedList<>();
for (Connection listener : listeners) {
try {
listener.write(msg);
} catch (IOException e) {
- try {
- listener.close();
- } catch (IOException e1) { /* Closing failed, probably not a big deal. */}
- listeners.remove(listener);
+ toRemove.add(listener);
}
}
+ for (Connection listener : toRemove) {
+ LOG.info("Removing listener from topic="+topic);
+ try {
+ listener.close();
+ } catch (IOException e1) { /* Closing failed, probably not a big deal. */}
+ listeners.remove(listener);
+ }
}
@Override
@@ -58,11 +66,16 @@ public class Publisher implements PublisherRecord {
@Override
public synchronized void connect(Connection conn) {
+ LOG.fine("Handling subscriber connection for topic: "+topic);
if (latching && lastMsg != null) {
try {
conn.write(lastMsg);
} catch (IOException e) {
- return; // TODO: Log
+ LOG.warning("Writing latched message failed on topic="+topic);
+ try {
+ conn.close();
+ } catch (IOException e1) { /* Closing failed, probably not a big deal. */}
+ return;
}
}
listeners.add(conn);
diff --git a/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Subscriber.java b/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Subscriber.java
index 993a2ff41c..12770eef74 100644
--- a/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Subscriber.java
+++ b/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Subscriber.java
@@ -66,7 +66,7 @@ public class Subscriber {
}
} catch (IOException e) {
// FIXME: Connection lost, let's make sure it's closed and complain.
- // Hopefully they the reconnect, maybe we should try to recover better?
+ // Hopefully they try to reconnect, maybe we should try to recover better?
try {
conn.close();
} catch (IOException e1) {
diff --git a/simulation/debs/frcsim-gazebo-plugins/frcsim-gazebo-plugins/debian/control b/simulation/debs/frcsim-gazebo-plugins/frcsim-gazebo-plugins/debian/control
index a147439ed9..dae690a2e5 100644
--- a/simulation/debs/frcsim-gazebo-plugins/frcsim-gazebo-plugins/debian/control
+++ b/simulation/debs/frcsim-gazebo-plugins/frcsim-gazebo-plugins/debian/control
@@ -3,11 +3,11 @@ Maintainer: Alex Henning
Section: misc
Priority: optional
Standards-Version: 3.9.4
-Build-Depends: debhelper (>= 9), ros-indigo-ros-base, gazebo2, libtinyxml2-dev
+Build-Depends: debhelper (>= 9), gazebo3, libgazebo-dev, libavcodec-dev, libavformat-dev, libswscale-dev, libtinyxml2-dev
Package: frcsim-gazebo-plugins
Architecture: i386 amd64
-Depends: ${shlibs:Depends}, ${misc:Depends}, gazebo2, libtinyxml2-dev
+Depends: ${shlibs:Depends}, ${misc:Depends}, gazebo3, libgazebo-dev, libavcodec-dev, libswscale-dev, libtinyxml2-dev
Description: FRC plugin for controlling robots in the gazebo plugin.
This plugin allows robots in gazebo to communicate with WPILib
programs and enables the control of the robots.
\ No newline at end of file
diff --git a/simulation/frc_gazebo_plugin/src/RobotMapParser.cpp b/simulation/frc_gazebo_plugin/src/RobotMapParser.cpp
index fa8c32e07e..28d769f529 100644
--- a/simulation/frc_gazebo_plugin/src/RobotMapParser.cpp
+++ b/simulation/frc_gazebo_plugin/src/RobotMapParser.cpp
@@ -2,6 +2,7 @@
#include "RobotMapParser.h"
#include "components/Motor.h"
+#include "components/Piston.h"
#include "components/Potentiometer.h"
#include "components/Encoder.h"
#include "components/Gyro.h"
@@ -13,7 +14,7 @@
#include
#include
-RobotMapParser::RobotMapParser(std::string robot, std::string filename, physics::ModelPtr model) {
+RobotMapParser::RobotMapParser(string robot, string filename, physics::ModelPtr model) {
this->robot = robot;
this->filename = filename;
this->model = model;
@@ -23,7 +24,7 @@ RobotMapParser::RobotMapParser(std::string robot, std::string filename, physics:
/*
* Parse the file and return a list of robot components for the robot
*/
-void RobotMapParser::parse(std::vector &components) {
+void RobotMapParser::parse(vector &components) {
// Load an xml document with root tag
XMLDocument doc;
doc.LoadFile(filename.c_str());
@@ -35,6 +36,8 @@ void RobotMapParser::parse(std::vector &components) {
// We don't care about comments and other non-element types
} else if (strcmp(current->Value(), "motor") == 0) {
components.push_back(parseMotor(current->ToElement()));
+ } else if (strcmp(current->Value(), "piston") == 0) {
+ components.push_back(parsePiston(current->ToElement()));
} else if (strcmp(current->Value(), "potentiometer") == 0) {
components.push_back(parsePotentiometer(current->ToElement()));
} else if (strcmp(current->Value(), "encoder") == 0) {
@@ -48,7 +51,7 @@ void RobotMapParser::parse(std::vector &components) {
} else if (strcmp(current->Value(), "rangefinder") == 0) {
components.push_back(parseRangefinder(current->ToElement()));
} else {
- std::cerr << "Unknown element: " << current->Value() << std::endl;
+ cerr << "Unknown element: " << current->Value() << endl;
}
}
}
@@ -67,13 +70,41 @@ void RobotMapParser::parse(std::vector &components) {
*
*/
RobotComponentPtr RobotMapParser::parseMotor(XMLElement *node) {
- std::string location = locationToPath(node->FirstChildElement("location"));
+ string location = locationToPath(node->FirstChildElement("location"));
physics::JointPtr joint = getJoint(node);
- double multiplier = boost::lexical_cast(getTagValue(
+ double multiplier = boost::lexical_cast(getChildTagValue(
node->FirstChildElement("parameters"), "multiplier").c_str());
return new Motor(location, joint, multiplier);
}
+/**
+ * Parse a piston tag. Piston tags have a joint the act on, a location
+ * they're "plugged into", the area of the cylinder and optionally a
+ * direction. The template of a piston| tag is as follows:
+ *
+ *
+ * {{ joint name (string) }}
+ * {{ location (see below for formats }}
+ *
+ * {{ area in proper units (number) }}
+ *
+ * {{ area in newtons (number) }}
+ * {{ area in newtons (number) }}
+ * {{ forward (default) or reversed }}
+ *
+ */
+RobotComponentPtr RobotMapParser::parsePiston(XMLElement *node) {
+ string location = locationToPath(node->FirstChildElement("location"));
+ physics::JointPtr joint = getJoint(node);
+ double forward = boost::lexical_cast(getChildTagValue(node, "forward-force").c_str());
+ double reverse = boost::lexical_cast(getChildTagValue(node, "reverse-force").c_str());
+ if (getChildTagValue(node, "direction") == "reversed") {
+ forward = -forward;
+ reverse = -reverse;
+ }
+ return new Piston(location, joint, forward, reverse);
+}
+
/**
* Parse a potentiometer tag. Potentiometer tags have a joint they measure, a
* location they're "plugged into" and one of three types (linear,
@@ -87,10 +118,10 @@ RobotComponentPtr RobotMapParser::parseMotor(XMLElement *node) {
*
*/
RobotComponentPtr RobotMapParser::parsePotentiometer(XMLElement *node) {
- std::string location = locationToPath(node->FirstChildElement("location"));
+ string location = locationToPath(node->FirstChildElement("location"));
physics::JointPtr joint = getJoint(node);
- std::string radians_str = getTagValue(node, "type", "degrees");
+ string radians_str = getChildTagValue(node, "type", "degrees");
bool radians;
if (radians_str == "linear" || radians_str == "radians") {
radians = true;
@@ -112,7 +143,7 @@ RobotComponentPtr RobotMapParser::parsePotentiometer(XMLElement *node) {
*
*/
RobotComponentPtr RobotMapParser::parseEncoder(XMLElement *node) {
- std::string location = locationToPath(node->FirstChildElement("location"));
+ string location = locationToPath(node->FirstChildElement("location"));
physics::JointPtr joint = getJoint(node);
return new Encoder(location, joint);
}
@@ -129,10 +160,10 @@ RobotComponentPtr RobotMapParser::parseEncoder(XMLElement *node) {
*
*/
RobotComponentPtr RobotMapParser::parseGyro(XMLElement *node) {
- std::string location = locationToPath(node->FirstChildElement("location"));
+ string location = locationToPath(node->FirstChildElement("location"));
physics::LinkPtr link = getLink(node);
ROTATION axis;
- std::string axisString = getTagValue(node, "axis");
+ string axisString = getChildTagValue(node, "axis");
if (axisString == "roll") axis = Roll;
if (axisString == "pitch") axis = Pitch;
if (axisString == "yaw") axis = Yaw;
@@ -153,13 +184,13 @@ RobotComponentPtr RobotMapParser::parseGyro(XMLElement *node) {
*
*/
RobotComponentPtr RobotMapParser::parseInternalLimitSwitch(XMLElement *node) {
- std::string location = locationToPath(node->FirstChildElement("location"));
+ string location = locationToPath(node->FirstChildElement("location"));
physics::JointPtr joint = getJoint(node);
- double min = boost::lexical_cast(getTagValue(node, "min", "0"));
- double max = boost::lexical_cast(getTagValue(node, "max", "1"));
+ double min = boost::lexical_cast(getChildTagValue(node, "min", "0"));
+ double max = boost::lexical_cast(getChildTagValue(node, "max", "1"));
- std::string radians_str = getTagValue(node, "type", "degrees");
+ string radians_str = getChildTagValue(node, "type", "degrees");
bool radians;
if (radians_str == "linear" || radians_str == "radians") {
radians = true;
@@ -180,7 +211,7 @@ RobotComponentPtr RobotMapParser::parseInternalLimitSwitch(XMLElement *node) {
*
*/
RobotComponentPtr RobotMapParser::parseExternalLimitSwitch(XMLElement *node) {
- std::string location = locationToPath(node->FirstChildElement("location"));
+ string location = locationToPath(node->FirstChildElement("location"));
sensors::ContactSensorPtr sensor =
boost::dynamic_pointer_cast(getSensor(node));
return new ExternalLimitSwitch(location, sensor);
@@ -197,18 +228,19 @@ RobotComponentPtr RobotMapParser::parseExternalLimitSwitch(XMLElement *node) {
*
*/
RobotComponentPtr RobotMapParser::parseRangefinder(XMLElement *node) {
- std::string location = locationToPath(node->FirstChildElement("location"));
+ string location = locationToPath(node->FirstChildElement("location"));
sensors::SonarSensorPtr sensor =
boost::dynamic_pointer_cast(getSensor(node));
return new Rangefinder(location, sensor);
}
+
/**
* Convert the location tag to a topic name to subscribe/publish
* to. Currently, there are two supported location styles: double_port
* and single port.
*/
-std::string RobotMapParser::locationToPath(XMLElement *location) {
+string RobotMapParser::locationToPath(XMLElement *location) {
if (location == NULL)
return "";
else if (location->Attribute("style", "double_port"))
@@ -217,24 +249,24 @@ std::string RobotMapParser::locationToPath(XMLElement *location) {
return portLocationToPath(location);
}
-std::string RobotMapParser::portLocationToPath(XMLElement *location) {
- std::string type = getTagValue(location, "type");
- std::string module = getTagValue(location, "module");
- std::string pin = getTagValue(location, "pin");
+string RobotMapParser::portLocationToPath(XMLElement *location) {
+ string type = getChildTagValue(location, "type");
+ string module = getChildTagValue(location, "module");
+ string pin = getChildTagValue(location, "pin");
return "~/" + robot + "/" + type + "/" + module + "/" + pin;
}
-std::string RobotMapParser::doublePortLocationToPath(XMLElement *location) {
- std::string type = getTagValue(location, "type");
- std::string module1 = getTagValue(location, "module1");
- std::string pin1 = getTagValue(location, "pin1");
- std::string module2 = getTagValue(location, "module2");
- std::string pin2 = getTagValue(location, "pin2");
+string RobotMapParser::doublePortLocationToPath(XMLElement *location) {
+ string type = getChildTagValue(location, "type");
+ string module1 = getChildTagValue(location, "module1");
+ string pin1 = getChildTagValue(location, "pin1");
+ string module2 = getChildTagValue(location, "module2");
+ string pin2 = getChildTagValue(location, "pin2");
// Swap pins if needed, to maintain consistent ordering
if ((module2 < module1) || ((module2 == module1) && (pin2 < pin1))) {
- std::string module = module2;
- std::string pin = pin2;
+ string module = module2;
+ string pin = pin2;
module2 = module1;
pin2 = pin1;
module1 = module;
@@ -243,7 +275,11 @@ std::string RobotMapParser::doublePortLocationToPath(XMLElement *location) {
return "~/" + robot + "/" + type + "/" + module1 + "/" + pin1 + "/" + module2 + "/" + pin2;
}
-std::string RobotMapParser::getTagValue(XMLElement *node, std::string tag, std::string default_value) {
+/**
+ * Returns the value of the first child tag element of the given node
+ * with the matching tag name.
+ */
+string RobotMapParser::getChildTagValue(XMLElement *node, string tag, string default_value) {
if (node->FirstChildElement(tag.c_str()) != NULL) {
return node->FirstChildElement(tag.c_str())->FirstChild()->Value();
} else {
@@ -251,23 +287,49 @@ std::string RobotMapParser::getTagValue(XMLElement *node, std::string tag, std::
}
}
+/**
+ * Returns the matching attribute of the given node.
+ */
+string RobotMapParser::getTagAttribute(XMLElement *node, string attr, string default_value) {
+ if (node->Attribute(attr.c_str())) {
+ return node->Attribute(attr.c_str());
+ } else {
+ return default_value;
+ }
+}
+
+/**
+ * Returns the matching attribute of the first child tag element of the given node
+ * with the matching tag name.
+ */
+string RobotMapParser::getChildTagAttribute(XMLElement *node, string tag,
+ string attr, string default_value) {
+ if (node->FirstChildElement(tag.c_str()) != NULL) {
+ string value = node->FirstChildElement(tag.c_str())->Attribute(attr.c_str());
+ if (value != "") {
+ return value;
+ }
+ }
+ return default_value;
+}
+
physics::JointPtr RobotMapParser::getJoint(XMLElement *node) {
- std::string joint_name = node->FirstChildElement("joint")->FirstChild()->Value();
+ string joint_name = node->FirstChildElement("joint")->FirstChild()->Value();
physics::JointPtr joint = model->GetJoint(joint_name);
- if (joint == NULL) std::cerr << "Joint doesn't exist: " << joint_name << std::endl;
+ if (joint == NULL) cerr << "Joint doesn't exist: " << joint_name << endl;
return joint;
}
physics::LinkPtr RobotMapParser::getLink(XMLElement *node) {
- std::string link_name = node->FirstChildElement("link")->FirstChild()->Value();
+ string link_name = node->FirstChildElement("link")->FirstChild()->Value();
physics::LinkPtr link = model->GetLink(link_name);
- if (link == NULL) std::cerr << "Link doesn't exist: " << link_name << std::endl;
+ if (link == NULL) cerr << "Link doesn't exist: " << link_name << endl;
return link;
}
sensors::SensorPtr RobotMapParser::getSensor(XMLElement *node) {
- std::string sensor_name = node->FirstChildElement("sensor")->FirstChild()->Value();
+ string sensor_name = node->FirstChildElement("sensor")->FirstChild()->Value();
sensors::SensorPtr sensor = sensors::get_sensor(sensor_name);
- if (sensor == NULL) std::cerr << "Sensor doesn't exist: " << sensor_name << std::endl;
+ if (sensor == NULL) cerr << "Sensor doesn't exist: " << sensor_name << endl;
return sensor;
}
diff --git a/simulation/frc_gazebo_plugin/src/RobotMapParser.h b/simulation/frc_gazebo_plugin/src/RobotMapParser.h
index cbdd9ddfbc..af5517afa6 100644
--- a/simulation/frc_gazebo_plugin/src/RobotMapParser.h
+++ b/simulation/frc_gazebo_plugin/src/RobotMapParser.h
@@ -8,32 +8,36 @@
#ifndef _ROBOTMAPPARSER_H_
#define _ROBOTMAPPARSER_H_
+using namespace std;
using namespace tinyxml2;
using namespace gazebo;
class RobotMapParser {
public:
- RobotMapParser(std::string robot, std::string filename, physics::ModelPtr model);
- void parse(std::vector &components);
+ RobotMapParser(string robot, string filename, physics::ModelPtr model);
+ void parse(vector &components);
private:
- std::string robot, filename;
+ string robot, filename;
physics::ModelPtr model;
physics::WorldPtr world;
RobotComponentPtr parseMotor(XMLElement *node);
+ RobotComponentPtr parsePiston(XMLElement *node);
RobotComponentPtr parsePotentiometer(XMLElement *node);
RobotComponentPtr parseEncoder(XMLElement *node);
RobotComponentPtr parseGyro(XMLElement *node);
RobotComponentPtr parseInternalLimitSwitch(XMLElement *node);
RobotComponentPtr parseExternalLimitSwitch(XMLElement *node);
RobotComponentPtr parseRangefinder(XMLElement *node);
-
- std::string locationToPath(XMLElement *location);
- std::string portLocationToPath(XMLElement *location);
- std::string doublePortLocationToPath(XMLElement *location);
- std::string getTagValue(XMLElement *node, std::string tag, std::string default_value="");
+ string locationToPath(XMLElement *location);
+ string portLocationToPath(XMLElement *location);
+ string doublePortLocationToPath(XMLElement *location);
+
+ string getChildTagValue(XMLElement *node, string tag, string default_value="");
+ string getTagAttribute(XMLElement *node, string attr, string default_value="");
+ string getChildTagAttribute(XMLElement *node, string tag, string attr, string default_value="");
physics::JointPtr getJoint(XMLElement *node);
physics::LinkPtr getLink(XMLElement *node);
sensors::SensorPtr getSensor(XMLElement *node);
diff --git a/simulation/frc_gazebo_plugin/src/components/Piston.cpp b/simulation/frc_gazebo_plugin/src/components/Piston.cpp
new file mode 100644
index 0000000000..ac0abe422f
--- /dev/null
+++ b/simulation/frc_gazebo_plugin/src/components/Piston.cpp
@@ -0,0 +1,32 @@
+
+#include "components/Piston.h"
+
+Piston::Piston(std::string topic, physics::JointPtr joint,
+ double forward_force, double reverse_force) {
+ std::cout << "Initializing piston: " << topic << " -- " << forward_force
+ << "/" << reverse_force << "N" << std::endl;
+ this->topic = topic;
+ this->forward_force = forward_force;
+ this->reverse_force = reverse_force;
+ this->joint = joint;
+ signal = -1;
+}
+
+Piston::~Piston() {
+
+}
+
+void Piston::connect(transport::NodePtr node) {
+ sub = node->Subscribe(topic, &Piston::callback, this);
+}
+
+void Piston::callback(const msgs::ConstFloat64Ptr &msg) {
+ signal = msg->data();
+ if (signal < -0.001) { signal = -reverse_force; }
+ else if (signal > 0.001) { signal = forward_force; }
+ else { signal = 0; }
+}
+
+void Piston::update(bool enabled) {
+ joint->SetForce(0, signal);
+}
diff --git a/simulation/frc_gazebo_plugin/src/components/Piston.h b/simulation/frc_gazebo_plugin/src/components/Piston.h
new file mode 100644
index 0000000000..5a1049b174
--- /dev/null
+++ b/simulation/frc_gazebo_plugin/src/components/Piston.h
@@ -0,0 +1,36 @@
+
+#include "components/RobotComponent.h"
+
+#include
+
+#ifndef _COMPONENTS_PISTON_H_
+#define _COMPONENTS_PISTON_H_
+
+// PSI in Newtons/m^2
+#define PSI(x) (x*6895)
+
+/**
+ * Piston models a pneumatic piston. In the URDF/SDF, the joint should
+ * be modeled as a prismatic joint with the limits corresponding to
+ * extended and retracted positions. Pistons have three states:
+ * forward, reverse and off. Currently, the forward and reverse forces
+ * are specified in Newtons and are constant. Changes in pressure are
+ * not modeled.
+ */
+class Piston : public RobotComponent {
+public:
+ Piston(std::string topic, physics::JointPtr joint, double forward_force, double reverse_force);
+ virtual ~Piston();
+
+ void connect(transport::NodePtr node);
+ void update(bool enabled);
+private:
+ std::string topic;
+ double forward_force, reverse_force, signal;
+ transport::SubscriberPtr sub;
+ physics::JointPtr joint;
+
+ void callback(const msgs::ConstFloat64Ptr &msg);
+};
+
+#endif /* _COMPONENTS_PISTON_H_ */
diff --git a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
new file mode 100644
index 0000000000..f036b29f41
--- /dev/null
+++ b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
@@ -0,0 +1,184 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. 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;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.simulation.SimEncoder;
+import edu.wpi.first.wpilibj.simulation.SimSpeedController;
+import edu.wpi.first.wpilibj.tables.ITable;
+import edu.wpi.first.wpilibj.tables.ITableListener;
+import edu.wpi.first.wpilibj.util.AllocationException;
+import edu.wpi.first.wpilibj.util.CheckedAllocationException;
+
+/**
+ * DoubleSolenoid class for running 2 channels of high voltage Digital Output
+ * (9472 module).
+ *
+ * The DoubleSolenoid class is typically used for pneumatics solenoids that
+ * have two positions controlled by two separate channels.
+ */
+public class DoubleSolenoid implements LiveWindowSendable {
+
+ /**
+ * Possible values for a DoubleSolenoid
+ */
+ public static class Value {
+
+ public final int value;
+ public static final int kOff_val = 0;
+ public static final int kForward_val = 1;
+ public static final int kReverse_val = 2;
+ public static final Value kOff = new Value(kOff_val);
+ public static final Value kForward = new Value(kForward_val);
+ public static final Value kReverse = new Value(kReverse_val);
+
+ private Value(int value) {
+ this.value = value;
+ }
+ }
+
+ private int m_forwardChannel; ///< The forward channel on the module to control.
+ private int m_reverseChannel; ///< The reverse channel on the module to control.
+ private int m_moduleNumber;
+ private SimSpeedController m_impl;
+ private Value m_value;
+
+ /**
+ * Common function to implement constructor behavior.
+ */
+ private synchronized void initSolenoid(int moduleNumber, int forwardChannel, int reverseChannel) {
+ m_forwardChannel = forwardChannel;
+ m_reverseChannel = reverseChannel;
+ m_moduleNumber = moduleNumber;
+// LiveWindow.addActuator("DoubleSolenoid", m_moduleNumber, m_forwardChannel, this);
+
+ if (reverseChannel < forwardChannel) { // Swap ports
+ int channel = forwardChannel;
+ forwardChannel = reverseChannel;
+ reverseChannel = channel;
+ }
+ m_impl = new SimSpeedController("simulator/pneumatic/"+moduleNumber+"/"+forwardChannel+"/"+moduleNumber+"/"+reverseChannel);
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param forwardChannel The forward channel on the module to control.
+ * @param reverseChannel The reverse channel on the module to control.
+ */
+ public DoubleSolenoid(final int forwardChannel, final int reverseChannel) {
+ initSolenoid(1, forwardChannel, reverseChannel);
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param moduleNumber The module number of the solenoid module to use.
+ * @param forwardChannel The forward channel on the module to control.
+ * @param reverseChannel The reverse channel on the module to control.
+ */
+ public DoubleSolenoid(final int moduleNumber, final int forwardChannel, final int reverseChannel) {
+ initSolenoid(moduleNumber, forwardChannel, reverseChannel);
+ }
+
+ /**
+ * Destructor.
+ */
+ public synchronized void free() {}
+
+ /**
+ * Set the value of a solenoid.
+ *
+ * @param value Move the solenoid to forward, reverse, or don't move it.
+ */
+ public void set(final Value value) {
+ m_value = value;
+ switch (value.value) {
+ case Value.kOff_val:
+ m_impl.set(0);
+ break;
+ case Value.kForward_val:
+ m_impl.set(1);
+ break;
+ case Value.kReverse_val:
+ m_impl.set(-1);
+ break;
+ }
+ }
+
+ /**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+ public Value get() {
+ return m_value;
+ }
+
+ /*
+ * Live Window code, only does anything if live window is activated.
+ */
+ public String getSmartDashboardType() {
+ return "Double Solenoid";
+ }
+ private ITable m_table;
+ private ITableListener m_table_listener;
+
+ /**
+ * {@inheritDoc}
+ */
+ public void initTable(ITable subtable) {
+ m_table = subtable;
+ updateTable();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public ITable getTable() {
+ return m_table;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public void updateTable() {
+ if (m_table != null) {
+ //TODO: this is bad
+ m_table.putString("Value", (get() == Value.kForward ? "Forward" : (get() == Value.kReverse ? "Reverse" : "Off")));
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public void startLiveWindowMode() {
+ set(Value.kOff); // Stop for safety
+ m_table_listener = new ITableListener() {
+ public void valueChanged(ITable itable, String key, Object value, boolean bln) {
+ //TODO: this is bad also
+ if (value.toString().equals("Reverse"))
+ set(Value.kReverse);
+ else if (value.toString().equals("Forward"))
+ set(Value.kForward);
+ else
+ set(Value.kOff);
+ }
+ };
+ m_table.addTableListener("Value", m_table_listener, true);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public void stopLiveWindowMode() {
+ set(Value.kOff); // Stop for safety
+ // TODO: Broken, should only remove the listener from "Value" only.
+ m_table.removeTableListener(m_table_listener);
+ }
+}
diff --git a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Solenoid.java b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
new file mode 100644
index 0000000000..aed39ca75d
--- /dev/null
+++ b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
@@ -0,0 +1,141 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. 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;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.simulation.SimSpeedController;
+import edu.wpi.first.wpilibj.tables.ITable;
+import edu.wpi.first.wpilibj.tables.ITableListener;
+
+/**
+ * Solenoid class for running high voltage Digital Output (9472 module).
+ *
+ * The Solenoid class is typically used for pneumatics solenoids, but could be used
+ * for any device within the current spec of the 9472 module.
+ */
+public class Solenoid implements LiveWindowSendable {
+
+ private int m_moduleNumber, m_channel; ///< The channel on the module to control.
+ private boolean m_value;
+ private SimSpeedController m_impl;
+
+ /**
+ * Common function to implement constructor behavior.
+ */
+ private synchronized void initSolenoid(int slot, int channel) {
+ m_moduleNumber = slot;
+ m_channel = channel;
+ m_impl = new SimSpeedController("simulator/pneumatic/"+slot+"/"+channel);
+
+ LiveWindow.addActuator("Solenoid", m_moduleNumber, m_channel, this);
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param channel The channel on the module to control.
+ */
+ public Solenoid(final int channel) {
+ initSolenoid(1, channel);
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param moduleNumber The module number of the solenoid module to use.
+ * @param channel The channel on the module to control.
+ */
+ public Solenoid(final int moduleNumber, final int channel) {
+ initSolenoid(moduleNumber, channel);
+ }
+
+ /**
+ * Destructor.
+ */
+ public synchronized void free() {}
+
+ /**
+ * Set the value of a solenoid.
+ *
+ * @param on Turn the solenoid output off or on.
+ */
+ public void set(boolean on) {
+ m_value = on;
+ if (on) {
+ m_impl.set(1);
+ } else {
+ m_impl.set(-1);
+ }
+ }
+
+ /**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+ public boolean get() {
+ return m_value;
+ }
+
+ /*
+ * Live Window code, only does anything if live window is activated.
+ */
+ public String getSmartDashboardType() {
+ return "Solenoid";
+ }
+ private ITable m_table;
+ private ITableListener m_table_listener;
+
+ /**
+ * {@inheritDoc}
+ */
+ public void initTable(ITable subtable) {
+ m_table = subtable;
+ updateTable();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public ITable getTable() {
+ return m_table;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public void updateTable() {
+ if (m_table != null) {
+ m_table.putBoolean("Value", get());
+ }
+ }
+
+
+ /**
+ * {@inheritDoc}
+ */
+ public void startLiveWindowMode() {
+ set(false); // Stop for safety
+ m_table_listener = new ITableListener() {
+ public void valueChanged(ITable itable, String key, Object value, boolean bln) {
+ set(((Boolean) value).booleanValue());
+ }
+ };
+ m_table.addTableListener("Value", m_table_listener, true);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public void stopLiveWindowMode() {
+ set(false); // Stop for safety
+ // TODO: Broken, should only remove the listener from "Value" only.
+ m_table.removeTableListener(m_table_listener);
+ }
+}