a

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d6400f26c9bd5e7cf267cf35b6cc8b83cd79c73e..6dbc0656700f78d991480e6db38c5e1f278459fd 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -86,8 +86,8 @@ public class RobotContainer {
 
       case WaffleHouse: // AKA Betabot
         turret = new Turret();
-        shooter = new Shooter();
-        hood = new Hood();
+        //shooter = new Shooter();
+        //hood = new Hood();
 
       case SwerveCompetition: // AKA "Vantage"
 

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index db0800538449fe51f0f7a5f8492bf136f4d373ea..6117761d4de2742d37b960c5cba2194c6ae52035 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -97,7 +97,7 @@ public class AutoShootCommand extends Command {
                                FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
                                8.0); // Random initial goalState to prevent it being null
         
-        addRequirements(turret, hood, shooter);
+        addRequirements(turret);
     }
 
     public void updateSetpoints(Pose2d drivepose) {
@@ -134,7 +134,7 @@ public class AutoShootCommand extends Command {
             Translation3d target3d = new Translation3d(target.getX(), target.getY(), FieldConstants.getHubTranslation().getZ()); // Add if statement so that it's only when it's shooting
             goalState = ShooterPhysics.getShotParams(
                                target3d.minus(lookahead3d),
-                               8.0);
+                               2.0);
 
             timeOfFlight = goalState.timeOfFlight();
             double offsetX = turretVelocityX * timeOfFlight;
@@ -198,12 +198,13 @@ public class AutoShootCommand extends Command {
     public void execute() {
         updateDrivePose();
         updateSetpoints(drivepose);
-        //turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
+        turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
         //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity);
         //shooter.setShooter(goalState.exitVel());
 
         SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint);
         SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint);
+        SmartDashboard.putNumber("Shooter Calculate Velocity", goalState.exitVel());
         System.out.println("COMMAND IS WORKINNGGG");
 
         /** Spindexer Stuff!! */
@@ -216,7 +217,8 @@ public class AutoShootCommand extends Command {
     public void end(boolean interrupted) {
         // Set the turret to a safe position when the command ends
         turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
-        hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0);
+        //shooter.setShooter(0.0);
+        //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0);
         if(spindexer != null){
             spindexer.stopSpindexer();
         }

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
index d2ad56663846bc970f46b84c04c07c12bb6a2ec7..33cdcff83e515b3640e62f6e402236bc00d3d4b6 100644 (file)
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -31,7 +31,7 @@ public class FieldConstants {
 
   /** Location of hub target */
   public static final Translation3d HUB_BLUE =
-      new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035 + .67, Units.inchesToMeters(72));
+      new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035, Units.inchesToMeters(72));
   
   public static final Translation3d HUB_RED =
       new Translation3d(FIELD_LENGTH - Units.inchesToMeters(156.8 + 20), 4.035 + .67, Units.inchesToMeters(72));

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index c707453cc4ca610e04f1b5fa8f19dee0eab5924e..e0631aa4444c44b3d0494661a24f6956f155ae81 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -14,6 +14,7 @@ import frc.robot.commands.gpm.AutoShootCommand;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterConstants;
 import frc.robot.subsystems.spindexer.Spindexer;
 import frc.robot.subsystems.turret.Turret;
 import frc.robot.subsystems.hood.Hood;
@@ -86,7 +87,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         }
 
         // Auto shoot
-        if(turret != null && hood != null){
+        if(turret != null){
             driver.get(PS5Button.SQUARE).onTrue(
             new InstantCommand(()->{
                         if (autoShoot != null && autoShoot.isScheduled()){
@@ -96,9 +97,10 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                             CommandScheduler.getInstance().schedule(autoShoot);
                         }
                     })
-        );
+            );
         }
 
+
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 1af09981b40be948c4044632a5951979d20da27a..defdefec6b4cf993ca30aa4995cf6e0f057ac44f 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -48,7 +48,7 @@ public class Shooter extends SubsystemBase implements ShooterIO {
 
     public Shooter(){
         TalonFXConfiguration config = new TalonFXConfiguration();
-        config.Slot0.kP = 0.15; 
+        config.Slot0.kP = 2.0; 
         config.Slot0.kI = 0;
         config.Slot0.kD = 0.03;
         config.Slot0.kV = 0.20; 
@@ -114,7 +114,7 @@ public class Shooter extends SubsystemBase implements ShooterIO {
 
     public void updateInputs() {
         inputs.leftShooterVelocity = Units.rotationsToRadians(motorLeft.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
-        inputs.rightShooterVelocity =Units.rotationsToRadians(motorRight.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
+        inputs.rightShooterVelocity = Units.rotationsToRadians(motorRight.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
         inputs.leftShooterCurrent = motorLeft.getStatorCurrent().getValueAsDouble();
         inputs.rightShooterCurrent = motorRight.getStatorCurrent().getValueAsDouble();
     }

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 4d0d3712514f78e0687a0d877ec1bd84037a18c3..51783d70963325fde0f12885a3e218fa1c741153 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -52,9 +52,10 @@ public class Turret extends SubsystemBase implements TurretIO{
        private static final double MIN_ANGLE_RAD = Units.degreesToRadians(-180); // Change this later to the actual values
        private static final double MAX_ANGLE_RAD = Units.degreesToRadians(180);
 
-       private static final double MAX_VEL_RAD_PER_SEC = 25;
+       private static final double MAX_VEL_RAD_PER_SEC = 15;
        //private static final double MAX_VEL_RAD_PER_SEC = 3.0; // Starting super duper slow for now
-       private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
+       // private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
+       private static final double MAX_ACCEL_RAD_PER_SEC2 = 60.0;
 
        private static final double GEAR_RATIO = TurretConstants.TURRET_GEAR_RATIO;