Merge pull request #40 from 2491-NoMythic/shootButtons

Shoot buttons

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

@@ -109,6 +109,7 @@ public class RobotContainer {
   private Pigeon2 pigeon;
   private IndexCommand defaulNoteHandlingCommand;
   private IndexerSubsystem indexer;
+  private AimShooter defaultShooterAngleCommand;
   private SendableChooser<String> climbSpotChooser;
   private SendableChooser<Command> autoChooser;
   // Replace with CommandPS4Controller or CommandJoystick if needed
@@ -132,13 +133,13 @@ public RobotContainer() {
     limelightInit();
 
 
+    if(intakeExists && shooterExists && indexerExists) {indexCommandInst();}
     if(intakeExists) {intakeInst();}
     if(shooterExists) {shooterInst();}
     if(climberExists) {climberInst();}
     climbSpotChooserInit();
     if(lightsExist) {lightsInst();}
     if(indexerExists) {indexInit();}
-    if(intakeExists && shooterExists && indexerExists) {indexCommandInst();}
     Limelight.useDetectorLimelight(useDetectorLimelight);
     autoInit();
     // Configure the trigger bindings
@@ -171,20 +172,24 @@ private void driveTrainInst() {
   }
   private void shooterInst() {
     shooter = new ShooterSubsystem(ShooterConstants.SHOOTER_MOTOR_POWER);
+    shooter.setDefaultCommand(defaulNoteHandlingCommand);
     angleShooterSubsystem = new AngleShooterSubsystem();
-    angleShooterSubsystem.setDefaultCommand(new AimShooter(angleShooterSubsystem, ()-> (operatorController.getPOV() == 0)));
+    defaultShooterAngleCommand = new AimShooter(angleShooterSubsystem, operatorController::getPOV);
+    angleShooterSubsystem.setDefaultCommand(defaultShooterAngleCommand);
   }
   private void intakeInst() {
     intake = new IntakeSubsystem();
+    intake.setDefaultCommand(defaulNoteHandlingCommand);
   }
   private void climberInst() {
     climber = new Climber();
   }
   private void indexInit() {
     indexer = new IndexerSubsystem();
+    indexer.setDefaultCommand(defaulNoteHandlingCommand);
   }
   private void indexCommandInst() {
-    defaulNoteHandlingCommand = new IndexCommand(indexer, driverController::getR2Button, shooter, intake);
+    defaulNoteHandlingCommand = new IndexCommand(indexer, driverController::getR2Button, driverController::getL2Button, shooter, intake, driveTrain, angleShooterSubsystem);
   }
 
   private void autoInit() {
@@ -222,6 +227,7 @@ private void configureBindings() {
     new Trigger(driverController::getCrossButton).onTrue(new GoToAmp(driveTrain));
     new Trigger(driverController::getR1Button).whileTrue(new AimRobotMoving(
       driveTrain,
+      () -> modifyAxis(-driverController.getRawAxis(Z_AXIS), DEADBAND_NORMAL),
       () -> modifyAxis(-driverController.getRawAxis(Y_AXIS), DEADBAND_NORMAL),
       () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL),
       driverController::getR1Button));

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

@@ -29,8 +29,10 @@ public class AimRobotMoving extends Command {
     DoubleSupplier translationXSupplier;
     DoubleSupplier translationYSupplier;
     BooleanSupplier run;
+    DoubleSupplier rotationSupplier;
+    double rotationSpeed;
 
-  public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run){
+  public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run){
         m_drivetrain = drivetrain;
         speedController = new PIDController(
           AUTO_AIM_ROBOT_kP, 
@@ -40,6 +42,7 @@ public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier translation
           speedController.enableContinuousInput(-180, 180);
           this.translationXSupplier = translationXSupplier;
           this.translationYSupplier = translationYSupplier;
+          this.rotationSupplier = rotationSupplier;
           this.run = run;
           addRequirements(drivetrain);
         }
@@ -58,11 +61,15 @@ public void execute() {
           speedController.setSetpoint(desiredRobotAngle);
         //move robot to desired angle
         this.currentHeading = m_drivetrain.getGyroscopeRotation().getDegrees();
-
+        if(Math.abs(rotationSupplier.getAsDouble()) > 0.3) {
+          rotationSpeed = rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND;
+        } else {
+          rotationSpeed = speedController.calculate(currentHeading);
+        }
         m_drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds(
           translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
           translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
-          speedController.calculate(currentHeading),
+          rotationSpeed,
           m_drivetrain.getGyroscopeRotation()));
         // m_drivetrain.drive(new ChassisSpeeds(
         //   translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,

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

@@ -1,6 +1,7 @@
 package frc.robot.commands;
 
 import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.settings.Constants.Field;
@@ -9,11 +10,11 @@
 
 public class AimShooter extends Command {
 	AngleShooterSubsystem angleShooterSubsystem;
-	BooleanSupplier aimAtAmp;
+	DoubleSupplier aimAtAmpSupplier;
 
-	public AimShooter(AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier aimAtAmp) {
+	public AimShooter(AngleShooterSubsystem angleShooterSubsystem, DoubleSupplier aimAtAmp) {
 		this.angleShooterSubsystem = angleShooterSubsystem;
-		this.aimAtAmp = aimAtAmp;
+		this.aimAtAmpSupplier = aimAtAmp;
 	}
 
 	@Override
@@ -23,8 +24,8 @@ public void initialize() {
 
 	@Override
 	public void execute() {
-		if (!aimAtAmp.getAsBoolean()) {
-			double desiredShooterAngleSpeed = angleShooterSubsystem.calculateSpeakerAngle() * ShooterConstants.AUTO_AIM_SHOOTER_kP;
+		if (!(aimAtAmpSupplier.getAsDouble() == 0)) {
+			double desiredShooterAngleSpeed = angleShooterSubsystem.calculateSpeakerAngleDifference() * ShooterConstants.AUTO_AIM_SHOOTER_kP;
 			angleShooterSubsystem.pitchShooter(desiredShooterAngleSpeed);
 		} else {
 			double differenceAmp = Field.AMPLIFIER_ANGLE - angleShooterSubsystem.getShooterAngle();

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

@@ -26,7 +26,6 @@ public AngleShooter(AngleShooterSubsystem shooter, DoubleSupplier desiredShooter
     // Use addRequirements() here to declare subsystem dependencies.\
     m_shooter = shooter;
     this.desiredShooterAngleSupplier = desiredShooterAngleSupplier;
-    this.desiredShooterAngle = desiredShooterAngle;
     addRequirements(shooter);
   }
 

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

@@ -8,24 +8,36 @@
 
 import edu.wpi.first.wpilibj.PS4Controller;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.settings.Constants.DriveConstants;
+import frc.robot.settings.Constants.ShooterConstants;
+import frc.robot.subsystems.AngleShooterSubsystem;
+import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 
 
 public class IndexCommand extends Command {
-  BooleanSupplier shootButtonSupplier;
-  Boolean shootButton;
+  BooleanSupplier revUpSupplier;
+  Boolean revUp;
+  BooleanSupplier shootIfReadySupplier;
+  Boolean shootIfReady;
+
   IndexerSubsystem m_Indexer;
   ShooterSubsystem shooter;
   IntakeSubsystem intake;
+  DrivetrainSubsystem drivetrain;
+  AngleShooterSubsystem angleShooterSubsytem;
 
   /** Creates a new IndexCommand. */
-  public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootButtonSupplier, ShooterSubsystem shooter, IntakeSubsystem intake) {
+  public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem) {
     this.m_Indexer = m_IndexerSubsystem;
-    this.shootButtonSupplier = shootButtonSupplier;
+    this.shootIfReadySupplier = shootIfReadySupplier;
+    this.revUpSupplier = revUpSupplier;
     this.shooter = shooter;
     this.intake = intake;
+    this.drivetrain = drivetrain;
+    this.angleShooterSubsytem = angleShooterSubsystem;
 
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(m_IndexerSubsystem, shooter, intake);
@@ -46,13 +58,20 @@ public void execute() {
       shooter.turnOff();
     } else {
       intake.intakeOff();
-      if(!shootButtonSupplier.getAsBoolean()) {
-        m_Indexer.off();
+      if(revUpSupplier.getAsBoolean()) {
+        shooter.shootThing(1);
+      } else {
+        shooter.shootThing(0.3);
       }
-      shooter.shootThing(1);
     }
-    if (shootButtonSupplier.getAsBoolean()) {
-      m_Indexer.on();
+    if (shootIfReadySupplier.getAsBoolean()) {
+      if((angleShooterSubsytem.calculateSpeakerAngleDifference()<ShooterConstants.ALLOWED_ERROR)
+        && drivetrain.getSpeakerAngleDifference()<DriveConstants.ALLOWED_ERROR
+        && Math.abs(shooter.getError())<ShooterConstants.ALLOWED_SPEED_ERROR) {
+          m_Indexer.on();
+      } else {
+        m_Indexer.off();
+      }
     }
   }
 

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

@@ -36,6 +36,7 @@ private Constants () {}
   public static final int LED_COUNT = 52;
 
   public static final class DriveConstants {
+    public static final double ALLOWED_ERROR = 1;
     public enum Positions{
       FL(0),
       FR(0.25),
@@ -253,7 +254,9 @@ public static final class ShooterConstants{
   public static final int PITCH_MOTOR_ID = 2491;
   public static final double SHOOTER_MOTOR_POWER = 1;
   public static final double SHOOTING_SPEED_MPS = 10;
-
+  public static final double RUNNING_VELOCITY_RPS = 2491;
+  public static final double ALLOWED_ERROR = 1;
+  public static final double ALLOWED_SPEED_ERROR = 1;
 
   public static final double AUTO_AIM_ROBOT_kP = 0.125;
   public static final double AUTO_AIM_ROBOT_kI = 0.00;

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

@@ -52,7 +52,7 @@ public static void setDTChassisSpeeds(ChassisSpeeds speeds) {
 		DTChassisSpeeds = speeds;
 	}
 
-	public double calculateSpeakerAngle() {
+	public double calculateSpeakerAngleDifference() {
 		double deltaY;
 		shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS;
 		// triangle for robot angle

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

@@ -366,6 +366,9 @@ public double calculateSpeakerAngleMoving(){
 		SmartDashboard.putString("adjusted target", adjustedTarget.toString());
 		return m_desiredRobotAngle;
 	}
+	public double getSpeakerAngleDifference() {
+		return calculateSpeakerAngleMoving()-(getGyroscopeRotation().getDegrees()%360);
+	}
 	public Pose2d getAverageBotPose(LimelightValues ll2, LimelightValues ll3) {
 		double ll2X = ll2.getbotPose().getX();
 		double ll3X = ll3.getbotPose().getX();

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

@@ -10,6 +10,7 @@
  import com.revrobotics.CANSparkBase.ControlType;
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.SparkAbsoluteEncoder.Type;
 
 import frc.robot.commands.AngleShooter;
 import frc.robot.commands.RotateRobot;
@@ -116,6 +117,9 @@ public void shootThing(double runSpeed) {
      shooterR.set(runSpeed);
      shooterL.set(runSpeed);
    }
+  public double getError() {
+    return Math.abs(shooterR.getAbsoluteEncoder(Type.kDutyCycle).getVelocity()-ShooterConstants.RUNNING_VELOCITY_RPS);
+  }
   public void turnOff(){
      shooterR.set(0);
      shooterL.set(0);