pre week 0

Author: robertdeml

src/main/java/org/mayheminc/robot2020/Constants.java

@@ -49,6 +49,8 @@ public final class Solenoid {
         public static final int SPARE_1 = 1;
         public static final int SPARE_2 = 2;
         public static final int CLIMBER_PISTONS = 3;
+
+        public static final int CAMERA_LIGHTS = 7;
     }
 
     public final class PDP {

src/main/java/org/mayheminc/robot2020/RobotContainer.java

@@ -11,6 +11,7 @@
 
 import edu.wpi.first.wpilibj.Compressor;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Solenoid;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.Button;
 
@@ -41,6 +42,8 @@ public class RobotContainer {
     public static PidTuner pidtuner;
     public static final Chimney chimney = new Chimney();
 
+    public static final Solenoid cameraLights = new Solenoid(Constants.Solenoid.CAMERA_LIGHTS);
+
     // Operator Inputs
     public static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick();
     public static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad();
@@ -59,7 +62,9 @@ public RobotContainer() {
         pidtuner = new PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SIX,
                 RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SEVEN,
                 RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_ELEVEN,
-                RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, shooter);
+                RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, intake);
+
+        cameraLights.set(true);
     }
 
     public static void init() {
@@ -70,6 +75,7 @@ private void configureDefaultCommands() {
         drive.setDefaultCommand(new DriveDefault());
         // intake.setDefaultCommand(new IntakeExtenderVBus());
         magazine.setDefaultCommand(new MagazineDefault());
+        compressor.setDefaultCommand(new AirCompressorDefault());
     }
 
     private void configureAutonomousPrograms() {
@@ -126,11 +132,25 @@ private void configureDriverPadButtons() {
         // ShooterAdjustWheelVBus(-0.1));
         // DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheelVBus(0.0));
 
+        // Debug Turret
+        // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new
+        // ShooterSetTurretAbs(-5500));// about -30 degrees
+        // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new
+        // ShooterSetTurretAbs(+5500));// about +30 degrees
+        DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2));// about -30 degrees
+        DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new ShooterSetTurretVBus(+0.2));// about +30 degrees
+
+        // Debug Hood
+        // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ShooterSetHoodRel(+1000));
+        // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHoodRel(-1000));
+        // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whileHeld(new ShooterSetHoodVBus(+1.0));
+        // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0));
+
         // Debug shooter pid velocity
         DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheel(100.0));
         DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheel(-100.0));
         DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheel(0.0));
-        DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(4600));
+        DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(2700));
 
         DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(0.5));
 
@@ -144,28 +164,37 @@ private void configureOperatorStickButtons() {
     }
 
     private void configureOperatorPadButtons() {
-        // OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new IntakeZero());
-        // new ShooterAdjustWheel(-100));
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new MagazineSetTurntable(0.2));
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
-        // new ShooterSetWheel(0));
-        // OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new
-        // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
-        // new ShooterAdjustWheel(+100));
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new MagazineSetTurntable(1.0));
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP));
+
         // new ShooterSetWheel(1000));
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySetChimney(1.0));
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0));
 
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_SEVEN.whileHeld(new TurretAimToTarget());
+
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_NINE.whenPressed(new ClimberSetPistons(true));
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_TEN.whenPressed(new ClimberSetPistons(false));
+
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0));
-        OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP));
-        OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
-        OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whenPressed(new ShooterSetHood(-0.2));
-        OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whenPressed(new ShooterSetHood(-0.2));
+        // OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new
+        // IntakeSetPosition(RobotContainer.intake.PIVOT_UP));
+        // OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new
+        // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
+        OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2));
+        OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new ShooterSetTurretVBus(+0.2));
+        OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new ShooterSetHoodVBus(+1.0));
+        OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0));
 
         OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(0.7));
         OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-0.7));
-        OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new ClimberSetPistons(true));
-        OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whenPressed(new ClimberSetPistons(false));
+
+        // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new
+        // MagazineSetTurntable());
+        // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whenPressed(new
+        // ClimberSetPistons(false));
     }
 
     /**

src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java

@@ -20,8 +20,8 @@ public DriveStraight() {
 
     addCommands(new DriveZeroGyro());
     addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 100, 0));
-    addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 100, 10));
-    addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 20));
+    addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 100, 0));
+    addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 0));
 
     // addCommands(new ParallelCommandGroup(new IntakeSetPosition(true), new
     // MagazineSetTurntable(0.0)));

src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.mayheminc.robot2020.autonomousroutines;
+
+import org.mayheminc.robot2020.commands.ShooterSetWheel;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class ShootAndDriveForward extends SequentialCommandGroup {
+  /**
+   * Creates a new ShootAndDriveForward.
+   */
+  public ShootAndDriveForward() {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    super();
+
+    addCommands(new ShooterSetWheel(5500));
+    // addCommands(new Wait(2.0));
+  }
+}

src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java

@@ -17,6 +17,7 @@ public class AirCompressorDefault extends CommandBase {
    */
   public AirCompressorDefault() {
     // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.compressor);
   }
 
   // Called when the command is initially scheduled.

src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java

@@ -28,7 +28,7 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    RobotContainer.magazine.setTurntableSpeed(RobotContainer.OPERATOR_PAD.getLeftYAxis());
+    RobotContainer.magazine.setTurntableSpeed(-RobotContainer.OPERATOR_PAD.getLeftYAxis());
 
   }
 

src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java

@@ -29,10 +29,11 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    boolean wheelsGood = Math.abs(Targeting.convertRangeToWheelSpeed(RobotContainer.targeting.getRangeToTarget())
-        - RobotContainer.shooter.getShooterWheelSpeed()) < 100;
+    // boolean wheelsGood =
+    // Math.abs(Targeting.convertRangeToWheelSpeed(RobotContainer.targeting.getRangeToTarget())
+    // - RobotContainer.shooter.getShooterWheelSpeed()) < 100;
 
-    RobotContainer.shooter.setFeederSpeed((wheelsGood) ? 0.5 : 0.0);
+    // RobotContainer.shooter.setFeederSpeed((wheelsGood) ? 0.5 : 0.0);
   }
 
   // Called once the command ends or is interrupted.

src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodAbs.java

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.mayheminc.robot2020.commands;
+
+import org.mayheminc.robot2020.RobotContainer;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class ShooterSetHoodAbs extends InstantCommand {
+  double m_set;
+
+  /**
+   * Creates a new ShooterSetHoodAbs.
+   */
+  public ShooterSetHoodAbs(double set) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.shooter);
+
+    m_set = set;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    RobotContainer.shooter.setHoodPosition(m_set);
+  }
+
+}

src/main/java/org/mayheminc/robot2020/commands/ShooterSetHood.java renamed to src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodRel.java

@@ -11,13 +11,13 @@
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
-public class ShooterSetHood extends InstantCommand {
+public class ShooterSetHoodRel extends InstantCommand {
   double m_adjust;
 
   /**
    * Creates a new ShooterSetHood.
    */
-  public ShooterSetHood(double adjust) {
+  public ShooterSetHoodRel(double adjust) {
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(RobotContainer.shooter);
 

src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodVBus.java

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.mayheminc.robot2020.commands;
+
+import org.mayheminc.robot2020.RobotContainer;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class ShooterSetHoodVBus extends CommandBase {
+  double m_power;
+
+  /**
+   * Creates a new ShooterSetHoodVBus.
+   */
+  public ShooterSetHoodVBus(double power) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.shooter);
+    m_power = power;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    RobotContainer.shooter.setHoodVBus(m_power);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.shooter.setHoodVBus(0.0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}