Shooter

src/com/nutrons/nu17/Robot.java

@@ -1,5 +1,6 @@
 package com.nutrons.nu17;
 
+import com.nutrons.nu17.OperatorInterface;
 import com.nutrons.nu17.subsystems.Climber;
 import com.nutrons.nu17.subsystems.Drivetrain;
 import com.nutrons.nu17.subsystems.GearPlacer;
@@ -8,23 +9,31 @@
 import com.nutrons.nu17.subsystems.TwinShooter;
 
 import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Preferences;
+import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 
 public class Robot extends IterativeRobot {
 
   public static final TwinShooter TWIN_SHOOTER = new TwinShooter();
-  public static final Shooter SHOOTER = new Shooter();
+  public static Shooter SHOOTER = new Shooter();
   public static final GroundIntake GROUND_INTAKE = new GroundIntake();
   public static final GearPlacer GP = new GearPlacer();
+  public static final Drivetrain DRIVE_TRAIN = new Drivetrain();
   public static final Climber CLIMBER = new Climber();
-  public static final Drivetrain DRIVETRAIN = new Drivetrain();
+  public static final OperatorInterface OI = new OperatorInterface();
+  Preferences prefs;
 
-  public static final OperatorInterface OperatorInterface = new OperatorInterface();
+  Command autonomousCommand;
+  SendableChooser<Command> chooser = new SendableChooser<>();
 
   @Override
   public void robotInit() {
-    // empty
+    SmartDashboard.putData("Auto mode", chooser);
   }
 
   @Override
@@ -39,7 +48,11 @@ public void disabledPeriodic() {
 
   @Override
   public void autonomousInit() {
-    // empty
+    autonomousCommand = chooser.getSelected();
+
+    if (autonomousCommand != null) {
+      autonomousCommand.start();
+    }
   }
 
   @Override
@@ -49,16 +62,19 @@ public void autonomousPeriodic() {
 
   @Override
   public void teleopInit() {
-    // empty
+    prefs = Preferences.getInstance();
+    Shooter.SHOOTER_SPEED = prefs.getDouble("shooter_speed", 1.0);
   }
 
   @Override
   public void teleopPeriodic() {
     Scheduler.getInstance().run();
+    SmartDashboard.putNumber("current_shooter_power", Robot.SHOOTER.getRpm());
   }
 
   @Override
   public void testPeriodic() {
     LiveWindow.run();
   }
+
 }

src/com/nutrons/nu17/RobotMap.java

@@ -44,10 +44,10 @@ public class RobotMap {
   public static final int FLY_ENCODER_BACK_RIGHT = 6;
   public static final int FLY_ENCODER_FRONT_LEFT = 7;
   public static final int FLY_ENCODER_BACK_LEFT = 8;
-  public static final int TWIN_LEFT_ENCODER_1 = 9;
-  public static final int TWIN_LEFT_ENCODER_2 = 10;
-  public static final int TWIN_RIGHT_ENCODER_1 = 11;
-  public static final int TWIN_RIGHT_ENCODER_2 = 12;
+  public static final int TWIN_ENCODER_1 = 9;
+  public static final int TWIN_ENCODER_2 = 10;
+  public static final int TWIN_ENCODER_3 = 11;
+  public static final int TWIN_ENCODER_4 = 12;
   public static final int SHOOT_ENCODER_1 = 0;
   public static final int SHOOT_ENCODER_2 = 1;
 
@@ -65,4 +65,5 @@ public class RobotMap {
 
   // TODO tune this constant for deadband
   public static final double JOYSTICK_DEADBAND = 0.05;
+  public static final int SHOOTER = 0;
 }

src/com/nutrons/nu17/commands/ShootShooterCmd.java

@@ -1,6 +1,8 @@
 package com.nutrons.nu17.commands;
 
+import com.ctre.CANTalon.TalonControlMode;
 import com.nutrons.nu17.Robot;
+import com.nutrons.nu17.subsystems.Shooter;
 
 import edu.wpi.first.wpilibj.command.Command;
 
@@ -14,15 +16,17 @@ public ShootShooterCmd() {
    * Resets encoder and starts running motor at a consistent speed.
    */
   protected void initialize() {
-    Robot.SHOOTER.resetEncoder();
-    Robot.SHOOTER.runShooter(1.0);
+    Robot.SHOOTER.shooter.changeControlMode(TalonControlMode.Speed);
+    Robot.SHOOTER.shooter.setSetpoint(Shooter.SHOOTER_SPEED);
+    Robot.SHOOTER.shooter.enable();
   }
 
   /**
    * Keeps running motor to a consistent speed.
    */
   protected void execute() {
-    Robot.SHOOTER.runShooter(Robot.SHOOTER.speedPid.get());
+    Robot.SHOOTER.setRpm(2000);
+
   }
 
   // Don't want the shooter to stop
@@ -37,4 +41,5 @@ protected void end() {
   protected void interrupted() {
     end();
   }
+
 }

src/com/nutrons/nu17/commands/StopShooterCmd.java

@@ -0,0 +1,31 @@
+package com.nutrons.nu17.commands;
+
+import com.nutrons.nu17.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+
+public class StopShooterCmd extends Command {
+
+  public StopShooterCmd() {
+    requires(Robot.SHOOTER);
+  }
+
+  protected void initialize() {
+    Robot.SHOOTER.shooter.disable();
+  }
+
+
+  protected void execute() {}
+
+
+  protected boolean isFinished() {
+    return true;
+  }
+
+
+  protected void end() {}
+
+
+  protected void interrupted() {}
+}

src/com/nutrons/nu17/subsystems/Shooter.java

@@ -1,60 +1,95 @@
 package com.nutrons.nu17.subsystems;
 
+import com.ctre.CANTalon;
+import com.ctre.CANTalon.FeedbackDevice;
+import com.ctre.CANTalon.TalonControlMode;
 import com.nutrons.nu17.Robot;
 import com.nutrons.nu17.RobotMap;
+import com.nutrons.nu17.commands.ShootShooterCmd;
 
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.PIDController;
 import edu.wpi.first.wpilibj.PIDSourceType;
 import edu.wpi.first.wpilibj.Talon;
 import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import lib.EncoderWrapper;
 import lib.HoldPid;
 
+// Travis liked the steak
 public class Shooter extends Subsystem {
 
-  private final Talon shooter = new Talon(RobotMap.SHOOTER_MOTOR);
-  private final Encoder shooterEncoder =
-      new Encoder(RobotMap.SHOOT_ENCODER_1, RobotMap.SHOOT_ENCODER_2);
-  private EncoderWrapper encWrap = new EncoderWrapper(PIDSourceType.kDisplacement, shooterEncoder,
-      RobotMap.SHOOT_ENCODER_1, RobotMap.SHOOT_ENCODER_2);
+
+  public static double SHOOTER_SPEED = 10.0;
 
-  private HoldPid shootHold = new HoldPid();
+  public CANTalon shooter = new CANTalon(RobotMap.SHOOTER_MOTOR);
+
+  // TODO: tune these constants
+  public static double P_SHOOT = 0.025;
+  public static double I_SHOOT = 0.0;
+  public static double D_SHOOT = 0.01;
+  public static double F_SHOOT = 0.025;
 
+  /**
+   * Constructs an instance of shooter.
+   */
   public Shooter() {
-    // empty
-  }
-
-  // TODO: tune these constants
-  private static final double P_SHOOT = 0.025;
-  private static final double I_SHOOT = 0.0;
-  private static final double D_SHOOT = 0.01;
+    this.shooter.configNominalOutputVoltage(+0.0f, -0.0f);
+    this.shooter.configPeakOutputVoltage(+12.0f, 0.0f);
+    this.shooter.configEncoderCodesPerRev((int) (256 / 0.14));
+    this.shooter.configEncoderCodesPerRev((int) (256 / 0.14));
+    this.shooter.setFeedbackDevice(FeedbackDevice.QuadEncoder);
+    this.shooter.setP(P_SHOOT);
+    this.shooter.setI(I_SHOOT);
+    this.shooter.setD(D_SHOOT);
+    this.shooter.setF(F_SHOOT);
 
-  public final PIDController speedPid =
-      new PIDController(P_SHOOT, I_SHOOT, D_SHOOT, encWrap, shootHold);
+  }
+  /**
+   * Sets the default command to the ShootShooterCmd.
+   */
+  public void initDefaultCommand() {
+    setDefaultCommand(new ShootShooterCmd());
 
-  public void initDefaultCommand() {}
+  }
 
   /**
-   * Runs shooter at given param power.
+   * Changes to manual and sets the speed.
    * 
-   * @param power Speed to run the shooting motor.
+   * @param speed Speed of shooter.
    */
-  public void runShooter(double power) {
-    this.shooter.set(power);
+  public void setOpenLoop(double speed) {
+    this.shooter.changeControlMode(TalonControlMode.PercentVbus);
+    this.shooter.set(speed);
+  }
+
+  /**
+   * Changes to closed loop. Sets the RPM from the shooter.
+   */
+  public void setRpm(double rpm) {
+    this.shooter.changeControlMode(TalonControlMode.Speed);
+    this.shooter.set(rpm);
   }
 
   /**
    * Cuts the power to the shooter, setting it to 0.0.
    */
   public void stopShooter() {
-    runShooter(0.0);
+    setOpenLoop(0.0);
+  }
+
+  /**
+   * Gets RPM from shooter.
+   */
+  public double getRpm() {
+    return this.shooter.getSpeed();
   }
 
   /**
    * Resets Encoder.
    */
   public void resetEncoder() {
-    shooterEncoder.reset();
+    this.shooter.setEncPosition(0);
   }
+
 }

src/com/nutrons/nu17/subsystems/TwinShooter.java

@@ -11,21 +11,20 @@
 
 public class TwinShooter extends Subsystem {
 
+
   private final Shooter twinA = new Shooter();
   private final Shooter twinB = new Shooter();
-  private final Encoder encoderOne =
-      new Encoder(RobotMap.TWIN_LEFT_ENCODER_1, RobotMap.TWIN_LEFT_ENCODER_2);
-  private final Encoder encoderTwo =
-      new Encoder(RobotMap.TWIN_RIGHT_ENCODER_1, RobotMap.TWIN_RIGHT_ENCODER_2);
+  private final Encoder encoder1 = new Encoder(RobotMap.TWIN_ENCODER_1, RobotMap.TWIN_ENCODER_2);
+  private final Encoder encoder2 = new Encoder(RobotMap.TWIN_ENCODER_3, RobotMap.TWIN_ENCODER_4);
 
   // PID Wrappers and Holder Objects
-  private EncoderWrapper encWrap1 = new EncoderWrapper(PIDSourceType.kDisplacement, encoderOne,
-      RobotMap.TWIN_LEFT_ENCODER_1, RobotMap.TWIN_LEFT_ENCODER_2);
+  private EncoderWrapper encWrap1 = new EncoderWrapper(PIDSourceType.kDisplacement, encoder1,
+      RobotMap.TWIN_ENCODER_1, RobotMap.TWIN_ENCODER_2);
 
   private HoldPid twinHold1 = new HoldPid();
 
-  private EncoderWrapper encWrap2 = new EncoderWrapper(PIDSourceType.kDisplacement, encoderTwo,
-      RobotMap.TWIN_RIGHT_ENCODER_1, RobotMap.TWIN_RIGHT_ENCODER_2);
+  private EncoderWrapper encWrap2 = new EncoderWrapper(PIDSourceType.kDisplacement, encoder2,
+      RobotMap.TWIN_ENCODER_3, RobotMap.TWIN_ENCODER_4);
 
   private HoldPid twinHold2 = new HoldPid();
 
@@ -43,7 +42,8 @@ public TwinShooter() {
   public PIDController speedPidB =
       new PIDController(P_SHOOT, I_SHOOT, D_SHOOT, encWrap2, twinHold2);
 
-
+  private static double holdShootA;
+  private static double holdShootB;
 
   public void initDefaultCommand() {
     // empty
@@ -55,7 +55,7 @@ public void initDefaultCommand() {
    * @param power Speed to run the first shooting motor at.
    */
   public void runTwinA(double power) {
-    twinA.runShooter(power);
+    twinA.setRpm(power);
   }
 
   /**
@@ -64,7 +64,7 @@ public void runTwinA(double power) {
    * @param power Speed to run the second shooting motor at.
    */
   public void runTwinB(double power) {
-    twinB.runShooter(power);
+    twinB.setRpm(power);
   }
 
   /**
@@ -81,14 +81,14 @@ public void twinRun(double power) {
    * Stops the first shooting motor.
    */
   public void stopTwinA() {
-    twinA.runShooter(0.0);
+    twinA.setRpm(0.0);
   }
 
   /**
    * Stops the second shooting motor.
    */
   public void stopTwinB() {
-    twinB.runShooter(0.0);
+    twinB.setRpm(0.0);
   }
 
   /**