--- /dev/null
+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
--- /dev/null
+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();
+ }
+}