Merged with master

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index d51dfe76479c73135d26c9d7c0afcedaaddbf3eb..312e0c355a8b584dcda01b230f30bb15d121471a 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -24,8 +24,12 @@ public class Constants {
     public final static int REVERSE_INTAKE_PORT = 4;
     public static final int INCREASE_SHOOTER_SPEED_PORT = 6;
     public static final int DECREASE_SHOOTER_SPEED_PORT = 2;
+
+    public static final int CHANGE_CAMERA_VIEW = 6;
+
     public static final int BRAKE_CANTALONS_PORT = 5;
     public static final int COAST_CANTALONS_PORT = 3;
+
   }
 
   public static class Shooter {
@@ -36,8 +40,11 @@ public class Constants {
 
     public final static int HALL_EFFECT_PORT = 9;
 
+    public final static int TOGGLE_INDEXER = 8;
+
     public static final int MODULE_NUMBER = 10, PISTON_FORWARD = 4,
         PISTON_REVERSE = 5;
+
     public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward;
     public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse;
   }
@@ -50,6 +57,8 @@ public class Constants {
     public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward;
     public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse;
 
+    public final static int TOGGLE_DRIVE_PISTON = 7;
+
     // MOTOR CONTROLLERS
     public static final int FRONT_LEFT = 1;
     public static final int FRONT_RIGHT = 3;

diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java
index a1abe0ad63a7b3e3a1fd22c450c5b547402d0a71..880894f810c81705e164e0e1edeb922b0b4cb493 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/OI.java
+++ b/src/org/usfirst/frc/team3501/robot/OI.java
@@ -1,7 +1,5 @@
 package org.usfirst.frc.team3501.robot;
 
-import org.usfirst.frc.team3501.robot.commands.driving.BrakeCANTalons;
-import org.usfirst.frc.team3501.robot.commands.driving.CoastCANTalons;
 import org.usfirst.frc.team3501.robot.commands.driving.ToggleGear;
 import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous;
 import org.usfirst.frc.team3501.robot.commands.intake.RunIntakeContinuous;
@@ -10,12 +8,14 @@ import org.usfirst.frc.team3501.robot.commands.shooter.IncreaseShootingSpeed;
 import org.usfirst.frc.team3501.robot.commands.shooter.ReverseIndexWheelContinuous;
 import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheelContinuous;
 import org.usfirst.frc.team3501.robot.commands.shooter.RunIndexWheelContinuous;
+import org.usfirst.frc.team3501.robot.commands.shooter.ToggleIndexerPiston;
+import org.usfirst.frc.team3501.robot.utils.ChangeCameraView;
 
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.Button;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
 
-public class OI {
+public class OI /* implements KeyListener */ {
   private static OI oi;
   public static Joystick leftJoystick;
   public static Joystick rightJoystick;
@@ -32,10 +32,16 @@ public class OI {
   public static Button increaseShooterSpeed;
   public static Button decreaseShooterSpeed;
 
+  private static Button changeCam;
+
+  private static Button togglePiston;
+  private static Button toggleDriveTrainPiston;
+
   public static Button brakeCANTalons;
   public static Button coastCANTalons;
 
   public OI() {
+
     leftJoystick = new Joystick(Constants.OI.LEFT_STICK_PORT);
     rightJoystick = new Joystick(Constants.OI.RIGHT_STICK_PORT);
 
@@ -70,6 +76,18 @@ public class OI {
         Constants.OI.DECREASE_SHOOTER_SPEED_PORT);
     decreaseShooterSpeed.whenPressed(new DecreaseShootingSpeed());
 
+    changeCam = new JoystickButton(rightJoystick,
+        Constants.OI.CHANGE_CAMERA_VIEW);
+    changeCam.toggleWhenPressed(new ChangeCameraView());
+
+    togglePiston = new JoystickButton(rightJoystick,
+        Constants.Shooter.TOGGLE_INDEXER);
+    togglePiston.whenPressed(new ToggleIndexerPiston());
+
+    toggleDriveTrainPiston = new JoystickButton(rightJoystick,
+        Constants.DriveTrain.TOGGLE_DRIVE_PISTON);
+    toggleDriveTrainPiston.whenPressed(new ToggleGear());
+
     brakeCANTalons = new JoystickButton(rightJoystick,
         Constants.OI.BRAKE_CANTALONS_PORT);
     brakeCANTalons.whenPressed(new BrakeCANTalons());
@@ -84,4 +102,17 @@ public class OI {
       oi = new OI();
     return oi;
   }
+
+  /*
+   * @Override public void keyPressed(KeyEvent e) { if (e.getKeyCode() ==
+   * KeyEvent.VK_Z) { // Robot.swapCameraFeed();
+   * Robot.getShooter().runIndexWheel(); } }
+   *
+   * @Override public void keyReleased(KeyEvent e) { if (e.getKeyCode() ==
+   * KeyEvent.VK_Z) { // Robot.swapCameraFeed();
+   * Robot.getShooter().stopIndexWheel(); } }
+   *
+   * @Override public void keyTyped(KeyEvent e) { }
+   */
+
 }

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index a8c48643019bffbf7786a505e4264316be539b42..5e9bd3cbdb496727cd9722efc1630c5ad13c2228 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -16,6 +16,7 @@ public class Robot extends IterativeRobot {
   private static Shooter shooter;
   private static OI oi;
   private static Intake intake;
+  private static CameraServer server;
 
   @Override
   public void robotInit() {
@@ -24,7 +25,7 @@ public class Robot extends IterativeRobot {
     shooter = Shooter.getShooter();
     intake = Intake.getIntake();
 
-    CameraServer server = CameraServer.getInstance();
+    server = CameraServer.getInstance();
     UsbCamera climberCam = server.startAutomaticCapture("climbercam", 0);
     UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 1);
 
@@ -32,6 +33,7 @@ public class Robot extends IterativeRobot {
   }
 
   public static DriveTrain getDriveTrain() {
+
     return DriveTrain.getDriveTrain();
   }
 
@@ -47,6 +49,11 @@ public class Robot extends IterativeRobot {
     return Intake.getIntake();
   }
 
+  public static void swapCameraFeed() {
+    UsbCamera climberCam = server.startAutomaticCapture("climbercam", 1);
+    UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 0);
+  }
+
   // If the gear values do not match in the left and right piston, then they are
   // both set to high gear
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java
index ce7489096ec57d9443c0e17f1cc5da1ec71ad9f3..ca802e705291cff2ebd02ae5658a25f9998dd840 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java
@@ -29,11 +29,13 @@ public class ToggleGear extends Command {
   protected void execute() {
     Value leftGearPistonValue = driveTrain.getLeftGearPistonValue();
     Value rightGearPistonValue = driveTrain.getRightGearPistonValue();
+
     if (leftGearPistonValue == Constants.DriveTrain.LOW_GEAR) {
       driveTrain.setHighGear();
     } else {
       driveTrain.setLowGear();
     }
+
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java
index 95940130f11aac8c50ebc54dc29af5d2d13dcf76..ccc0e4374d6223121eefff87bc28ced1bee495c2 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java
@@ -3,7 +3,7 @@ package org.usfirst.frc.team3501.robot.commands.shooter;
 import org.usfirst.frc.team3501.robot.Constants;
 import org.usfirst.frc.team3501.robot.Robot;
 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
-
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Command;
 
 /**
@@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj.command.Command;
  */
 public class RunIndexWheelContinuous extends Command {
   private Shooter shooter = Robot.getShooter();
+  private Timer t = new Timer();
 
   /**
    * See JavaDoc comment in class for details
@@ -30,23 +31,23 @@ public class RunIndexWheelContinuous extends Command {
 
   @Override
   protected void initialize() {
+    t.start();
   }
 
   @Override
   protected void execute() {
-    if (timeSinceInitialized() % 0.5 <= 0.02) {
+    if (t.get() >= 1) {
       if (Shooter.getShooter().getPistonValue() == Constants.Shooter.LOW_GEAR) {
         Shooter.getShooter().setHighGear();
       } else {
         Shooter.getShooter().setLowGear();
       }
+      t.reset();
     }
 
-    double shooterSpeed = shooter.getShooterRPM();
-    double targetShooterSpeed = shooter.getTargetShootingSpeed();
-    double threshold = shooter.getRPMThreshold();
-    if (Math.abs(shooterSpeed - targetShooterSpeed) <= threshold)
+    if (shooter.isShooterRPMWithinRangeOfTargetSpeed(25))
       shooter.runIndexWheel();
+
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/ToggleIndexerPiston.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/ToggleIndexerPiston.java
new file mode 100644 (file)
index 0000000..a653293
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/ToggleIndexerPiston.java
@@ -0,0 +1,55 @@
+package org.usfirst.frc.team3501.robot.commands.shooter;
+
+import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ToggleIndexerPiston extends Command {
+  private Shooter shooter = Robot.getShooter();
+
+  /**
+   * See JavaDoc comment in class for details
+   *
+   * @param motorVal
+   *          value range from -1 to 1
+   */
+  public ToggleIndexerPiston() {
+    requires(shooter);
+  }
+
+  // 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() {
+    if (shooter.getPistonValue() == Constants.Shooter.LOW_GEAR) {
+      shooter.setHighGear();
+    } else {
+      shooter.setLowGear();
+    }
+  }
+
+  // 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() {
+    end();
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return false;
+
+  }
+
+}

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
index faca5ddac1c1e7b9caabab21298a932c8192e5d0..820bc6a85c25f2429f5cf9c90153d48475914190 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
@@ -23,6 +23,7 @@ public class Shooter extends Subsystem {
   private static final double DEFAULT_INDEXING_MOTOR_VALUE = -0.75;
   private static final double DEFAULT_SHOOTING_SPEED = 3100; // rpm
   private static final double SHOOTING_SPEED_INCREMENT = 50;
+  private static final int ACCEPTABLE_SHOOTING_DEVIATION = 300;
 
   private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED;
   private double currentShooterMotorValue = 0;
@@ -163,4 +164,17 @@ public class Shooter extends Subsystem {
   private void changeGear(DoubleSolenoid.Value gear) {
     piston.set(gear);
   }
+
+  public boolean isShooterRPMAtTargetSpeed() {
+    return isShooterRPMWithinRangeOfTargetSpeed(ACCEPTABLE_SHOOTING_DEVIATION);
+  }
+
+  public boolean isShooterRPMWithinRangeOfTargetSpeed(int acceptableRPMError) {
+    double shooterSpeed = getShooterRPM();
+    if (shooterSpeed > DEFAULT_SHOOTING_SPEED - acceptableRPMError
+        && shooterSpeed < DEFAULT_SHOOTING_SPEED + acceptableRPMError) {
+      return true;
+    }
+    return false;
+  }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/utils/ChangeCameraView.java b/src/org/usfirst/frc/team3501/robot/utils/ChangeCameraView.java
new file mode 100644 (file)
index 0000000..9615c4e
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/utils/ChangeCameraView.java
@@ -0,0 +1,36 @@
+package org.usfirst.frc.team3501.robot.utils;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ChangeCameraView extends Command {
+
+  public ChangeCameraView() {
+  }
+
+  @Override
+  protected void initialize() {
+  }
+
+  @Override
+  protected void execute() {
+    Robot.swapCameraFeed();
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  protected void end() {
+  }
+
+  @Override
+  protected void interrupted() {
+  }
+}