Merge branch 'main' into collectNoteExtra

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

@@ -34,6 +34,7 @@
 
 import frc.robot.settings.Constants.Field;
 import frc.robot.commands.IndexCommand;
+import frc.robot.commands.IndicatorLights;
 import frc.robot.settings.Constants;
 import frc.robot.settings.Constants.ClimberConstants;
 import frc.robot.settings.Constants.DriveConstants;
@@ -57,6 +58,7 @@
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.Limelight;
+import frc.robot.subsystems.RobotState;
 import frc.robot.subsystems.ShooterSubsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -170,7 +172,7 @@ private void climbSpotChooserInit() {
     SmartDashboard.putData(climbSpotChooser);
   }
   private void driveTrainInst() {
-    driveTrain = new DrivetrainSubsystem(lights, lightsExist);
+    driveTrain = new DrivetrainSubsystem();
     defaultDriveCommand = new Drive(
       driveTrain, 
       () -> driverController.getL1Button(),
@@ -197,8 +199,6 @@ private void indexInit() {
   private void indexCommandInst() {
     defaulNoteHandlingCommand = new IndexCommand(indexer, driverController::getR2Button, driverController::getL2Button, shooter, intake, driveTrain, angleShooterSubsystem);
     indexer.setDefaultCommand(defaulNoteHandlingCommand);
-    shooter.setDefaultCommand(defaulNoteHandlingCommand);
-    intake.setDefaultCommand(defaulNoteHandlingCommand);
   }
 
   private void autoInit() {
@@ -212,6 +212,7 @@ private void limelightInit() {
   }
   private void lightsInst() {
     lights = new Lights(Constants.LED_COUNT-1);
+    lights.setDefaultCommand(new IndicatorLights(lights));
   }
 
 
@@ -225,21 +226,15 @@ private void lightsInst() {
    * joysticks}.
    */
   private void configureBindings() {
-    // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
-    new Trigger(m_exampleSubsystem::exampleCondition)
-        .onTrue(new ExampleCommand(m_exampleSubsystem));
-
     // new Trigger(driverController::getCrossButton).onTrue(new autoAimParallel(driveTrain/*, shooter*/));
     new Trigger(driverController::getPSButton).onTrue(new InstantCommand(driveTrain::zeroGyroscope));
-    SmartDashboard.putData(new RotateRobot(driveTrain, driveTrain::calculateSpeakerAngle));
-    new Trigger(driverController::getTriangleButton).onTrue(new GoToClimbSpot(driveTrain, climbSpotChooser));
-    new Trigger(driverController::getCrossButton).whileTrue(new GoToAmp(driveTrain));
-    new Trigger(driverController::getR1Button).whileTrue(new AimRobotMoving(
+    new Trigger(driverController::getCircleButton).whileTrue(new GoToAmp(driveTrain));
+    new Trigger(driverController::getL2Button).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));
+      driverController::getL2Button));
 
     new Trigger(driverController::getR1Button).onTrue(new SequentialCommandGroup(
       new CollectNote(driveTrain, limelight),
@@ -248,24 +243,33 @@ private void configureBindings() {
     new Trigger(driverController::getTouchpadPressed).onTrue(new InstantCommand(driveTrain::stop, driveTrain));
 
     if(shooterExists) {
-      new Trigger(operatorController::getCircleButton).onTrue(new ManualShoot(shooter));
       AngleShooter shooterUpCommand = new AngleShooter(angleShooterSubsystem, () -> Constants.ShooterConstants.shooterup);
-      new Trigger(driverController::getL1Button).onTrue(shooterUpCommand);
+      new Trigger(()->driverController.getPOV() == 180).whileTrue(shooterUpCommand);
       SmartDashboard.putData("Manual Angle Shooter Up", shooterUpCommand);
     }
+    if(indexerExists) {
+      new Trigger(driverController::getL1Button).whileTrue(new ManualShoot(indexer));
+    }
     if(climberExists) {
-      new Trigger(operatorController::getCrossButton).onTrue(new AutoClimb(climber)).onFalse(new InstantCommand(()-> climber.climberStop()));
-      new Trigger(operatorController::getTriangleButton).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_UP))).onFalse(new InstantCommand(()-> climber.climberStop()));
-      new Trigger(operatorController::getSquareButton).onTrue(new ClimberPullDown(climber));
+      new Trigger(driverController::getCrossButton).onTrue(new AutoClimb(climber)).onFalse(new InstantCommand(()-> climber.climberStop()));
+      new Trigger(driverController::getTriangleButton).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_UP))).onFalse(new InstantCommand(()-> climber.climberStop()));
+      new Trigger(driverController::getSquareButton).whileTrue(new ClimberPullDown(climber));
     }
     SmartDashboard.putData("set offsets", new InstantCommand(driveTrain::setEncoderOffsets));
     SmartDashboard.putData(new InstantCommand(driveTrain::forceUpdateOdometryWithVision));
-
-    // //Intake bindings
-    // new Trigger(operatorController::getL1Button).onTrue(new IntakeCommand(intake, iDirection.INTAKE));
-   // new Trigger(operatorController::getL2Button).onTrue(new IntakeCommand(intake, iDirection.OUTAKE));
-   // new Trigger(operatorController::getR2Button).onTrue(new IntakeCommand(intake, iDirection.COAST));
-    //for testing Rotate Robot command
+/* bindings:
+ *    L2: aim at speaker and rev up shooter to max (hold)
+ *    L1: manually feed shooter (hold)
+ *    R2: shoot if everything is lined up (hold)
+ *    R1: automatically pick up note (press)
+ *    Circle: lineup with the amp (hold)
+ *    D-Pad down: move shooter up manually (hold)
+ *    D-pad right: aim shooter at amp (hold)
+ *    Triangle: move climber up (hold)
+ *    Cross: auto-climb down (hold)
+ *    Square: manually pull down with climber (hold)
+ *    
+ */
     };
 
 
@@ -344,7 +348,11 @@ public void teleopPeriodic() {
     driveTrain.calculateSpeakerAngle();
     if(useDetectorLimelight) {
       SmartDashboard.putNumber("Is Note Seen?", limelight.getNeuralDetectorValues().ta);
+      RobotState.getInstance().IsNoteSeen = limelight.getNeuralDetectorValues().isResultValid;
     }
+    SmartDashboard.putBoolean("is note seen", RobotState.getInstance().IsNoteSeen);
+		SmartDashboard.putBoolean("shooter in range", RobotState.getInstance().ShooterInRange);
+		SmartDashboard.putBoolean("shooter ready", RobotState.getInstance().ShooterReady);
   }
 
   public void disabledPeriodic() {

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

@@ -24,7 +24,7 @@ public void initialize() {
 
 	@Override
 	public void execute() {
-		if (!(aimAtAmpSupplier.getAsDouble() == 0)) {
+		if (!(aimAtAmpSupplier.getAsDouble() == 90)) {
 			double desiredShooterAngleSpeed = angleShooterSubsystem.calculateSpeakerAngleDifference() * ShooterConstants.AUTO_AIM_SHOOTER_kP;
 			angleShooterSubsystem.pitchShooter(desiredShooterAngleSpeed);
 		} else {

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

@@ -15,6 +15,7 @@
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.RobotState;
 import frc.robot.subsystems.ShooterSubsystem;
 
 
@@ -69,23 +70,33 @@ public void execute() {
       if(revUpSupplier.getAsBoolean()) {
         shooter.shootThing(1);
       } else {
-        shooter.shootThing(0.3);
+        shooter.shootThing(ShooterConstants.SHOOTER_AMP_POWER);
       }
     }
-    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();
+    boolean indexer = false;
+    if((angleShooterSubsytem.calculateSpeakerAngleDifference()<ShooterConstants.ALLOWED_ERROR)
+      && drivetrain.getSpeakerAngleDifference()<DriveConstants.ALLOWED_ERROR
+      && Math.abs(shooter.getError())<ShooterConstants.ALLOWED_SPEED_ERROR)
+    {
+      RobotState.getInstance().ShooterReady = true;
+      if (shootIfReadySupplier.getAsBoolean()) {
+        indexer = true;
       }
+    } else {
+      RobotState.getInstance().ShooterReady = false;
+    }
+    if (indexer) {
+      m_Indexer.on();
+    } else {
+      m_Indexer.off();
     }
   }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    RobotState.getInstance().ShooterReady = false;
+  }
 
   // Returns true when the command should end.
   @Override

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

@@ -0,0 +1,52 @@
+// 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.Command;
+import frc.robot.subsystems.Lights;
+import frc.robot.subsystems.RobotState;
+public class IndicatorLights extends Command {
+
+  Lights lights;
+
+  public IndicatorLights(Lights lights) {
+    addRequirements(lights);
+    this.lights = lights;
+  }
+
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    if (RobotState.getInstance().IsNoteSeen) {
+    lights.setSectionOne(50,40,0);
+    } else {
+      lights.setSectionOne(50, 0, 50);
+    }
+    if (RobotState.getInstance().ShooterInRange && RobotState.getInstance().ShooterReady){
+      lights.setSectionTwo(0,50,0);
+    } else if (RobotState.getInstance().ShooterInRange){
+      lights.setSectionTwo(50, 50, 0);
+    } else {
+      lights.setSectionTwo(50, 0, 50);
+    }
+    lights.dataSetter();
+  }
+
+
+  @Override
+  public void end(boolean interrupted) {
+    lights.lightsOut();
+    lights.dataSetter();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -4,16 +4,16 @@
 
 package frc.robot.commands;
 import frc.robot.settings.Constants.ShooterConstants;
-import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.IndexerSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class ManualShoot extends Command {
-  public ShooterSubsystem shooter;
+  private IndexerSubsystem indexer;
   /** Creates a new ManualShoot. */
-  public ManualShoot(ShooterSubsystem shooter) {
+  public ManualShoot(IndexerSubsystem indexer) {
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(shooter);
-    this.shooter = shooter;
+    addRequirements(indexer);
+    this.indexer = indexer;
   }
 
   // Called when the command is initially scheduled.
@@ -23,13 +23,13 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    shooter.shootThing(ShooterConstants.SHOOTER_MOTOR_POWER);
+    indexer.on();
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    shooter.turnOff();
+    indexer.off();
   }
 
   // Returns true when the command should end.

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

@@ -33,7 +33,7 @@ public final class Constants {
 
   private Constants () {}
 
-  public static final int LED_COUNT = 52;
+  public static final int LED_COUNT = 24;
 
   public static final class DriveConstants {
     public static final double ALLOWED_ERROR = 1;
@@ -107,7 +107,7 @@ private DriveConstants() {
     /**
      * The diameter of the module's wheel in meters.
      */
-    public static final double DRIVETRAIN_WHEEL_DIAMETER = 0.10033;
+    public static final double DRIVETRAIN_WHEEL_DIAMETER = 0.097;
 
     /**
      * The overall drive reduction of the module. Multiplying motor rotations by
@@ -258,6 +258,7 @@ public static final class ShooterConstants{
   public static final int SHOOTER_L_MOTORID = 2491;
   public static final int PITCH_MOTOR_ID = 2491;
   public static final double SHOOTER_MOTOR_POWER = 1;
+  public static final double SHOOTER_AMP_POWER = 0.3;
   public static final double SHOOTING_SPEED_MPS = 10;
   public static final double RUNNING_VELOCITY_RPS = 2491;
   public static final double ALLOWED_ERROR = 1;
@@ -275,6 +276,8 @@ public static final class ShooterConstants{
   public static final double DEGREES_PER_ROTATION = 2491;
   public static final double DISTANCE_MULTIPLIER = 0;
   public static final double OFFSET_MULTIPLIER = 1;
+  public static final double MINIMUM_SHOOTER_ANGLE = 10;//still has to be found
+  public static final double MAXIMUM_SHOOTER_ANGLE = 120;//still has to be found
 
 
 

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

@@ -99,6 +99,12 @@ public double calculateSpeakerAngleDifference() {
 		double desiredShooterAngle = Math
 				.toDegrees(Math.asin(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT / totalDistToSpeaker));
 		desiredShooterAngle = desiredShooterAngle+(speakerDist*DISTANCE_MULTIPLIER);
+		if(desiredShooterAngle<ShooterConstants.MINIMUM_SHOOTER_ANGLE) {
+			desiredShooterAngle = ShooterConstants.MINIMUM_SHOOTER_ANGLE;
+		}
+		if(desiredShooterAngle>ShooterConstants.MAXIMUM_SHOOTER_ANGLE) {
+			desiredShooterAngle = ShooterConstants.MAXIMUM_SHOOTER_ANGLE;
+		}
 		SmartDashboard.putNumber("desired shooter angle", desiredShooterAngle);
 
 		double differenceAngle = (desiredShooterAngle - this.getShooterAngle());

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

@@ -80,7 +80,6 @@ public class DrivetrainSubsystem extends SubsystemBase {
 
 	private final SwerveDrivePoseEstimator odometer;
 	private final Field2d m_field = new Field2d();
-	Lights lights;
 
 	//speaker angle calculating variables:
 	double m_desiredRobotAngle;
@@ -106,16 +105,12 @@ public class DrivetrainSubsystem extends SubsystemBase {
 	Translation2d adjustedTarget;
 	double offsetSpeakerdist;
 	public double speakerDist;
-
-	Boolean lightsExist;
 	Limelight limelight;
 
 	double MathRanNumber;
 
-	public DrivetrainSubsystem(Lights lights, Boolean lightsExist) {
+	public DrivetrainSubsystem() {
 		MathRanNumber = 0;
-		this.lights = lights;
-		this.lightsExist = lightsExist;
 		this.limelight=Limelight.getInstance();
 
 		Preferences.initDouble("FL offset", 0);
@@ -292,11 +287,8 @@ public double calculateSpeakerAngle(){
 		}
 		speakerDist = Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2));
 		SmartDashboard.putNumber("dist to speakre", speakerDist);
-		if(speakerDist<Field.MAX_SHOOTING_DISTANCE && lightsExist) {
-			lights.setLights(0, Constants.LED_COUNT, 0, 100, 0);
-		} else {if(lights != null) {
-			lights.lightsOut();
-		} }
+
+		// RobotState.getInstance().ShooterInRange = speakerDist<Field.MAX_SHOOTING_DISTANCE;
 
 		//getting desired robot angle
 		// if (alliance.isPresent() && alliance.get() == Alliance.Blue) {
@@ -365,11 +357,7 @@ public double calculateSpeakerAngleMoving(){
 		adjustedTarget = new Translation2d(offsetSpeakerX, offsetSpeakerY);
 		offsetSpeakerdist = Math.sqrt(Math.pow(offsetDeltaY, 2) + Math.pow(offsetDeltaX, 2));
 		SmartDashboard.putNumber("offsetSpeakerDis", offsetSpeakerdist);
-		if(offsetSpeakerdist<Field.MAX_SHOOTING_DISTANCE && lightsExist) {
-			lights.setLights(0, Constants.LED_COUNT, 0, 100, 0);
-		} else {if(lightsExist) {
-			lights.lightsOut();
-		}}
+		RobotState.getInstance().ShooterInRange = offsetSpeakerdist<Field.MAX_SHOOTING_DISTANCE;
 		SmartDashboard.putString("offset amount", targetOffset.toString());
 		SmartDashboard.putString("offset speaker location", new Translation2d(offsetSpeakerX, offsetSpeakerY).toString());
 		//getting desired robot angle