From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Sun, 1 Feb 2026 00:20:12 +0000 (-0800) Subject: fdsaf X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=dcdab323d67bcfe3662e9fcb74fc581d2138f41e;p=FRC2026.git fdsaf --- diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java index f2e5494..663b5fc 100644 --- a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java +++ b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java @@ -3,15 +3,20 @@ package frc.robot.commands.gpm; import java.lang.reflect.Field; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Unit; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Robot; +import frc.robot.constants.Constants; import frc.robot.constants.FieldConstants; import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.turret.ShotInterpolation; import frc.robot.subsystems.turret.Turret; import frc.robot.util.FieldZone; import frc.robot.util.ShootingTarget; @@ -21,6 +26,7 @@ public class SimpleAutoShoot extends Command { private Turret turret; private Drivetrain drivetrain; private TurretVision turretVision; + private Shooter shooter; private double fieldAngleRad; private double turretSetpoint; @@ -31,9 +37,15 @@ public class SimpleAutoShoot extends Command { private boolean turretVisionEnabled = false; private boolean SOTM = true; private Translation2d drivepose; - public SimpleAutoShoot(Turret turret, Drivetrain drivetrain) { + + private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); + private double lastTurretAngle = 0; + private double lastTurretVelocity = 0; + + public SimpleAutoShoot(Turret turret, Drivetrain drivetrain, Shooter shooter) { this.turret = turret; this.drivetrain = drivetrain; + this.shooter = shooter; drivepose = drivetrain.getPose().getTranslation(); addRequirements(turret); @@ -47,7 +59,7 @@ public class SimpleAutoShoot extends Command { double D_y; double D_x; // TODO: Change time to goal on actual comp bot - double timeToGoal = 1.0; + double timeToGoal = 0.0; // If the robot is moving, adjust the target position based on velocity if (SOTM) { @@ -107,12 +119,24 @@ public class SimpleAutoShoot extends Command { @Override public void execute() { + //shooter.setShooterPower(ShotInterpolation.shooterPowerMap.get(FieldConstants.getHubTranslation().toTranslation2d().getDistance(drivetrain.getPose().getTranslation()))); // Continuously update setpoints and adjust based on vision if available drivepose = drivetrain.getPose().getTranslation(); updateTurretSetpoint(drivepose); updateYawToTag(); - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2) * 1.0); + // double turretVelocity = + // turretAngleFilter.calculate( + // new Rotation2d(Units.degreesToRadians(turretSetpoint)).minus(new Rotation2d(Units.degreesToRadians(lastTurretAngle))).getRadians() / Constants.LOOP_TIME); + + double turretAcceleration = ((-drivetrain.getAngularRate(2)) - (lastTurretVelocity)) / Constants.LOOP_TIME; + + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2)); + // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), (-drivetrain.getAngularRate(2)) + turretAcceleration * 0.3); + //turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity); + + lastTurretAngle = turretSetpoint; + lastTurretVelocity = -drivetrain.getAngularRate(2); } @Override diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 5fe646b..f7fba67 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -85,7 +85,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { if (simpleTurretAutoShoot != null && simpleTurretAutoShoot.isScheduled()){ simpleTurretAutoShoot.cancel(); } else{ - simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain()); + simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain(), shooter); CommandScheduler.getInstance().schedule(simpleTurretAutoShoot); } }) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index e77f3d2..492b2fe 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -24,6 +24,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.FieldConstants; @@ -186,6 +187,7 @@ public class Drivetrain extends SubsystemBase { @Override public void periodic() { + SmartDashboard.putNumber("Shot Distance", FieldConstants.getHubTranslation().toTranslation2d().getDistance(getPose().getTranslation())); odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 2247f83..9f5e340 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -30,6 +30,8 @@ public class Shooter extends SubsystemBase implements ShooterIO { // Goal Velocity / Double theCircumfrence private double shooterTargetSpeed = 0; private double feederPower = 0; + + public double shooterPower = 1.0; //Velocity in rotations per second VelocityVoltage voltageRequest = new VelocityVoltage(0); @@ -67,11 +69,13 @@ public class Shooter extends SubsystemBase implements ShooterIO { public void periodic(){ updateInputs(); + SmartDashboard.putNumber("Shot Power", shooterPower); + shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower); shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed)); shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed)); - shooterMotorLeft.set(-1); - shooterMotorRight.set(-1); + // shooterMotorLeft.set(-shooterPower); + // shooterMotorRight.set(-shooterPower); feederMotor.set(feederPower); } @@ -85,13 +89,15 @@ public class Shooter extends SubsystemBase implements ShooterIO { feederPower = power; } - public void setShooter(double linearVelocityMps) { - double wheelCircumference = Math.PI * ShooterConstants.SHOOTER_LAUNCH_DIAMETER; - System.out.println("PRINTING WHEEEEEEEEEEEEL CIRUM:" + wheelCircumference); - shooterTargetSpeed = linearVelocityMps * ShooterConstants.SHOOTER_GEAR_RATIO / wheelCircumference; // rps + public void setShooter(double velocityRPS) { + shooterTargetSpeed = velocityRPS; System.out.println("Shooter is working"); } + public void setShooterPower(double power){ + this.shooterPower = power; + } + public double getFeederVelocity() { return inputs.feederVelocity; } diff --git a/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java b/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java index bb1968a..7a5e300 100644 --- a/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java +++ b/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java @@ -1,13 +1,17 @@ package frc.robot.subsystems.turret; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; public class ShotInterpolation { public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap(); - public static Object timeOfFlight; + public static final InterpolatingDoubleTreeMap shooterPowerMap = new InterpolatingDoubleTreeMap(); static{ timeOfFlightMap.put(0.0, 0.67); timeOfFlightMap.put(1.0, 0.67); + + shooterPowerMap.put(0.0, 1.0); + shooterPowerMap.put(1.0, 1.0); } } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 499b1c6..2fa85cc 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -4,7 +4,9 @@ import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; @@ -47,6 +49,7 @@ public class Turret extends SubsystemBase { private static final PIDController velocityPid = new PIDController(15, 0, 0.25); + private double lastFrameVelocity = 0; /* ---------------- Hardware ---------------- */ @@ -73,9 +76,9 @@ public class Turret extends SubsystemBase { /* ---------------- Gains ---------------- */ - // private static final double kP = 3500.0; + private static final double kP = 15.0; - // private static final double kD = 150.0; + private static final double kD = 0.2; /* ---------------- Visualization ---------------- */ @@ -85,6 +88,7 @@ public class Turret extends SubsystemBase { // private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0.010); private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0); + private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0); /* ---------------- Constructor ---------------- */ @@ -92,12 +96,16 @@ public class Turret extends SubsystemBase { public Turret() { motor.setNeutralMode(NeutralModeValue.Coast); - // motor.getConfigurator().apply( - // new Slot0Configs() - // .withKP(kP) - // .withKD(kD) - // .withKV(1) - // .withKA(1)); + motor.getConfigurator().apply( + new Slot0Configs() + .withKP(kP) + .withKD(kD)); + + TalonFXConfiguration config = new TalonFXConfiguration(); + var motionMagicConfigs = config.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = 10 * GEAR_RATIO; + motionMagicConfigs.MotionMagicAcceleration = 50 * GEAR_RATIO; + motor.getConfigurator().apply(config); motor.getConfigurator().apply( new com.ctre.phoenix6.configs.TalonFXConfiguration() { @@ -106,21 +114,6 @@ public class Turret extends SubsystemBase { } }); - // motor.getConfigurator().apply( - // new TalonFXConfiguration() { - // { - // Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; - // } - // }); - - // var talonFXConfigs = new TalonFXConfiguration(); - // var motionMagicConfigs = talonFXConfigs.MotionMagic; - // motionMagicConfigs.MotionMagicCruiseVelocity = 10.0; - // motionMagicConfigs.MotionMagicAcceleration = 50.0; - // motor.getConfigurator().apply(talonFXConfigs); - - - setpoint = new State(getPositionRad(), 0.0); lastGoalRad = setpoint.position; @@ -203,8 +196,11 @@ public class Turret extends SubsystemBase { // double voltage = feedForward.calculateWithVelocities(lastFrameVelocity, targetVelocity); double voltage = feedForward.calculate(targetVelocity); lastFrameVelocity = targetVelocity; - - motor.setVoltage(voltage); + if(Math.abs(setpoint.position * GEAR_RATIO - Units.rotationsToRadians(motor.getPosition().getValueAsDouble())) > Math.PI/2){ + motor.setControl(mmVoltageRequest.withPosition(Units.radiansToRotations(setpoint.position * GEAR_RATIO))); + } else{ + motor.setVoltage(voltage); + } // var request = velocityRequest.withVelocity(Units.radiansToRotations(targetVelocity)).withEnableFOC(false); Logger.recordOutput("Turret Voltage", motor.getMotorVoltage().getValue());