The POV angles start at 0 in the up direction, and increase clockwise (eg right is 90,
+ * upper-left is 315).
+ *
+ * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ */
+ public int getDegree() {
+ return gamepad.gamepad.getPOV(id);
+ }
+
+ public String getName() {
+ return name;
+ }
+
+ }
+
+ public class Thumbstick {
+
+ public final GamepadAxis X, Y;
+
+ private String name;
+
+ public Thumbstick(Gamepad gamepad, String name, int axisX, int axisY, boolean invertX, boolean invertY) {
+ X = new GamepadAxis(gamepad, name + "_X", axisX, invertX);
+ Y = new GamepadAxis(gamepad, name + "_Y", axisY, invertY);
+
+ this.name = name;
+
+ gamepad.registerThumbstick(this, name);
+ }
+
+ public String getName() {
+ return name;
+ }
+
+ }
+
+ public final Joystick gamepad;
+
+ private String name;
+
+ private Map You can add additional auto modes by adding additional commands to the
- * chooser code above (like the commented example) or additional comparisons
- * to the switch structure below with additional strings & commands.
+ *
+ * You can add additional auto modes by adding additional commands to the
+ * chooser code above (like the commented example) or additional comparisons to
+ * the switch structure below with additional strings & commands.
*/
@Override
public void autonomousInit() {
- m_autonomousCommand = m_chooser.getSelected();
-
+ //autonomousCommand = chooser.getSelected();
+ autonomousCommand = new AutoBlindBaseline();
/*
- * String autoSelected = SmartDashboard.getString("Auto Selector",
- * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
- * = new MyAutoCommand(); break; case "Default Auto": default:
- * autonomousCommand = new ExampleCommand(); break; }
+ * String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
+ * switch(autoSelected) { case "My Auto": autonomousCommand = new
+ * MyAutoCommand(); break; case "Default Auto": default: autonomousCommand = new
+ * ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
- if (m_autonomousCommand != null) {
- m_autonomousCommand.start();
+ if (autonomousCommand != null) {
+ autonomousCommand.start();
}
}
@@ -99,8 +117,8 @@ public void teleopInit() {
// 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();
+ if (autonomousCommand != null) {
+ autonomousCommand.cancel();
}
}
@@ -118,4 +136,5 @@ public void teleopPeriodic() {
@Override
public void testPeriodic() {
}
+
}
diff --git a/src/org/usfirst/frc/team4737/robot/RobotMap.java b/src/org/usfirst/frc/team4737/robot/RobotMap.java
index e4f15f9..a8e9bd8 100644
--- a/src/org/usfirst/frc/team4737/robot/RobotMap.java
+++ b/src/org/usfirst/frc/team4737/robot/RobotMap.java
@@ -14,13 +14,71 @@
* floating around.
*/
public class RobotMap {
- // For example to map the left and right motors, you could define the
- // following variables to use with your drivetrain subsystem.
- // public static int leftMotor = 1;
- // public static int rightMotor = 2;
-
- // If you are using multiple modules, make sure to define both the port
- // number and the module. For example you with a rangefinder:
- // public static int rangefinderPort = 1;
- // public static int rangefinderModule = 1;
+
+ // #####################
+ // Actuator IDs/Channels
+ // #####################
+
+ // Drivetrain
+ public static final int DRIVE_LEFT_MASTER = 11;
+ public static final int DRIVE_LEFT_SLAVE = 12;
+ public static final int DRIVE_RIGHT_MASTER = 13;
+ public static final int DRIVE_RIGHT_SLAVE = 14;
+
+ // Intake
+ public static final int INTAKE_LEFT = 15;
+ public static final int INTAKE_RIGHT = 16;
+
+ public static final int INTAKE_LEFT_PISTON_MODULE = 0;
+ public static final int INTAKE_LEFT_PISTON_FORWARD_CHANNEL = 0;
+ public static final int INTAKE_LEFT_PISTON_REVERSE_CHANNEL = 1;
+ public static final int INTAKE_RIGHT_PISTON_MODULE = 0;
+ public static final int INTAKE_RIGHT_PISTON_FORWARD_CHANNEL = 2;
+ public static final int INTAKE_RIGHT_PISTON_REVERSE_CHANNEL = 3;
+
+ // Elevator
+ public static final int ELEVATOR_MOTOR = 17;
+
+ // #########
+ // Constants
+ // #########
+
+ /**
+ * The voltage to give to the elevator for it to hold constant position with a
+ * cube
+ *
+ * Increase this value if the elevator droops too much while holding the cube
+ */
+ public static final double ELEVATOR_HOLD_V = 1;
+ /**
+ * The number of seconds to keep the elevator in place before letting the motor
+ * relax. To disable this, set the value to 0.1
+ */
+ public static final double ELEVATOR_HOLD_TIME = 4; // TODO tune
+ /**
+ * The maximum speed the elevator is allowed to go down. This prevents
+ * unspooling too quickly, which causes the rope to fall off the spool.
+ */
+ public static final double ELEVATOR_MAX_DOWN_SPEED = -0.6;
+
+ /**
+ * The value to multiply the throttle by when slow driving is enabled
+ */
+ public static final double DRIVE_SLOW_SCALE = 0.5;
+
+ /*
+ * Adjust these variables to tune autonomous
+ */
+ public static final double AUTO_BLIND_TIME = 5;
+ public static final double AUTO_BLIND_SPEED = 0.5;
+ /**
+ * The steering value to use in autonomous to counteract any natural tendencies
+ * for the robot to veer left or right. A positive value (should) steer the
+ * robot to the right, and negative to the left.
+ *
+ * Tune this value accordingly. It may need to be adjusted throughout the
+ * competition.
+ */
+ public static final double AUTO_BLIND_STEER = 0;
+
}
diff --git a/src/org/usfirst/frc/team4737/robot/commands/BuzzController.java b/src/org/usfirst/frc/team4737/robot/commands/BuzzController.java
new file mode 100644
index 0000000..c3ca684
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/BuzzController.java
@@ -0,0 +1,41 @@
+package org.usfirst.frc.team4737.robot.commands;
+
+import org.usfirst.frc.team4737.lib.Gamepad;
+
+import edu.wpi.first.wpilibj.command.TimedCommand;
+
+/**
+ *
+ */
+public class BuzzController extends TimedCommand {
+
+ private double strength;
+ private Gamepad controller;
+
+ public BuzzController(double timeout, double strength, Gamepad controller) {
+ super(timeout);
+ // requires(none);
+ this.strength = strength;
+ this.controller = controller;
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ controller.setRumble(strength);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ }
+
+ // Called once after timeout
+ protected void end() {
+ controller.setRumble(0);
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ }
+
+}
diff --git a/src/org/usfirst/frc/team4737/robot/commands/ExampleCommand.java b/src/org/usfirst/frc/team4737/robot/commands/ExampleCommand.java
deleted file mode 100644
index d8e5616..0000000
--- a/src/org/usfirst/frc/team4737/robot/commands/ExampleCommand.java
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package org.usfirst.frc.team4737.robot.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team4737.robot.Robot;
-
-/**
- * An example command. You can replace me with your own command.
- */
-public class ExampleCommand extends Command {
- public ExampleCommand() {
- // Use requires() here to declare subsystem dependencies
- requires(Robot.kExampleSubsystem);
- }
-
- // Called just before this Command runs the first time
- @Override
- protected void initialize() {
- }
-
- // Called repeatedly when this Command is scheduled to run
- @Override
- protected void execute() {
- }
-
- // Make this return true when this Command no longer needs to run execute()
- @Override
- protected boolean isFinished() {
- return false;
- }
-
- // Called once after isFinished returns true
- @Override
- protected void end() {
- }
-
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
- @Override
- protected void interrupted() {
- }
-}
diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java
new file mode 100644
index 0000000..7dbe617
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java
@@ -0,0 +1,90 @@
+package org.usfirst.frc.team4737.robot.commands.drivetrain;
+
+import org.usfirst.frc.team4737.robot.Robot;
+import org.usfirst.frc.team4737.robot.RobotMap;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Autonomously accelerates/decelerates the robot to a target speed from a
+ * starting speed.
+ */
+public class AutoBlindAccelerate extends Command {
+
+ private double percentPerSecond;
+ private double startPercent;
+ private double targetPercent;
+
+ private boolean startLow;
+
+ private double lastTime;
+ private double percent;
+
+ /**
+ *
+ * @param percentPerSecond
+ * The % speed to accelerate by per second. A value of 1 will
+ * accelerate the robot from a standstill to full speed in one
+ * second.
+ * @param startPercent
+ * The initial speed (between -1.0 and 1.0) of the robot.
+ * @param targetPercent
+ * The speed (between -1.0 and 1.0) to accelerate to.
+ */
+ public AutoBlindAccelerate(double percentPerSecond, double startPercent, double targetPercent) {
+ requires(Robot.DRIVETRAIN);
+ this.percentPerSecond = percentPerSecond;
+ this.startPercent = startPercent;
+ this.targetPercent = targetPercent;
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ lastTime = 0;
+ percent = startPercent;
+ startLow = startPercent < targetPercent;
+
+ Robot.DRIVETRAIN.enableVoltageCompensation();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ double time = this.timeSinceInitialized();
+ double delta = time - lastTime;
+ lastTime = time;
+
+ // Adjust speed based on time passed
+ if (startLow)
+ percent += percentPerSecond * delta;
+ else
+ percent -= percentPerSecond * delta;
+
+ // Check bounds
+ if (startLow && percent > targetPercent)
+ percent = targetPercent;
+ if (!startLow && percent < targetPercent)
+ percent = targetPercent;
+ if (percent < -1)
+ percent = -1;
+ if (percent > 1)
+ percent = 1;
+
+ // Set drive speed
+ Robot.DRIVETRAIN.arcadeDrive(percent, RobotMap.AUTO_BLIND_STEER);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return percent == targetPercent;
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindBaseline.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindBaseline.java
new file mode 100644
index 0000000..75f39e1
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindBaseline.java
@@ -0,0 +1,47 @@
+package org.usfirst.frc.team4737.robot.commands.drivetrain;
+
+import org.usfirst.frc.team4737.robot.RobotMap;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutoBlindBaseline extends CommandGroup {
+
+ public AutoBlindBaseline() {
+ // Add Commands here:
+ // e.g. addSequential(new Command1());
+ // addSequential(new Command2());
+ // these will run in order.
+
+ // To run multiple commands at the same time,
+ // use addParallel()
+ // e.g. addParallel(new Command1());
+ // addSequential(new Command2());
+ // Command1 and Command2 will run in parallel.
+
+ // A command group will require all of the subsystems that each member
+ // would require.
+ // e.g. if Command1 requires chassis, and Command2 requires arm,
+ // a CommandGroup containing them would require both the chassis and the
+ // arm.
+
+ /*
+ * Accelerates the robot to the configured speed, drives for the configured
+ * time, and decelerates to a stop.
+ *
+ * If the robot drives too far, decrease the time. If the robot drives too fast,
+ * decrease the speed. If the robot speeds up too fast, decrease the
+ * acceleration. Likewise, if it takes too long to speed up, increase
+ * acceleration.
+ *
+ * If the robot veers left or right, adjust the AUTO_BLIND_STEER value in RobotMap.
+ */
+ double acceleration = 0.1;
+ addSequential(new AutoBlindAccelerate(acceleration, 0, RobotMap.AUTO_BLIND_SPEED));
+ addSequential(new AutoBlindDrive(RobotMap.AUTO_BLIND_TIME, RobotMap.AUTO_BLIND_SPEED));
+ addSequential(new AutoBlindAccelerate(acceleration, RobotMap.AUTO_BLIND_SPEED, 0));
+ }
+
+}
diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindDrive.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindDrive.java
new file mode 100644
index 0000000..17d23e6
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindDrive.java
@@ -0,0 +1,52 @@
+package org.usfirst.frc.team4737.robot.commands.drivetrain;
+
+import org.usfirst.frc.team4737.robot.Robot;
+import org.usfirst.frc.team4737.robot.RobotMap;
+
+import edu.wpi.first.wpilibj.command.TimedCommand;
+
+/**
+ * Drives the robot forward autonomously without sensor input. Drives at a
+ * certain speed for a certain time.
+ *
+ * This should be used only in conjunction with AutoBlindAccelerate to get up to
+ * speed. If the robot does not accelerate first, it may jerk and tip over.
+ */
+public class AutoBlindDrive extends TimedCommand {
+
+ private double percent;
+
+ /**
+ *
+ * @param time
+ * The number of seconds to drive forward for.
+ * @param percent
+ * The percent output, or speed (between -1.0 and 1.0) to drive at.
+ */
+ public AutoBlindDrive(double time, double percent) {
+ super(time);
+ requires(Robot.DRIVETRAIN);
+
+ this.percent = percent;
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.DRIVETRAIN.enableVoltageCompensation();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.DRIVETRAIN.arcadeDrive(percent, RobotMap.AUTO_BLIND_STEER);
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/commands/drivetrain/RelaxDrivetrain.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/RelaxDrivetrain.java
new file mode 100644
index 0000000..d2a04b2
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/RelaxDrivetrain.java
@@ -0,0 +1,23 @@
+package org.usfirst.frc.team4737.robot.commands.drivetrain;
+
+import org.usfirst.frc.team4737.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+/**
+ * Configures the drivetrain to coast mode. This allows the robot to be pushed
+ * around more easily.
+ */
+public class RelaxDrivetrain extends InstantCommand {
+
+ public RelaxDrivetrain() {
+ requires(Robot.DRIVETRAIN);
+ this.setRunWhenDisabled(true);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.DRIVETRAIN.setCoastMode();
+ }
+
+}
diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopRacingDrive.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopRacingDrive.java
new file mode 100644
index 0000000..fa848c2
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopRacingDrive.java
@@ -0,0 +1,47 @@
+package org.usfirst.frc.team4737.robot.commands.drivetrain;
+
+import org.usfirst.frc.team4737.robot.Robot;
+import org.usfirst.frc.team4737.robot.RobotMap;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class TeleopRacingDrive extends Command {
+
+ public TeleopRacingDrive() {
+ requires(Robot.DRIVETRAIN);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.DRIVETRAIN.setBrakeMode();
+ Robot.DRIVETRAIN.disableVoltageCompensation();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ boolean slow = Robot.OI.driver.getButton("LB").get();
+ double throttle = (Robot.OI.driver.getAxis("RT").get() - Robot.OI.driver.getAxis("LT").get())
+ * (slow ? RobotMap.DRIVE_SLOW_SCALE : 1);
+ double steer = Robot.OI.driver.getThumbstick("LS").X.get() * (slow ? RobotMap.DRIVE_SLOW_SCALE : 1);
+
+ Robot.DRIVETRAIN.arcadeDrive(throttle, steer);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java
new file mode 100644
index 0000000..141679c
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java
@@ -0,0 +1,45 @@
+package org.usfirst.frc.team4737.robot.commands.drivetrain;
+
+import org.usfirst.frc.team4737.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class TeleopTankDrive extends Command {
+
+ public TeleopTankDrive() {
+ // Use requires() here to declare subsystem dependencies
+ requires(Robot.DRIVETRAIN);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.DRIVETRAIN.setBrakeMode();
+ Robot.DRIVETRAIN.disableVoltageCompensation();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ boolean slow = Robot.OI.driver.getButton("RB").get();
+ Robot.DRIVETRAIN.tankDrive(
+ -Robot.OI.driver.getThumbstick("LS").Y.get() * (slow ? 0.6 : 1),
+ -Robot.OI.driver.getThumbstick("RS").Y.get() * (slow ? 0.6 : 1));
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/commands/elevator/ControlElevator.java b/src/org/usfirst/frc/team4737/robot/commands/elevator/ControlElevator.java
new file mode 100644
index 0000000..8ca2264
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/elevator/ControlElevator.java
@@ -0,0 +1,61 @@
+package org.usfirst.frc.team4737.robot.commands.elevator;
+
+import org.usfirst.frc.team4737.robot.Robot;
+import org.usfirst.frc.team4737.robot.RobotMap;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ControlElevator extends Command {
+
+ public ControlElevator() {
+ requires(Robot.ELEVATOR);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.ELEVATOR.setBrakeMode();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ // Control elevator movement, holding it in place by default
+
+ // Grab input
+ double input = Robot.OI.operator.getAxis("LS_Y").get();
+
+ // Make sure we don't go down too fast
+ if (input < RobotMap.ELEVATOR_MAX_DOWN_SPEED) {
+ input = RobotMap.ELEVATOR_MAX_DOWN_SPEED;
+ }
+
+ // Start hold command if no input
+ if (input == 0) {
+ if (this.isRunning()) {
+ new HoldElevator().start();
+ this.cancel();
+ }
+ return;
+ }
+
+ // Set elevator speed
+ Robot.ELEVATOR.setSpeed(input);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/commands/elevator/HoldElevator.java b/src/org/usfirst/frc/team4737/robot/commands/elevator/HoldElevator.java
new file mode 100644
index 0000000..6bd2c20
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/elevator/HoldElevator.java
@@ -0,0 +1,52 @@
+package org.usfirst.frc.team4737.robot.commands.elevator;
+
+import org.usfirst.frc.team4737.robot.Robot;
+import org.usfirst.frc.team4737.robot.RobotMap;
+import org.usfirst.frc.team4737.robot.commands.BuzzController;
+
+import edu.wpi.first.wpilibj.command.TimedCommand;
+
+/**
+ *
+ */
+public class HoldElevator extends TimedCommand {
+
+ private boolean startedBuzz;
+
+ public HoldElevator() {
+ super(RobotMap.ELEVATOR_HOLD_TIME);
+ requires(Robot.ELEVATOR);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.ELEVATOR.setHoldPosition(true);
+ Robot.ELEVATOR.setBrakeMode();
+
+ startedBuzz = false;
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.ELEVATOR.setSpeed(0);
+
+ // If one second remaining, buzz controller to notify operator
+ if (RobotMap.ELEVATOR_HOLD_TIME - this.timeSinceInitialized() < 1.0 && !startedBuzz) {
+ new BuzzController(1, 0.3, Robot.OI.operator).start();
+ startedBuzz = true;
+ }
+ }
+
+ // Called once after timeout
+ protected void end() {
+ new RelaxElevator().start();
+ Robot.ELEVATOR.setHoldPosition(false);
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ Robot.ELEVATOR.setHoldPosition(false);
+ }
+
+}
diff --git a/src/org/usfirst/frc/team4737/robot/commands/elevator/RelaxElevator.java b/src/org/usfirst/frc/team4737/robot/commands/elevator/RelaxElevator.java
new file mode 100644
index 0000000..2e50df0
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/elevator/RelaxElevator.java
@@ -0,0 +1,42 @@
+package org.usfirst.frc.team4737.robot.commands.elevator;
+
+import org.usfirst.frc.team4737.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Relaxes the elevator. Allows it to fall down on its own and disables any
+ * motor output.
+ */
+public class RelaxElevator extends Command {
+
+ public RelaxElevator() {
+ requires(Robot.ELEVATOR);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.ELEVATOR.setCoastMode();
+ Robot.ELEVATOR.setHoldPosition(false);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.ELEVATOR.setSpeed(0);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/commands/elevator/StopElevator.java b/src/org/usfirst/frc/team4737/robot/commands/elevator/StopElevator.java
new file mode 100644
index 0000000..73a370f
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/elevator/StopElevator.java
@@ -0,0 +1,43 @@
+package org.usfirst.frc.team4737.robot.commands.elevator;
+
+import org.usfirst.frc.team4737.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Default command for the elevator. Disables all motor output, but puts
+ * elevator in brake mode to prevent it from falling too fast. Keep in mind that
+ * enabling this command does not put the elevator in a safe state. That is, it
+ * does not stop it from chopping off fingers.
+ */
+public class StopElevator extends Command {
+
+ public StopElevator() {
+ requires(Robot.ELEVATOR);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.ELEVATOR.setBrakeMode();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.ELEVATOR.setSpeed(0);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/commands/intake/ControlIntake.java b/src/org/usfirst/frc/team4737/robot/commands/intake/ControlIntake.java
new file mode 100644
index 0000000..fbd9256
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/intake/ControlIntake.java
@@ -0,0 +1,42 @@
+package org.usfirst.frc.team4737.robot.commands.intake;
+
+import org.usfirst.frc.team4737.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ControlIntake extends Command {
+
+ public ControlIntake() {
+ requires(Robot.INTAKE);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ double speed = Robot.OI.operator.getAxis("LT").get() - Robot.OI.operator.getAxis("RT").get();
+ double twist = Robot.OI.operator.getAxis("RS_X").get();
+ Robot.INTAKE.setLRSpeed(speed - twist, speed + twist);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // 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() {
+ Robot.INTAKE.setSpeed(0);
+ }
+
+}
diff --git a/src/org/usfirst/frc/team4737/robot/commands/intake/ReverseIntake.java b/src/org/usfirst/frc/team4737/robot/commands/intake/ReverseIntake.java
new file mode 100644
index 0000000..22aa992
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/intake/ReverseIntake.java
@@ -0,0 +1,39 @@
+package org.usfirst.frc.team4737.robot.commands.intake;
+
+import org.usfirst.frc.team4737.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ReverseIntake extends Command {
+
+ public ReverseIntake() {
+ // Use requires() here to declare subsystem dependencies
+ requires(Robot.INTAKE);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.INTAKE.setSpeed(-1);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/commands/intake/RunIntake.java b/src/org/usfirst/frc/team4737/robot/commands/intake/RunIntake.java
new file mode 100644
index 0000000..04913aa
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/intake/RunIntake.java
@@ -0,0 +1,39 @@
+package org.usfirst.frc.team4737.robot.commands.intake;
+
+import org.usfirst.frc.team4737.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class RunIntake extends Command {
+
+ public RunIntake() {
+ // Use requires() here to declare subsystem dependencies
+ requires(Robot.INTAKE);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.INTAKE.setSpeed(1);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/commands/intake/StopIntake.java b/src/org/usfirst/frc/team4737/robot/commands/intake/StopIntake.java
new file mode 100644
index 0000000..cc3b15e
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/intake/StopIntake.java
@@ -0,0 +1,39 @@
+package org.usfirst.frc.team4737.robot.commands.intake;
+
+import org.usfirst.frc.team4737.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class StopIntake extends Command {
+
+ public StopIntake() {
+ // Use requires() here to declare subsystem dependencies
+ requires(Robot.INTAKE);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.INTAKE.setSpeed(0);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/commands/intake/TwistIntake.java b/src/org/usfirst/frc/team4737/robot/commands/intake/TwistIntake.java
new file mode 100644
index 0000000..7b9f8bf
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/intake/TwistIntake.java
@@ -0,0 +1,43 @@
+package org.usfirst.frc.team4737.robot.commands.intake;
+
+import org.usfirst.frc.team4737.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class TwistIntake extends Command {
+
+ private double speed;
+
+ public TwistIntake(double speed) {
+ requires(Robot.INTAKE);
+
+ this.speed = speed;
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.INTAKE.setLRSpeed(speed, -speed);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/commands/intakegrip/ControlIntakeGrip.java b/src/org/usfirst/frc/team4737/robot/commands/intakegrip/ControlIntakeGrip.java
new file mode 100644
index 0000000..a14e189
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/intakegrip/ControlIntakeGrip.java
@@ -0,0 +1,46 @@
+package org.usfirst.frc.team4737.robot.commands.intakegrip;
+
+import org.usfirst.frc.team4737.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ControlIntakeGrip extends Command {
+
+ public ControlIntakeGrip() {
+ requires(Robot.INTAKEGRIP);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ boolean grab = Robot.OI.operator.getButton("A").get();
+ boolean release = Robot.OI.operator.getButton("B").get();
+
+ if (grab) {
+ Robot.INTAKEGRIP.closePneumatics();
+ } else if (release) {
+ Robot.INTAKEGRIP.openPneumatics();
+ }
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/commands/intakegrip/StopIntakeGrip.java b/src/org/usfirst/frc/team4737/robot/commands/intakegrip/StopIntakeGrip.java
new file mode 100644
index 0000000..9559cfc
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/commands/intakegrip/StopIntakeGrip.java
@@ -0,0 +1,39 @@
+package org.usfirst.frc.team4737.robot.commands.intakegrip;
+
+import org.usfirst.frc.team4737.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class StopIntakeGrip extends Command {
+
+ public StopIntakeGrip() {
+ requires(Robot.INTAKEGRIP);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.INTAKEGRIP.disablePneumatics();
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // 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/src/org/usfirst/frc/team4737/robot/subsystems/Climber.java b/src/org/usfirst/frc/team4737/robot/subsystems/Climber.java
new file mode 100644
index 0000000..d02bd28
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/subsystems/Climber.java
@@ -0,0 +1,25 @@
+package org.usfirst.frc.team4737.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ * This subsystem currently does not exist on the robot. This class serves as a
+ * placeholder in case it is added.
+ */
+public class Climber extends Subsystem {
+
+ // Put methods for controlling this subsystem
+ // here. Call these from Commands.
+
+ public Climber() {
+ // TODO when we know how the climber works #BlameMechanical
+ }
+
+ public void initDefaultCommand() {
+ // Set the default command for a subsystem here.
+ // setDefaultCommand(new MySpecialCommand());
+ }
+
+}
diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/ControlSystem.java b/src/org/usfirst/frc/team4737/robot/subsystems/ControlSystem.java
new file mode 100644
index 0000000..60635b6
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/subsystems/ControlSystem.java
@@ -0,0 +1,36 @@
+package org.usfirst.frc.team4737.robot.subsystems;
+
+import edu.wpi.first.wpilibj.Compressor;
+import edu.wpi.first.wpilibj.PowerDistributionPanel;
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ *
+ */
+public class ControlSystem extends Subsystem {
+
+ private Compressor compressor;
+ public PowerDistributionPanel pdp;
+
+ public ControlSystem() {
+ compressor = new Compressor(0);
+ pdp = new PowerDistributionPanel();
+
+ // Enable the compressor
+ enableCompressor();
+ }
+
+ public void initDefaultCommand() {
+ // Set the default command for a subsystem here.
+ // setDefaultCommand(new MySpecialCommand());
+ }
+
+ public void enableCompressor() {
+ compressor.setClosedLoopControl(true);
+ }
+
+ public void disableCompressor() {
+ compressor.setClosedLoopControl(false);
+ }
+
+}
diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java
new file mode 100644
index 0000000..07b8e37
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java
@@ -0,0 +1,93 @@
+package org.usfirst.frc.team4737.robot.subsystems;
+
+import org.usfirst.frc.team4737.robot.RobotMap;
+import org.usfirst.frc.team4737.robot.commands.drivetrain.TeleopRacingDrive;
+import org.usfirst.frc.team4737.robot.commands.drivetrain.TeleopTankDrive;
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+
+/**
+ *
+ */
+public class Drivetrain extends Subsystem {
+
+ private WPI_TalonSRX leftFrontMaster;
+ private WPI_TalonSRX rightFrontMaster;
+ private WPI_TalonSRX leftBackSlave;
+ private WPI_TalonSRX rightBackSlave;
+
+ private DifferentialDrive drive;
+
+ public Drivetrain() {
+ leftFrontMaster = new WPI_TalonSRX(RobotMap.DRIVE_LEFT_MASTER);
+ leftBackSlave = new WPI_TalonSRX(RobotMap.DRIVE_LEFT_SLAVE);
+ rightFrontMaster = new WPI_TalonSRX(RobotMap.DRIVE_RIGHT_MASTER);
+ rightBackSlave = new WPI_TalonSRX(RobotMap.DRIVE_RIGHT_SLAVE);
+
+ leftBackSlave.follow(leftFrontMaster);
+ rightBackSlave.follow(rightFrontMaster);
+
+ leftFrontMaster.configOpenloopRamp(0.25, 30);
+ rightFrontMaster.configOpenloopRamp(0.25, 30);
+
+ leftFrontMaster.configVoltageCompSaturation(12, 30);
+ leftBackSlave.configVoltageCompSaturation(12, 30);
+ rightFrontMaster.configVoltageCompSaturation(12, 30);
+ rightBackSlave.configVoltageCompSaturation(12, 30);
+
+ drive = new DifferentialDrive(leftFrontMaster, rightFrontMaster);
+ }
+
+ public void initDefaultCommand() {
+ setDefaultCommand(new TeleopRacingDrive());
+ }
+
+ public void setBrakeMode() {
+ leftFrontMaster.setNeutralMode(NeutralMode.Brake);
+ leftBackSlave.setNeutralMode(NeutralMode.Brake);
+ rightFrontMaster.setNeutralMode(NeutralMode.Brake);
+ rightBackSlave.setNeutralMode(NeutralMode.Brake);
+ }
+
+ public void setCoastMode() {
+ leftFrontMaster.setNeutralMode(NeutralMode.Coast);
+ leftBackSlave.setNeutralMode(NeutralMode.Coast);
+ rightFrontMaster.setNeutralMode(NeutralMode.Coast);
+ rightBackSlave.setNeutralMode(NeutralMode.Coast);
+ }
+
+ public void enableVoltageCompensation() {
+ leftFrontMaster.enableVoltageCompensation(true);
+ leftBackSlave.enableVoltageCompensation(true);
+ rightFrontMaster.enableVoltageCompensation(true);
+ rightBackSlave.enableVoltageCompensation(true);
+ }
+
+ public void disableVoltageCompensation() {
+ leftFrontMaster.enableVoltageCompensation(false);
+ leftBackSlave.enableVoltageCompensation(false);
+ rightFrontMaster.enableVoltageCompensation(false);
+ rightBackSlave.enableVoltageCompensation(false);
+ }
+
+ /**
+ * Controls the drivetrain using two tank-drive joystick inputs
+ *
+ * @param leftInput
+ * - Left joystick input from -1.0 to 1.0
+ * @param rightInput
+ * - Right joystick input from -1.0 to 1.0
+ */
+ public void tankDrive(double leftInput, double rightInput) {
+ drive.tankDrive(leftInput, rightInput);
+ }
+
+ public void arcadeDrive(double throttle, double steer) {
+ drive.arcadeDrive(throttle, steer);
+ }
+
+}
diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java b/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java
new file mode 100644
index 0000000..3f8b442
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java
@@ -0,0 +1,80 @@
+package org.usfirst.frc.team4737.robot.subsystems;
+
+import org.usfirst.frc.team4737.robot.RobotMap;
+import org.usfirst.frc.team4737.robot.commands.elevator.StopElevator;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class Elevator extends Subsystem {
+
+ private WPI_TalonSRX motor;
+
+ private double current;
+ private double lastCurrent;
+ private final double retention = 0.1;
+
+ private boolean hold;
+
+ public Elevator() {
+ motor = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR);
+ motor.setInverted(true);
+ // Prevent motor from stalling too hard
+ motor.configContinuousCurrentLimit(30, 30);
+ motor.enableCurrentLimit(true);
+
+ // Use voltage compensation to keep inputs reliable
+ motor.configVoltageCompSaturation(12, 30);
+ motor.enableVoltageCompensation(true);
+ }
+
+ public void initDefaultCommand() {
+ setDefaultCommand(new StopElevator());
+ }
+
+ @Override
+ public void periodic() {
+ // Measure current and apply a basic noise filter
+ double temp = current;
+ current = motor.getOutputCurrent() * (1 - retention) + lastCurrent * retention;
+ lastCurrent = temp;
+
+ SmartDashboard.putNumber("elevator_current", current);
+// SmartDashboard.putNumber("elevator_voltage", motor.getMotorOutputVoltage());
+
+ }
+
+ public double getMotorCurrent() {
+ return current;
+ }
+
+ /**
+ *
+ * @param speed
+ * ranges from -1.0 to 1.0
+ */
+ public void setSpeed(double speed) {
+ motor.set(ControlMode.PercentOutput, speed + (hold ? RobotMap.ELEVATOR_HOLD_V / motor.getBusVoltage() : 0));
+ }
+
+ public void setHoldPosition(boolean hold) {
+ this.hold = hold;
+ }
+
+ public void setBrakeMode() {
+ motor.setNeutralMode(NeutralMode.Brake);
+ }
+
+ public void setCoastMode() {
+ motor.setNeutralMode(NeutralMode.Coast);
+ }
+
+}
diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/ExampleSubsystem.java b/src/org/usfirst/frc/team4737/robot/subsystems/ExampleSubsystem.java
deleted file mode 100644
index bd46496..0000000
--- a/src/org/usfirst/frc/team4737/robot/subsystems/ExampleSubsystem.java
+++ /dev/null
@@ -1,23 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package org.usfirst.frc.team4737.robot.subsystems;
-
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-/**
- * An example subsystem. You can replace me with your own Subsystem.
- */
-public class ExampleSubsystem extends Subsystem {
- // Put methods for controlling this subsystem
- // here. Call these from Commands.
-
- public void initDefaultCommand() {
- // Set the default command for a subsystem here.
- // setDefaultCommand(new MySpecialCommand());
- }
-}
diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java b/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java
new file mode 100644
index 0000000..22ed700
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java
@@ -0,0 +1,42 @@
+package org.usfirst.frc.team4737.robot.subsystems;
+
+import org.usfirst.frc.team4737.robot.RobotMap;
+import org.usfirst.frc.team4737.robot.commands.intake.StopIntake;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ *
+ */
+public class Intake extends Subsystem {
+
+ private WPI_TalonSRX leftMotor;
+ private WPI_TalonSRX rightMotor;
+
+ public Intake() {
+ leftMotor = new WPI_TalonSRX(RobotMap.INTAKE_LEFT);
+ rightMotor = new WPI_TalonSRX(RobotMap.INTAKE_RIGHT);
+ }
+
+ public void initDefaultCommand() {
+ // Set the default command for a subsystem here.
+ setDefaultCommand(new StopIntake());
+ }
+
+ /**
+ *
+ * @param speed
+ * The speed (between -1.0 and 1.0) to run the intake motors at.
+ */
+ public void setSpeed(double speed) {
+ setLRSpeed(speed, speed);
+ }
+
+ public void setLRSpeed(double left, double right) {
+ leftMotor.set(left);
+ rightMotor.set(right);
+ }
+
+}
diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/IntakeGrip.java b/src/org/usfirst/frc/team4737/robot/subsystems/IntakeGrip.java
new file mode 100644
index 0000000..db9b21a
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/robot/subsystems/IntakeGrip.java
@@ -0,0 +1,46 @@
+package org.usfirst.frc.team4737.robot.subsystems;
+
+import org.usfirst.frc.team4737.robot.RobotMap;
+import org.usfirst.frc.team4737.robot.commands.intakegrip.StopIntakeGrip;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ *
+ */
+public class IntakeGrip extends Subsystem {
+
+ private DoubleSolenoid leftPneumatic;
+ private DoubleSolenoid rightPneumatic;
+
+ public IntakeGrip() {
+ leftPneumatic = new DoubleSolenoid(RobotMap.INTAKE_LEFT_PISTON_MODULE,
+ RobotMap.INTAKE_LEFT_PISTON_FORWARD_CHANNEL, RobotMap.INTAKE_LEFT_PISTON_REVERSE_CHANNEL);
+ rightPneumatic = new DoubleSolenoid(RobotMap.INTAKE_RIGHT_PISTON_MODULE,
+ RobotMap.INTAKE_RIGHT_PISTON_FORWARD_CHANNEL, RobotMap.INTAKE_RIGHT_PISTON_REVERSE_CHANNEL);
+ }
+
+ public void initDefaultCommand() {
+ setDefaultCommand(new StopIntakeGrip());
+ }
+
+ public void closePneumatics() {
+ setPneumatics(Value.kReverse, Value.kReverse);
+ }
+
+ public void openPneumatics() {
+ setPneumatics(Value.kForward, Value.kForward);
+ }
+
+ public void disablePneumatics() {
+ setPneumatics(Value.kOff, Value.kOff);
+ }
+
+ public void setPneumatics(DoubleSolenoid.Value left, DoubleSolenoid.Value right) {
+ leftPneumatic.set(left);
+ rightPneumatic.set(right);
+ }
+
+}
"LS" or
+ * "RS")
+ * @return Returns a thumbstick mapped with the given name
+ */
+ public Thumbstick getThumbstick(String name) {
+ return thumbstickMap.get(name.toLowerCase());
+ }
+
+ /**
+ * Enables retrieving POV DPads by name without knowing which controller is
+ * being used.
+ *
+ * @param name
+ * The name of the DPad (usually just "DPAD")
+ * @return Returns a button mapped with the given name
+ */
+ public DPad getDPad(String name) {
+ return dpadMap.get(name.toLowerCase());
+ }
+
+}
diff --git a/src/org/usfirst/frc/team4737/lib/XboxController.java b/src/org/usfirst/frc/team4737/lib/XboxController.java
new file mode 100644
index 0000000..631ffea
--- /dev/null
+++ b/src/org/usfirst/frc/team4737/lib/XboxController.java
@@ -0,0 +1,53 @@
+package org.usfirst.frc.team4737.lib;
+
+public class XboxController extends Gamepad {
+
+ public final GamepadButton A;
+ public final GamepadButton B;
+ public final GamepadButton X;
+ public final GamepadButton Y;
+ public final GamepadButton LB;
+ public final GamepadButton RB;
+ public final GamepadButton SELECT;
+ public final GamepadButton START;
+ public final GamepadButton L3;
+ public final GamepadButton R3;
+
+ public final Thumbstick LS;
+ public final Thumbstick RS;
+
+ public final GamepadAxis LT;
+ public final GamepadAxis RT;
+
+ public final DPad DPAD;
+
+ public XboxController(int usbPort) {
+ super(usbPort, "XboxController");
+ A = new GamepadButton(this, "A", 1);
+ B = new GamepadButton(this, "B", 2);
+ X = new GamepadButton(this, "X", 3);
+ Y = new GamepadButton(this, "Y", 4);
+ LB = new GamepadButton(this, "LB", 5);
+ RB = new GamepadButton(this, "RB", 6);
+ SELECT = new GamepadButton(this, "SELECT", 7);
+ registerButton(SELECT, "BACK");
+ START = new GamepadButton(this, "START", 8);
+ L3 = new GamepadButton(this, "L3", 9);
+ registerButton(L3, "LS");
+ R3 = new GamepadButton(this, "R3", 10);
+ registerButton(R3, "RS");
+
+ LS = new Thumbstick(this, "LS", 0, 1, false, false);
+ LS.X.setDeadzone(0.1);
+ LS.Y.setDeadzone(0.1);
+ RS = new Thumbstick(this, "RS", 4, 5, false, false);
+ RS.X.setDeadzone(0.1);
+ RS.Y.setDeadzone(0.1);
+
+ LT = new GamepadAxis(this, "LT", 2, false);
+ RT = new GamepadAxis(this, "RT", 3, false);
+
+ DPAD = new DPad(this, "DPAD", 0);
+ }
+
+}
diff --git a/src/org/usfirst/frc/team4737/robot/OI.java b/src/org/usfirst/frc/team4737/robot/OI.java
index 2d10bad..e29c93d 100644
--- a/src/org/usfirst/frc/team4737/robot/OI.java
+++ b/src/org/usfirst/frc/team4737/robot/OI.java
@@ -7,6 +7,16 @@
package org.usfirst.frc.team4737.robot;
+import org.usfirst.frc.team4737.lib.Gamepad;
+import org.usfirst.frc.team4737.lib.XboxController;
+import org.usfirst.frc.team4737.lib.F310Gamepad;
+import org.usfirst.frc.team4737.robot.commands.drivetrain.*;
+import org.usfirst.frc.team4737.robot.commands.elevator.*;
+import org.usfirst.frc.team4737.robot.commands.intake.*;
+import org.usfirst.frc.team4737.robot.commands.intakegrip.*;
+
+import edu.wpi.first.wpilibj.buttons.Trigger;
+
/**
* This class is the glue that binds the controls on the physical operator
* interface to the commands and command groups that allow control of the robot.
@@ -39,4 +49,47 @@ public class OI {
// Start the command when the button is released and let it run the command
// until it is finished as determined by it's isFinished method.
// button.whenReleased(new ExampleCommand());
+
+ public Gamepad driver;
+ public Gamepad operator;
+
+ public OI() {
+ driver = new XboxController(0);
+ operator = new F310Gamepad(1);
+
+ // User override to take control of the intake
+ new Trigger() {
+ public boolean get() {
+ return !Robot.getInstance().isAutonomous() && (operator.getAxis("LT").get() != 0
+ || operator.getAxis("RT").get() != 0 || operator.getAxis("RS_X").get() != 0);
+ }
+ }.whileActive(new ControlIntake());
+
+ // User override to take control of driving
+ new Trigger() {
+ public boolean get() {
+ return !Robot.getInstance().isAutonomous() && (driver.getThumbstick("LS").X.get() != 0
+ || driver.getAxis("LT").get() != 0 || driver.getAxis("RT").get() != 0);
+ }
+ }.whileActive(new TeleopRacingDrive());
+
+ // User override to take control of the elevator
+ new Trigger() {
+ public boolean get() {
+ return !Robot.getInstance().isAutonomous() && (operator.getAxis("LS_Y").get() != 0);
+ }
+ }.whileActive(new ControlElevator());
+
+ // Allow operator to instantly relax elevator in case of motor overheat
+ operator.getButton("Y").whileHeld(new RelaxElevator());
+
+ // Take control of intake grip
+ operator.getButton("A").whileHeld(new ControlIntakeGrip());
+ operator.getButton("B").whileHeld(new ControlIntakeGrip());
+
+ // operator.getDPad("DPAD").LEFT.whileActive(new TwistIntake(1));
+ // operator.getDPad("DPAD").RIGHT.whileActive(new TwistIntake(-1));
+
+ }
+
}
diff --git a/src/org/usfirst/frc/team4737/robot/Robot.java b/src/org/usfirst/frc/team4737/robot/Robot.java
index 3a178a5..c9d3ea8 100644
--- a/src/org/usfirst/frc/team4737/robot/Robot.java
+++ b/src/org/usfirst/frc/team4737/robot/Robot.java
@@ -7,13 +7,15 @@
package org.usfirst.frc.team4737.robot;
+import org.usfirst.frc.team4737.robot.commands.drivetrain.AutoBlindBaseline;
+import org.usfirst.frc.team4737.robot.commands.drivetrain.RelaxDrivetrain;
+import org.usfirst.frc.team4737.robot.subsystems.*;
+
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import org.usfirst.frc.team4737.robot.commands.ExampleCommand;
-import org.usfirst.frc.team4737.robot.subsystems.ExampleSubsystem;
/**
* The VM is configured to automatically run this class, and to call the
@@ -23,33 +25,48 @@
* project.
*/
public class Robot extends TimedRobot {
- public static final ExampleSubsystem kExampleSubsystem
- = new ExampleSubsystem();
- public static OI m_oi;
+
+ private static Robot instance;
+
+ public static Robot getInstance() {
+ return instance;
+ }
+
+ public static final Drivetrain DRIVETRAIN = new Drivetrain();
+ public static final Elevator ELEVATOR = new Elevator();
+ public static final Intake INTAKE = new Intake();
+ public static final IntakeGrip INTAKEGRIP = new IntakeGrip();
+ public static final Climber CLIMBER = new Climber();
+ public static final ControlSystem CONTROLSYSTEM = new ControlSystem();
+
+ public static final OI OI = new OI(); // Must initialize after subsystems
- Command m_autonomousCommand;
- SendableChooser