Merge branch 'main' into subwooferAngle

.pathplanner/settings.json

@@ -1,6 +1,6 @@
 {
   "robotWidth": 0.81,
-  "robotLength": 0.813,
+  "robotLength": 0.81,
   "holonomicMode": true,
   "pathFolders": [
     "Collect Notes",

src/main/deploy/pathplanner/paths/ScoreAmp.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.85,
-        "y": 6.7
+        "x": 2.0,
+        "y": 7.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.85,
-        "y": 6.889081499499318
+        "x": 2.0,
+        "y": 7.189081499499318
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.8527875727163086,
-        "y": 7.7
+        "x": 2.0,
+        "y": 7.9
       },
       "prevControl": {
-        "x": 1.8527875727163086,
-        "y": 7.6000000000000005
+        "x": 2.0,
+        "y": 7.800000000000001
       },
       "nextControl": null,
       "isLocked": false,
@@ -30,16 +30,16 @@
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 0.0,
+      "waypointRelativePos": 0.25,
       "rotationDegrees": -90.0,
-      "rotateFast": false
+      "rotateFast": true
     }
   ],
   "constraintZones": [],
   "eventMarkers": [
     {
       "name": "New Event Marker",
-      "waypointRelativePos": 0.1,
+      "waypointRelativePos": 0.6,
       "command": {
         "type": "parallel",
         "data": {
@@ -73,7 +73,7 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 5.0,
+    "maxVelocity": 2.0,
     "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
@@ -89,5 +89,5 @@
     "rotation": -90.0,
     "velocity": 0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/java/frc/robot/Robot.java

@@ -45,6 +45,7 @@ public void robotPeriodic() {
     // commands, running already-scheduled commands, removing finished or interrupted commands,
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
+    m_robotContainer.robotPeriodic();
     CommandScheduler.getInstance().run();
   }
 

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

@@ -14,8 +14,11 @@
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
+import org.littletonrobotics.urcl.URCL;
+
 import static frc.robot.settings.Constants.DriveConstants.*;
 
+import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.fasterxml.jackson.core.sym.Name;
 import com.pathplanner.lib.auto.AutoBuilder;
@@ -73,13 +76,16 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import frc.robot.commands.Drive;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.units.Angle;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PS4Controller;
+import edu.wpi.first.wpilibj.PowerDistribution;
 import frc.robot.commands.ManualShoot;
 import frc.robot.commands.AngleShooter;
 import frc.robot.commands.IntakeCommand;
@@ -123,6 +129,8 @@ public class RobotContainer {
   private SendableChooser<String> climbSpotChooser;
   private SendableChooser<Command> autoChooser;
   private DoubleSupplier angleSup;
+  private PowerDistribution PDP;
+
   // Replace with CommandPS4Controller or CommandJoystick if needed
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
@@ -139,10 +147,15 @@ public RobotContainer() {
     Preferences.initBoolean("Use Limelight", false);
     Preferences.initBoolean("Use 2 Limelights", false);
     Preferences.initDouble("wait # of seconds", 0);
-
+
+    // DataLogManager.start();
+    // URCL.start();
+    // SignalLogger.setPath("/media/sda1/ctre-logs/");
+    // SignalLogger.start();
     driverController = new PS4Controller(DRIVE_CONTROLLER_ID);
     operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID);
     pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID);
+    PDP = new PowerDistribution(1, ModuleType.kRev);
 
     // = new PathPlannerPath(null, DEFAUL_PATH_CONSTRAINTS, null, climberExists);
     limelightInit();
@@ -205,7 +218,7 @@ private void indexInit() {
     indexer = new IndexerSubsystem();
   }
   private void indexCommandInst() {
-    defaulNoteHandlingCommand = new IndexCommand(indexer, driverController::getR2Button, driverController::getL2Button, shooter, intake, driveTrain, angleShooterSubsystem, driverController::getR1Button);
+    defaulNoteHandlingCommand = new IndexCommand(indexer, driverController::getR2Button, driverController::getL2Button, shooter, intake, driveTrain, angleShooterSubsystem, driverController::getR1Button, driverController::getPOV);
     indexer.setDefaultCommand(defaulNoteHandlingCommand);
   }
 
@@ -244,19 +257,22 @@ private void configureBindings() {
       () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL),
       driverController::getL2Button,
       driverController::getR1Button));
-    new Trigger(driverController::getOptionsButton).whileTrue(new InstantCommand(driveTrain::forceUpdateOdometryWithVision));
-    new Trigger(driverController::getSquareButton).onTrue(new SequentialCommandGroup(
-      new CollectNote(driveTrain, limelight),
-      new DriveTimeCommand(-2, 0, 0, 0.5, driveTrain)
+
+    if(Preferences.getBoolean("Detector Limelight", false)) {
+      new Trigger(driverController::getR1Button).onTrue(new SequentialCommandGroup(
+        new CollectNote(driveTrain, limelight),
+        new DriveTimeCommand(-2, 0, 0, 0.5, driveTrain)
       ));
+    }
+    new Trigger(driverController::getOptionsButton).onTrue(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", true))).onFalse(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", false)));
     new Trigger(driverController::getTouchpadPressed).onTrue(new InstantCommand(driveTrain::stop, driveTrain));
     SmartDashboard.putData("force update limelight position", new InstantCommand(()->driveTrain.forceUpdateOdometryWithVision(), driveTrain));
     if(angleShooterExists) {
       new Trigger(()->driverController.getPOV() == 180).whileTrue(new AngleShooter(angleShooterSubsystem, ()->ShooterConstants.MAXIMUM_SHOOTER_ANGLE));
       SmartDashboard.putData("Manual Angle Shooter Up", new AngleShooter(angleShooterSubsystem, ()->ShooterConstants.MAXIMUM_SHOOTER_ANGLE));
     }
     if(indexerExists) {
-      new Trigger(driverController::getL1Button).whileTrue(new ManualShoot(indexer));
+      new Trigger(driverController::getL1Button).whileTrue(new ManualShoot(indexer, driverController::getPOV));
     }
     if(climberExists) {
       // new Trigger(driverController::getCrossButton).whileTrue(new AutoClimb(climber)).onFalse(new InstantCommand(()-> climber.climberStop()));
@@ -431,7 +447,15 @@ public void teleopPeriodic() {
 		SmartDashboard.putBoolean("shooter in range", RobotState.getInstance().ShooterInRange);
 		SmartDashboard.putBoolean("shooter ready", RobotState.getInstance().ShooterReady);
   }
-
+
+  public void logPower(){
+    for(int i = 0; i < 16; i++) { 
+      SmartDashboard.putNumber("PDP Current " + i, PDP.getCurrent(i));
+    }
+  }
+  public void robotPeriodic() {
+    logPower();
+  }
   public void disabledPeriodic() {
 
   }

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

@@ -30,6 +30,7 @@ public class IndexCommand extends Command {
   BooleanSupplier revUpSupplier;
   Boolean revUp;
   BooleanSupplier shootIfReadySupplier;
+  DoubleSupplier ampSupplier;;
   Boolean shootIfReady;
   // DoubleSupplier POVSupplier;
   BooleanSupplier humanPlayerSupplier;
@@ -45,19 +46,21 @@ public class IndexCommand extends Command {
   BooleanSupplier SubwooferSupplier3;
 
   /** Creates a new IndexCommand. */
-  public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier) {
+  public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier, DoubleSupplier ampSupplier) {
     this.m_Indexer = m_IndexerSubsystem;
     this.shootIfReadySupplier = shootIfReadySupplier;//R2
     this.revUpSupplier = revUpSupplier;//L2
     this.shooter = shooter;
     this.intake = intake;
     this.drivetrain = drivetrain;
+    this.ampSupplier = ampSupplier;
     this.angleShooterSubsytem = angleShooterSubsystem;
     this.humanPlayerSupplier = humanPlaySupplier;//R1
     this.SubwooferSupplier1 = SubwooferSupplier1;
     this.SubwooferSupplier2 = SubwooferSupplier2;
     this.SubwooferSupplier3 = SubwooferSupplier3;
     SmartDashboard.putNumber("amp RPS", AMP_RPS);
+    SmartDashboard.putNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED);
     SmartDashboard.putNumber("amp angle", Field.AMPLIFIER_ANGLE);
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(m_IndexerSubsystem, shooter, intake);
@@ -119,11 +122,14 @@ public void execute() {
           indexer = true;
         }
         if (indexer) {
-          m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED);
-        } else {
-          m_Indexer.off();
-        }
-     }
+          if(ampSupplier.getAsDouble() == 90) {
+            m_Indexer.set(SmartDashboard.getNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED));
+          } else {
+            m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED);
+          }
+         } else {
+            m_Indexer.off();
+         }
     }
   }
 

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

@@ -6,15 +6,21 @@
 import frc.robot.settings.Constants.IndexerConstants;
 import frc.robot.settings.Constants.ShooterConstants;
 import frc.robot.subsystems.IndexerSubsystem;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class ManualShoot extends Command {
   private IndexerSubsystem indexer;
+  DoubleSupplier ampSupplier;
   /** Creates a new ManualShoot. */
-  public ManualShoot(IndexerSubsystem indexer) {
+  public ManualShoot(IndexerSubsystem indexer, DoubleSupplier ampSupplier) {
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(indexer);
     this.indexer = indexer;
+    this.ampSupplier = ampSupplier;
   }
 
   // Called when the command is initially scheduled.
@@ -24,7 +30,11 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED);
+    if(ampSupplier.getAsDouble() == 90 || ampSupplier.getAsDouble() == 45|| ampSupplier.getAsDouble() == 135) {
+      indexer.set(IndexerConstants.INDEXER_AMP_SPEED);
+    } else {
+      indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED);
+    }
   }
 
   // Called once the command ends or is interrupted.

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

@@ -108,7 +108,7 @@ private DriveConstants() {
     /**
      * The diameter of the module's wheel in meters.
      */
-    public static final double DRIVETRAIN_WHEEL_DIAMETER = 0.097;
+    public static final double DRIVETRAIN_WHEEL_DIAMETER = 0.098;
 
     /**
      * The overall drive reduction of the module. Multiplying motor rotations by
@@ -209,6 +209,8 @@ private DriveConstants() {
     public static final double k_DRIVE_FF_V = 0;
     public static final double DRIVE_DEADBAND_MPS = 0.01;
     public static final double DRIVE_MOTOR_RAMP = 0.1;
+    public static final double DRIVE_CURRENT_LIMIT = 200;
+
     // Steer Motor
     /**
      * The maximum velocity of the steer motor. <p> 
@@ -267,13 +269,15 @@ public static final class ShooterConstants{
   public static final double ALLOWED_ANGLE_ERROR = 1.5;
   public static final double ALLOWED_SPEED_ERROR = 4;
 
+  public static final double CURRENT_LIMIT = 200; //amps the motor is limited to
+
   public static final double AUTO_AIM_ROBOT_kP = 0.125;
   public static final double AUTO_AIM_ROBOT_kI = 0.00;
   public static final double AUTO_AIM_ROBOT_kD = 0.00;
 
   public static final double LONG_SHOOTING_RPS = 90;
-  public static final double SHORT_SHOOTING_RPS = 75;
-  public static final double AMP_RPS = 9.5;
+  public static final double SHORT_SHOOTING_RPS = 80;
+  public static final double AMP_RPS = 7.5;
   public static final double SUBWOOFER_RPS = SHORT_SHOOTING_RPS;
 
   //the PID values used on the PID loop on the motor controller that control the position of the shooter angle
@@ -293,7 +297,7 @@ public static final class ShooterConstants{
   public static final double DISTANCE_MULTIPLIER = 0.15;
   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 = 98;//still has to be found
+  public static final double MAXIMUM_SHOOTER_ANGLE = 104;//still has to be found
   public static final double HUMAN_PLAYER_ANGLE = 97;//still has to be found
   public static final double HUMAN_PLAYER_RPS = -10;//still has to be found
 
@@ -329,9 +333,11 @@ public static final class ClimberConstants{
 }
 public static final class IndexerConstants{
   public static final int INDEXER_MOTOR = 11;
-  public static final double INDEXER_INTAKE_SPEED = -0.25;//should be 0.5 TODO change to positive
+  public static final int CURRENT_LIMIT = 50;
+  public static final double INDEXER_INTAKE_SPEED = 0.5;//should be 0.5 TODO change to positive
   public static final double HUMAN_PLAYER_INDEXER_SPEED = -0.5;//should be 0.5 TODO change to positive
   public static final double INDEXER_SHOOTING_SPEED = 1;
+  public static final double INDEXER_AMP_SPEED = 0.4;
 }
 public static final class IntakeConstants{
   public static final int INTAKE_1_MOTOR = 20;
@@ -380,6 +386,8 @@ public CTREConfigs() {
       driveMotorConfig.Voltage.PeakForwardVoltage = 12;
       driveMotorConfig.Voltage.PeakReverseVoltage = -12;
       driveMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+      driveMotorConfig.CurrentLimits.SupplyCurrentLimit = DriveConstants.DRIVE_CURRENT_LIMIT;
+      driveMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
 
       //  Steer encoder.
       steerEncoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
@@ -421,7 +429,7 @@ public final class Field{
     public static final double MAX_SHOOTING_DISTANCE = 2491;
     public static final double SHORT_RANGE_SHOOTING_DIST = 3;
 
-    public static final double AMPLIFIER_ANGLE = 97;
+    public static final double AMPLIFIER_ANGLE = 101;
     public static final double SUBWOOFER_ANGLE = 80;
     //angle at 60 for bounce techinque, didn't work
   }
@@ -430,7 +438,9 @@ public final class Vision{
   public static final String APRILTAG_LIMELIGHT2_NAME = "limelight-aprill";
   public static final String APRILTAG_LIMELIGHT3_NAME = "limelight-aprilr";
   public static final String OBJ_DETECITON_LIMELIGHT_NAME = "limelight-neural";
-
+
+  public static final double APRILTAG_CLOSENESS = 0.5;
+  public static final double MAX_TAG_DISTANCE = 3.05;
 
   public static final Translation2d fieldCorner = new Translation2d(16.54, 8.02);
 

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

@@ -35,7 +35,7 @@ public Pose2d getbotPose(){
          * @return true if the most recent vision pose estimation is inside the field and is close enough to the provided pose.
          */
         public boolean isPoseTrustworthy(Pose2d robotPose){
-            Pose2d poseEstimate = this.getbotPose();
+            Pose2d poseEstimate = this.botPoseBlue;
             if ((poseEstimate.getX()<Vision.fieldCorner.getX() && poseEstimate.getY()<Vision.fieldCorner.getY()) //Don't trust estimations that are outside the field perimeter.
                 && robotPose.getTranslation().getDistance(poseEstimate.getTranslation()) < 0.5) //Dont trust pose estimations that are more than half a meter from current pose.
                 return true;