save

src/main/java/frc/robot/Constants.java

@@ -33,12 +33,26 @@ public static class OperatorConstants {
     public final int DeviceNumberIntakeDroit = 107;
     public final int DeviceNumberIntakeGauche = 108;
     public final int DeviceNumberIntakePivot = 109;
+
+    public final int DeviceNumberAmpShooter = 110;
   }
 
   public static class PCM {
     public final int PISTON_FORWARD = 0;
     public final int PISTON_REVERSE = 1;
-    public final int PISTON_CLIMBER_FORWARD = 2;
-    public final int PISTON_CLIMBER_REVERSE = 3;
+
+    public final int PISTON_CLIMBER_FORWARD_1 = 2;
+    public final int PISTON_CLIMBER_REVERSE_1 = 3;
+
+    public final int PISTON_CLIMBER_FORWARD_2 = 4;
+    public final int PISTON_CLIMBER_REVERSE_2 = 5;
+
+    public final int PISTON_AMP_SHOOTER_FORWARD = 6;
+    public final int PISTON_AMP_SHOOTER_REVERSE = 7;
+  }
+
+  public static class DIGITAL {
+    public final int INTAKE_LIMIT_SWITCH_OUT = 6;
+    public final int INTAKE_LIMIT_SWITCH_IN = 7;
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -16,9 +16,11 @@
 import frc.robot.commands.MoveIntakeWheelCommand;
 import frc.robot.commands.OctocanumDrivetrainCommand;
 import frc.robot.commands.ResetGryoCommand;
+import frc.robot.commands.SetAmpShooterArmPositionCommand;
 import frc.robot.commands.SetShooterAngleCommand;
 import frc.robot.commands.SetShooterSpeedCommand;
 import frc.robot.commands.ShootNoteCommand;
+import frc.robot.subsystems.AmpShooter;
 import frc.robot.subsystems.AngleShooter;
 import frc.robot.subsystems.ClimberInAnBox;
 import frc.robot.subsystems.Drivetrain;
@@ -41,6 +43,7 @@ public class RobotContainer {
   private Intake intake;
   private AngleShooter angleShooter;
   private Limelight limelight;
+  private AmpShooter ampShooter;
   private CommandXboxController xboxController = new CommandXboxController(0);
   private CommandXboxController xboxController1 = new CommandXboxController(1);
 
@@ -53,6 +56,7 @@ public RobotContainer() {
     shooter = new Shooter();
     limelight = new Limelight();
     angleShooter = new AngleShooter();
+    ampShooter = new AmpShooter();
     xboxController = new CommandXboxController(0);
     xboxController1 = new CommandXboxController(1);
     drivetrain.setDefaultCommand(new OctocanumDrivetrainCommand(xboxController, drivetrain));
@@ -74,7 +78,6 @@ private void configureBindings() {
     xboxController.a().onTrue(new ActivateDrivetrainCommand(drivetrain));
     xboxController.b().onTrue(new ActivateMecanumCommand(drivetrain));
     xboxController.x().onTrue(new ResetGryoCommand(drivetrain));
-    xboxController.y().whileFalse(new SetShooterAngleCommand(angleShooter, limelight));
     xboxController1
         .a()
         .whileTrue(
@@ -89,6 +92,8 @@ private void configureBindings() {
                 new MoveIntakeCommand(intake, true), new MoveIntakeWheelCommand(intake, 1)))
         .onFalse(new MoveIntakeCommand(intake, false));
     xboxController1.leftBumper().whileTrue(new ShootNoteCommand(shooter, intake));
+    xboxController1.povUp().onTrue(new SetAmpShooterArmPositionCommand(ampShooter, true));
+    xboxController1.povDown().onTrue(new SetAmpShooterArmPositionCommand(ampShooter, false));
   }
 
   /**

src/main/java/frc/robot/commands/ClimberInABoxCommand.java

@@ -28,13 +28,17 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (xboxController.getRightTriggerAxis() != 0) {
+    if ((xboxController.getRightTriggerAxis() != 0 || xboxController.getLeftTriggerAxis() != 0)
+        && xboxController.x().getAsBoolean()) {
       ratchetActivated = true;
       climberInAnBox.activateRatchet();
-      climberInAnBox.climb(xboxController.getRightTriggerAxis());
-    }
-    if (xboxController.getLeftTriggerAxis() != 0 && !ratchetActivated) {
-      climberInAnBox.climb(-xboxController.getLeftTriggerAxis());
+      climberInAnBox.climb(
+          xboxController.getRightTriggerAxis(), xboxController.getLeftTriggerAxis());
+
+    } else if (xboxController.y().getAsBoolean() && !ratchetActivated) {
+      climberInAnBox.climb(1, 1);
+    } else {
+      climberInAnBox.climb(0, 0);
     }
   }
 

src/main/java/frc/robot/commands/MoveIntakeCommand.java

@@ -15,6 +15,7 @@ public class MoveIntakeCommand extends Command {
   public MoveIntakeCommand(Intake intake, boolean intakeOut) {
     this.intake = intake;
     this.intakeOut = intakeOut;
+
     addRequirements(intake);
     // Use addRequirements() here to declare subsystem dependencies.
   }
@@ -43,9 +44,9 @@ public void end(boolean interrupted) {
   @Override
   public boolean isFinished() {
     if (intakeOut) {
-      return true; // TODO change value to limit switch
+      return intake.getIntakeLimitOut();
     } else {
-      return true; // TODO change value to limit switch
+      return intake.getIntakeLimitIn();
     }
   }
 }

src/main/java/frc/robot/commands/OctocanumDrivetrainCommand.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.subsystems.Drivetrain;
+import frc.util.Range;
 
 public class OctocanumDrivetrainCommand extends Command {
   private CommandXboxController xboxController;
@@ -30,12 +31,12 @@ public void initialize() {}
   @Override
   public void execute() {
     drivetrain.drive(
-        xboxController.getRightX(),
-        xboxController.getRightY(),
-        xboxController.getLeftX(),
-        xboxController.getLeftY(),
-        xboxController.getRightTriggerAxis(),
-        xboxController.getLeftTriggerAxis());
+        Range.threshold(0.1, xboxController.getRightX()),
+        Range.threshold(0.1, xboxController.getRightY()),
+        Range.threshold(0.1, xboxController.getLeftX()),
+        Range.threshold(0.1, xboxController.getLeftY()),
+        Range.threshold(0.1, xboxController.getRightTriggerAxis()),
+        Range.threshold(0.1, xboxController.getLeftTriggerAxis()));
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/SetAmpShooterArmPositionCommand.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.AmpShooter;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class SetAmpShooterArmPositionCommand extends InstantCommand {
+  private AmpShooter ampShooter;
+  private boolean position;
+
+  public SetAmpShooterArmPositionCommand(AmpShooter ampShooter, boolean position) {
+    this.ampShooter = ampShooter;
+    this.position = position;
+    addRequirements(ampShooter);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    ampShooter.setPosition(position);
+  }
+}

src/main/java/frc/robot/commands/SetAmpShooterSpeedCommand.java

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.AmpShooter;
+
+public class SetAmpShooterSpeedCommand extends Command {
+  private XboxController xboxController;
+  private AmpShooter ampShooter;
+
+  /** Creates a new SetAmpShooterSpeedCommand. */
+  public SetAmpShooterSpeedCommand(XboxController xboxController, AmpShooter ampShooter) {
+    this.xboxController = xboxController;
+    this.ampShooter = ampShooter;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(ampShooter);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    ampShooter.setWheelPower(xboxController.getLeftY());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/SetShooterAngleCommand.java

@@ -5,6 +5,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.PIDCommand;
 import frc.robot.subsystems.AngleShooter;
 import frc.robot.subsystems.Limelight;
@@ -31,6 +32,11 @@ public SetShooterAngleCommand(AngleShooter angleShooter, Limelight limelight) {
         // This uses the output
         output -> {
           angleShooter.setPower(output);
+          if (output > -0.1 && output < 0.1) {
+            SmartDashboard.putBoolean("Angle Ready", true);
+          } else {
+            SmartDashboard.putBoolean("Angle Ready", false);
+          }
         });
     this.angleShooter = angleShooter;
     this.limelight = limelight;

src/main/java/frc/robot/commands/SetShooterSpeedCommand.java

@@ -5,6 +5,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Shooter;
 
@@ -34,6 +35,12 @@ public void execute() {
     shooter.shooterPower(
         (pidControllerDroitRPM.calculate(shooter.getVelocityDroit(), speed) + (kV * speed)),
         (pidControllerGaucheRPM.calculate(shooter.getVelocityGauche(), speed) + (kV * speed)));
+
+    if (shooter.getVelocityDroit() > speed - 100 && shooter.getVelocityGauche() > speed - 100) {
+      SmartDashboard.putBoolean("Speed Ready", true);
+    } else {
+      SmartDashboard.putBoolean("Speed Ready", false);
+    }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/subsystems/AmpShooter.java

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.OperatorConstants;
+import frc.robot.Constants.PCM;
+
+public class AmpShooter extends SubsystemBase {
+  OperatorConstants deviceNumber = new OperatorConstants();
+  PCM pcm = new PCM();
+
+  private CANSparkMax ampShooterMotor;
+  private DoubleSolenoid piston;
+
+  /** Creates a new AmpShooter. */
+  public AmpShooter() {
+    piston =
+        new DoubleSolenoid(
+            1,
+            PneumaticsModuleType.CTREPCM,
+            pcm.PISTON_AMP_SHOOTER_FORWARD,
+            pcm.PISTON_AMP_SHOOTER_REVERSE);
+
+    ampShooterMotor = new CANSparkMax(deviceNumber.DeviceNumberAmpShooter, MotorType.kBrushless);
+    ampShooterMotor.setInverted(false);
+  }
+
+  public void setPosition(boolean out) {
+    if (out) {
+      piston.set(DoubleSolenoid.Value.kForward);
+    } else {
+      piston.set(DoubleSolenoid.Value.kReverse);
+    }
+  }
+
+  public void setWheelPower(double power) {
+    ampShooterMotor.set(power);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

src/main/java/frc/robot/subsystems/ClimberInAnBox.java

@@ -14,7 +14,8 @@
 public class ClimberInAnBox extends SubsystemBase {
   private Talon climberDroit;
   private Talon climberGauche;
-  private DoubleSolenoid piston;
+  private DoubleSolenoid piston1;
+  private DoubleSolenoid piston2;
 
   /** Creates a new ClimberInAnBox. */
   public ClimberInAnBox() {
@@ -25,23 +26,31 @@ public ClimberInAnBox() {
 
     climberDroit.setInverted(false);
     climberGauche.setInverted(false);
-    piston =
+    piston1 =
         new DoubleSolenoid(
             1,
             PneumaticsModuleType.CTREPCM,
-            pcm.PISTON_CLIMBER_FORWARD,
-            pcm.PISTON_CLIMBER_REVERSE);
-    piston.set(DoubleSolenoid.Value.kReverse);
+            pcm.PISTON_CLIMBER_FORWARD_1,
+            pcm.PISTON_CLIMBER_REVERSE_1);
+    piston2 =
+        new DoubleSolenoid(
+            1,
+            PneumaticsModuleType.CTREPCM,
+            pcm.PISTON_CLIMBER_FORWARD_2,
+            pcm.PISTON_CLIMBER_REVERSE_2);
+    piston1.set(DoubleSolenoid.Value.kReverse);
+    piston2.set(DoubleSolenoid.Value.kReverse);
   }
 
-  public void climb(double activated) {
-    climberDroit.set(activated);
-    climberGauche.set(activated);
+  public void climb(double droit, double gauche) {
+    climberDroit.set(droit);
+    climberGauche.set(gauche);
   }
 
   public void activateRatchet() {
 
-    piston.set(DoubleSolenoid.Value.kForward);
+    piston1.set(DoubleSolenoid.Value.kForward);
+    piston2.set(DoubleSolenoid.Value.kForward);
   }
 
   @Override

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -57,6 +57,7 @@ public Drivetrain() {
     avantDroitEncoder = avantdroit.getEncoder();
     arriereDroitEncoder = arrieredroit.getEncoder();
     arriereGaucheEncoder = arrieregauche.getEncoder();
+    m_robotDrive.setSafetyEnabled(false);
   }
 
   public double getGyroAngle() {
@@ -76,11 +77,11 @@ public void resetGyroAngle() {
   }
 
   public void ActivateDrivetank() {
-    piston.set(DoubleSolenoid.Value.kReverse);
+    piston.set(DoubleSolenoid.Value.kForward);
   }
 
   public void ActivateMecanum() {
-    piston.set(DoubleSolenoid.Value.kForward);
+    piston.set(DoubleSolenoid.Value.kReverse);
   }
 
   public double[] getEncoder() {
@@ -106,8 +107,9 @@ public void drive(
       double leftTrigger,
       double rightTrigger) {
     if (isTankDrive == true) {
+      driveTank(Math.pow(rightY, 3), Math.pow(leftY, 3));
 
-      m_robotDrive.driveCartesian(Math.pow(leftY, 3), 0, -Math.pow(rightX, 3));
+      // m_robotDrive.driveCartesian(Math.pow(leftY, 3), 0, -Math.pow(rightX, 3));
     } else {
       if (leftTrigger > 0) {
         m_robotDrive.driveCartesian(0, -Math.pow(leftTrigger, 3) / 2, 0);