From 48a2e0299cc5a6163b712f45ed0e000eeeba3584 Mon Sep 17 00:00:00 2001 From: mixxlto Date: Thu, 5 Feb 2026 16:22:34 -0800 Subject: [PATCH] hood --- .../robot/commands/gpm/AutoShootCommand.java | 143 ++++++++++++++++++ .../java/frc/robot/constants/IdConstants.java | 3 + .../java/frc/robot/subsystems/hood/Hood.java | 118 +++++++++++++++ .../robot/subsystems/hood/HoodConstants.java | 8 + .../frc/robot/subsystems/hood/HoodIO.java | 14 ++ .../subsystems/turret/ShotInterpolation.java | 6 +- 6 files changed, 291 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/gpm/AutoShootCommand.java create mode 100644 src/main/java/frc/robot/subsystems/hood/Hood.java create mode 100644 src/main/java/frc/robot/subsystems/hood/HoodConstants.java create mode 100644 src/main/java/frc/robot/subsystems/hood/HoodIO.java diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java new file mode 100644 index 0000000..4e83ba1 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -0,0 +1,143 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; +import frc.robot.constants.FieldConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.hood.HoodConstants; +import frc.robot.subsystems.turret.ShotInterpolation; +import frc.robot.subsystems.turret.Turret; +import frc.robot.subsystems.turret.TurretConstants; + +public class AutoShootCommand extends Command { + private Turret turret; + private Drivetrain drivetrain; + private Hood hood; + + private double turretSetpoint; + private double hoodSetpoint; + + private Pose2d drivepose; + + private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); + private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); + + private Rotation2d lastTurretAngle; + private Rotation2d turretAngle; + private double turretVelocity; + + private double lastHoodAngle; + private double hoodAngle; + private double hoodVelocity; + + private final double phaseDelay = 0.03; + + public AutoShootCommand(Turret turret, Drivetrain drivetrain, Hood hood) { + this.turret = turret; + this.drivetrain = drivetrain; + this.hood = hood; + drivepose = drivetrain.getPose(); + + addRequirements(turret, hood); + } + + public void updateSetpoints(Pose2d drivepose) { + + Translation2d target = FieldConstants.getHubTranslation().toTranslation2d(); + Pose2d turretPosition = drivepose.transformBy(new Transform2d((TurretConstants.DISTANCE_FROM_ROBOT_CENTER), new Rotation2d())); + double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); + + // If the robot is moving, adjust the target position based on velocity + ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); + ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); + + double turretVelocityX = + fieldRelVel.vxMetersPerSecond + + fieldRelVel.omegaRadiansPerSecond + * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.cos(drivepose.getRotation().getRadians()) + - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.sin(drivepose.getRotation().getRadians())); + double turretVelocityY = + fieldRelVel.vyMetersPerSecond + + fieldRelVel.omegaRadiansPerSecond + * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.cos(drivepose.getRotation().getRadians()) + - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.sin(drivepose.getRotation().getRadians())); + + // Account for imparted velocity by robot (turret) to offset + double timeOfFlight; + Pose2d lookaheadPose = turretPosition; + double lookaheadTurretToTargetDistance = turretToTargetDistance; + + // Loop (20) until lookahreadTurretToTargetDistance converges + for (int i = 0; i < 20; i++) { + timeOfFlight = ShotInterpolation.timeOfFlightMap.get(lookaheadTurretToTargetDistance); + double offsetX = turretVelocityX * timeOfFlight; + double offsetY = turretVelocityY * timeOfFlight; + lookaheadPose = + new Pose2d( + turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), + turretPosition.getRotation()); + lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); + } + turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); + if (lastTurretAngle == null) { + lastTurretAngle = turretAngle; + } + turretVelocity = + turretAngleFilter.calculate( + turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME); + lastTurretAngle = turretAngle; + + // Add 180 since drivetrain is backwards + double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() + Math.PI); + turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint), -180.0, 180.0); + + // Hood stuff + hoodAngle = ShotInterpolation.hoodAngleMap.get(lookaheadTurretToTargetDistance); + hoodSetpoint = MathUtil.clamp(hoodAngle, Units.radiansToDegrees(HoodConstants.MIN_ANGLE), Units.radiansToDegrees(HoodConstants.MAX_ANGLE)); + hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME); + lastHoodAngle = hoodAngle; + } + + public void updateDrivePose(){ + ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); + drivepose = drivetrain.getPose().exp( + new Twist2d( + robotRelVel.vxMetersPerSecond * phaseDelay, + robotRelVel.vyMetersPerSecond * phaseDelay, + robotRelVel.omegaRadiansPerSecond * phaseDelay)); + } + + @Override + public void initialize() { + updateDrivePose(); + updateSetpoints(drivepose); + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2)); + hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity); + } + + @Override + public void execute() { + updateDrivePose(); + updateSetpoints(drivepose); + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2)); + hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity); + } + + @Override + 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); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java index b9fa6db..d7a0259 100644 --- a/src/main/java/frc/robot/constants/IdConstants.java +++ b/src/main/java/frc/robot/constants/IdConstants.java @@ -30,4 +30,7 @@ public class IdConstants { // Climb public static final int CLIMB_MOTOR_LEFT = 48; public static final int CLIMB_MOTOR_RIGHT = 49; + + // Hood + public static final int HOOD_ID = 50; } diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java new file mode 100644 index 0000000..157379a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -0,0 +1,118 @@ +package frc.robot.subsystems.hood; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; + +public class Hood extends SubsystemBase implements HoodIO{ + private TalonFX motor = new TalonFX(IdConstants.HOOD_ID); + + private double MIN_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MIN_ANGLE); + private double MAX_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MAX_ANGLE); + + private double MAX_VEL_RAD_PER_SEC = 25; + private double MAX_ACCEL_RAD_PER_SEC2 = 160.0; + + private double GEAR_RATIO = HoodConstants.GEAR_RATIO; + + private static final PIDController positionPID = new PIDController(15, 0, 0.25); + + private final TrapezoidProfile profile = new TrapezoidProfile( + new TrapezoidProfile.Constraints( + MAX_VEL_RAD_PER_SEC, + MAX_ACCEL_RAD_PER_SEC2)); + private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0); + + private State setpoint = new State(); + + private Rotation2d goalAngle = Rotation2d.kZero; + private double goalVelocityRadPerSec = 0.0; + + private static final double kP = 15.0; + private static final double kD = 0.2; + + private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged(); + + public Hood(){ + motor.setNeutralMode(NeutralModeValue.Coast); + + motor.getConfigurator().apply( + new Slot0Configs() + .withKP(kP) + .withKD(kD)); + + TalonFXConfiguration config = new TalonFXConfiguration(); + + motor.getConfigurator().apply( + new com.ctre.phoenix6.configs.TalonFXConfiguration() { + { + MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + } + }); + + setpoint = new State(getPositionRad(), 0.0); + } + + public double getPositionRad(){ + return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()); + } + + public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) { + goalAngle = angle; + goalVelocityRadPerSec = velocityRadPerSec; + } + + @Override + public void periodic() { + State goalState = new State( + MathUtil.clamp(goalAngle.getRadians(), MIN_ANGLE_RAD, MAX_ANGLE_RAD), + goalVelocityRadPerSec); + + setpoint = profile.calculate( + Constants.LOOP_TIME, + setpoint, + goalState); + + double motorVelRotPerSec = Units.radiansToRotations(setpoint.velocity) * GEAR_RATIO; + + double targetVelocity; + + double motorSetpointPosition = (setpoint.position) * GEAR_RATIO; + + targetVelocity = positionPID.calculate( + motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians), + motorSetpointPosition); + + targetVelocity += Units.rotationsToRadians(motorVelRotPerSec); + + double voltage = feedForward.calculate(targetVelocity); + + motor.setVoltage(voltage); + + Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue()); + Logger.recordOutput("Hood/velocitySetpoint", targetVelocity / GEAR_RATIO); + } + + @Override + public void updateInputs() { + inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO; + inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / GEAR_RATIO; + inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java new file mode 100644 index 0000000..b2d4938 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java @@ -0,0 +1,8 @@ +package frc.robot.subsystems.hood; + +public class HoodConstants { + public static final double MIN_ANGLE = 60.0; + public static final double MAX_ANGLE = 90.0; + + public static final double GEAR_RATIO = 70.0; +} diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/hood/HoodIO.java new file mode 100644 index 0000000..42886b5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hood/HoodIO.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.hood; + +import org.littletonrobotics.junction.AutoLog; + +public interface HoodIO { + @AutoLog + public static class HoodIOInputs{ + public double positionDeg = HoodConstants.MAX_ANGLE; + public double velocityRadPerSec = 0.0; + public double motorCurrent = 0.0; + } + + public void updateInputs(); +} diff --git a/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java b/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java index 7a5e300..c265cec 100644 --- a/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java +++ b/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java @@ -1,11 +1,12 @@ package frc.robot.subsystems.turret; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.math.interpolation.InterpolatingTreeMap; +import edu.wpi.first.math.util.Units; public class ShotInterpolation { public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap(); public static final InterpolatingDoubleTreeMap shooterPowerMap = new InterpolatingDoubleTreeMap(); + public static final InterpolatingDoubleTreeMap hoodAngleMap = new InterpolatingDoubleTreeMap(); static{ timeOfFlightMap.put(0.0, 0.67); @@ -13,5 +14,8 @@ public class ShotInterpolation { shooterPowerMap.put(0.0, 1.0); shooterPowerMap.put(1.0, 1.0); + + hoodAngleMap.put(0.0, Units.degreesToRadians(90)); + hoodAngleMap.put(1.0, Units.degreesToRadians(90)); } } -- 2.39.5