From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Fri, 27 Feb 2026 18:47:37 +0000 (-0800) Subject: a X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=e7ba43e551d1c41ee649137f44a36a4cd4361623;p=FRC2026.git a --- diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 628716a..8f8b015 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -145,7 +145,7 @@ public class RobotContainer { e.printStackTrace(); } if(turret != null){ - turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer)); + //turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer)); } drive.setDefaultCommand(new DefaultDriveCommand(drive, driver)); break; diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index dfb0ba9..d896b0d 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -62,10 +62,12 @@ public class Superstructure extends Command { private final double phaseDelay = 0.03; // Extrapolation delay due to latency - private Translation2d target = null; + private Translation2d target = FieldConstants.getHubTranslation().toTranslation2d(); private PhaseManager phaseManager = new PhaseManager(); + private double hoodOffset = 0.0; + public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) { this.turret = turret; this.drivetrain = drivetrain; @@ -143,6 +145,8 @@ public class Superstructure extends Command { lastTurretAngle = turretAngle; + Logger.recordOutput("Turret/Target Pose", target); + Logger.recordOutput("Lookahead Pose", lookaheadPose); // Subtract the rotational angle of the robot from the setpoint @@ -200,9 +204,11 @@ public class Superstructure extends Command { @Override public void execute() { + hoodOffset = SmartDashboard.getNumber("Hood Offset", hoodOffset); + SmartDashboard.putNumber("Hood Offset", hoodOffset); // Phase manager stuff phaseManager.update(drivepose, shooter, turret); - target = phaseManager.getTarget(); + //target = phaseManager.getTarget(); updateDrivePose(); updateSetpoints(drivepose); @@ -213,12 +219,12 @@ public class Superstructure extends Command { turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2)); if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){ - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE - hoodOffset), 0.0); } else{ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity); } - shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); + shooter.setShooter(-ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); // if (phaseManager.shouldFeed()) { // spindexer.maxSpindexer(); diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index dd5e171..64825eb 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -16,6 +16,7 @@ import frc.robot.commands.gpm.AutoShootCommand; import frc.robot.commands.gpm.ClimbDriveCommand; import frc.robot.commands.gpm.IntakeMovementCommand; import frc.robot.commands.gpm.ReverseMotors; +import frc.robot.commands.gpm.Superstructure; import frc.robot.constants.Constants; import frc.robot.subsystems.Climb.LinearClimb; import frc.robot.subsystems.drivetrain.Drivetrain; @@ -146,7 +147,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { // Auto shoot if (turret != null && hood != null && shooter != null) { - autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer); + autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer); controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot); } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index d48f879..1dbf65f 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -72,10 +72,10 @@ public class Turret extends SubsystemBase implements TurretIO{ TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.Slot0.kP = 12.0; + config.Slot0.kP = 10.0; config.Slot0.kS = 0.1; // Static friction compensation config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio - config.Slot0.kD = 0.15; // The "Braking" term to stop overshoot + config.Slot0.kD = 0.20; // The "Braking" term to stop overshoot var mm = config.MotionMagic; mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO; @@ -122,7 +122,7 @@ public class Turret extends SubsystemBase implements TurretIO{ //Sets the initial motor position motor.setPosition(motorRotations); - motor.setPosition(0.0); + motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO); SmartDashboard.putData("Turn to 0", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0);})); SmartDashboard.putData("Turn to -90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0);})); @@ -256,7 +256,7 @@ public class Turret extends SubsystemBase implements TurretIO{ @Override public void updateInputs() { - // inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO; + inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO; inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / TurretConstants.GEAR_RATIO; inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); inputs.encoderLeftRot = wrapUnit(encoderLeft.getAbsolutePosition().getValueAsDouble());