Operator and other

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

@@ -49,6 +49,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -62,7 +63,7 @@
 import edu.wpi.first.wpilibj.PS4Controller;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import frc.robot.commands.AngleShooter;
-
+import frc.robot.commands.OperatorAngler;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -92,7 +93,6 @@ public class RobotContainer {
   private Lights lights;
   private PS4Controller driverController;
   private PS4Controller operatorController;
-  //private PS4Controller operatorController;
   private Limelight limelight;
   private IndexCommand defaulNoteHandlingCommand;
   private IndexerSubsystem indexer;
@@ -117,6 +117,7 @@ public class RobotContainer {
   BooleanSupplier GroundIntakeSup;
   BooleanSupplier FarStageAngleSup;
   BooleanSupplier OperatorPreRevSup;
+  BooleanSupplier BackupManualShootSup;
 
   // Replace with CommandPS4Controller or CommandJoystick if needed
 
@@ -142,7 +143,6 @@ public RobotContainer() {
     // SignalLogger.start();
     driverController = new PS4Controller(DRIVE_CONTROLLER_ID);
     operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID);
-    //operatorController = new PS4Controller(OPERATOs_CONTROLLER_ID);
     PDP = new PowerDistribution(1, ModuleType.kRev);
 
     ZeroGyroSup = driverController::getPSButton;
@@ -156,6 +156,7 @@ public RobotContainer() {
     ClimberUpSup = operatorController::getTriangleButton;
     ShooterUpManualSup = ()->driverController.getPOV() == 0;
     ManualShootSup = driverController::getL1Button;
+    BackupManualShootSup = operatorController::getR1Button;
     ForceVisionSup = driverController::getOptionsButton;
     GroundIntakeSup = operatorController::getTouchpad;
     FarStageAngleSup = driverController::getTouchpad;
@@ -277,18 +278,24 @@ private void configureBindings() {
     if(angleShooterExists) {
       new Trigger(ShooterUpManualSup).whileTrue(new AngleShooter(angleShooterSubsystem, ()->ShooterConstants.PRAC_MAXIMUM_SHOOTER_ANGLE));
       SmartDashboard.putData("Manual Angle Shooter Up", new AngleShooter(angleShooterSubsystem, ()->ShooterConstants.PRAC_MAXIMUM_SHOOTER_ANGLE));
+      new Trigger(operatorController::getL2Button).whileTrue(new OperatorAngler(angleShooterSubsystem, operatorController));
     }
     if(indexerExists) {
       new Trigger(ManualShootSup).whileTrue(new ManualShoot(indexer, driverController::getPOV));
+      new Trigger(BackupManualShootSup).whileTrue(new RunCommand(() -> indexer.set(operatorController.getRightY()), indexer));
     }
     if(climberExists) {
       // new Trigger(driverController::getCrossButton).whileTrue(new AutoClimb(climber)).onFalse(new InstantCommand(()-> climber.climberStop()));
       new Trigger(ClimberDownSup).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_DOWN))).onFalse(new InstantCommand(()-> climber.climberGo(0)));
       new Trigger(ClimberUpSup).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_UP))).onFalse(new InstantCommand(()-> climber.climberGo(0)));
       // new Trigger(driverController::getSquareButton).whileTrue(new ClimberPullDown(climber));
+      new Trigger(operatorController::getTouchpad).whileTrue(new RunCommand(() -> climber.climberSeperate(operatorController.getLeftY(), operatorController.getRightY()), climber));
+
     }
     if(shooterExists) {
       new Trigger(AmpAngleSup).whileTrue(new InstantCommand(()->shooter.shootRPS(ShooterConstants.AMP_RPS), shooter));
+      new Trigger(operatorController::getR1Button).whileTrue(new InstantCommand(()->shooter.shootRPS(ShooterConstants.LONG_SHOOTING_RPS), shooter));
+      new Trigger(operatorController::getL1Button).whileTrue(new InstantCommand(()->shooter.shootRPS(ShooterConstants.HUMAN_PLAYER_RPS), shooter));
     }
     if(intakeExists) {
       new Trigger(GroundIntakeSup).whileTrue(new GroundIntake(intake, indexer));

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

@@ -0,0 +1,53 @@
+// 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.PS4Controller;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.settings.Constants.ShooterConstants;
+import frc.robot.subsystems.AngleShooterSubsystem;
+
+public class OperatorAngler extends Command {
+	AngleShooterSubsystem angler;
+	PS4Controller opController;
+	/** Creates a new OperatorShooter. */
+	public OperatorAngler(AngleShooterSubsystem angleShooterSubsystem, PS4Controller operatorController) {
+		// Use addRequirements() here to declare subsystem dependencies.
+		angler = angleShooterSubsystem;
+		opController = operatorController;
+		addRequirements(angleShooterSubsystem);
+	}
+
+	// Called when the command is initially scheduled.
+	@Override
+	public void initialize() {
+		angler.getDesiredShooterAngle();
+	}
+
+	// Called every time the scheduler runs while the command is scheduled.
+	@Override
+	public void execute() {
+		if (opController.getPOV() == 90){
+			angler.setDesiredShooterAngle(ShooterConstants.MINIMUM_SHOOTER_ANGLE);
+		} else if (opController.getPOV() == 270) {
+			angler.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE);
+		} else if (opController.getCircleButton()) {
+			angler.setDesiredShooterAngle(45);
+
+		} else {
+			angler.setDesiredShooterAngle(angler.getDesiredShooterAngle() + opController.getLeftY()*0.2);
+		}
+	}
+
+	// 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/settings/Constants.java

@@ -294,7 +294,7 @@ public static final class ShooterConstants{
 
   //PID values for pitch motor (changes angle of shooter):
   public static final double shooterup = 45;
-
+  
 }
 public static final class ClimberConstants{
   public static final int CLIMBER_MOTOR_RIGHT = 23;

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

@@ -36,7 +36,7 @@ public class AngleShooterSubsystem extends SubsystemBase {
 	public double desiredZeroOffset;
 	int runsValid;
 	double speakerDistGlobal;
-
+	double grabbableDSA; 
 	public AngleShooterSubsystem() {
 		runsValid = 0;
 		pitchMotor = new CANSparkMax(PITCH_MOTOR_ID, MotorType.kBrushless);
@@ -74,6 +74,10 @@ public void setDesiredShooterAngle(double degrees) {
 			degrees,
 			CANSparkMax.ControlType.kPosition
 		);
+		grabbableDSA = degrees;
+	}
+	public double getDesiredShooterAngle(){
+		return grabbableDSA;
 	}
 	public void pitchShooter(double pitchSpeed) {
 		pitchMotor.set(pitchSpeed);

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

@@ -33,10 +33,12 @@ public class Climber extends SubsystemBase {
   SparkLimitSwitch hallEffectL;
   Boolean leftClimberOn;
   Boolean rightClimberOn;
-  double runSpeed;
+  double runSpeedL;
+  double runSpeedR;
     /** Creates a new Climber. */
   public Climber() {
-    runSpeed = 0;
+    runSpeedL = 0;
+    runSpeedR = 0;
     climbMotorR = new CANSparkMax(ClimberConstants.CLIMBER_MOTOR_RIGHT, MotorType.kBrushless);
     climbMotorL = new CANSparkMax(ClimberConstants.CLIMBER_MOTOR_LEFT, MotorType.kBrushless);
     climbMotorR.setInverted(false);
@@ -53,9 +55,13 @@ public Climber() {
     climbMotorR.burnFlash();
   }
  public void climberGo(double speed){
-    runSpeed = speed;
+    runSpeedL = speed;
+    runSpeedR = speed;
+ }
+ public void climberSeperate(double lSpeed, double rSpeed){
+      runSpeedL = lSpeed;
+      runSpeedR = rSpeed;
  }
-
  public void climberStop(){
   climbMotorR.set(0);
   climbMotorL.set(0);
@@ -82,6 +88,18 @@ public void resetInitial(){
   initialEncoderRotationsL = 0;
   initialEncoderRotationsR = 0;
 }
+static double safeSpeed(double requestedSpeed, SparkLimitSwitch limiter, double currentEncoderRotations){
+  if (requestedSpeed > 0){
+    if (limiter.isPressed()){
+      return 0;
+    }
+  } else {
+    if (currentEncoderRotations > ClimberConstants.MAX_MOTOR_ROTATIONS) {
+      return 0;
+    }
+  }
+  return requestedSpeed;
+}
 @Override
 public void periodic() {
   currentEncoderRotationsL = Math.abs(Math.abs(climbEncoderL.getPosition()) - initialEncoderRotationsL);
@@ -90,24 +108,8 @@ public void periodic() {
   SmartDashboard.putNumber("RIGHT rotations since start", currentEncoderRotationsR);
   SmartDashboard.putBoolean("left hall effect value", hallEffectL.isPressed());
   SmartDashboard.putBoolean("right hall effect value", hallEffectR.isPressed());
-  double rSpeed = 0;
-  double lSpeed = 0;
-   if (runSpeed>0) {
-    if(!hallEffectL.isPressed()) {
-      lSpeed = runSpeed;
-    } 
-    if(!hallEffectR.isPressed()) {
-      rSpeed = runSpeed;
-    }
-  } else {
-    if (currentEncoderRotationsL < ClimberConstants.MAX_MOTOR_ROTATIONS){
-      lSpeed = runSpeed;
-    }
-    if (currentEncoderRotationsR < ClimberConstants.MAX_MOTOR_ROTATIONS){
-      rSpeed = runSpeed;
-    } 
-  }
-  climbMotorL.set(lSpeed);
-  climbMotorR.set(rSpeed);
+
+  climbMotorL.set(safeSpeed(runSpeedL, limitSwitchL, currentEncoderRotationsL));
+  climbMotorR.set(safeSpeed(runSpeedR, limitSwitchR, currentEncoderRotationsR));
 }
 }