From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Mon, 16 Feb 2026 20:03:15 +0000 (-0800) Subject: Merge branch 'arnavs-mega-velocityvoltage-turret' into beta-bot X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=ae21e1f74e22818c6c74568da58b5e09ac92f439;p=FRC2026.git Merge branch 'arnavs-mega-velocityvoltage-turret' into beta-bot --- ae21e1f74e22818c6c74568da58b5e09ac92f439 diff --cc src/main/java/frc/robot/RobotContainer.java index 10600ef,377285c..dee69e0 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@@ -7,9 -7,12 +7,10 @@@ import org.json.simple.parser.ParseExce import org.littletonrobotics.junction.Logger; import com.pathplanner.lib.auto.AutoBuilder; + import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.livewindow.LiveWindow; diff --cc src/main/java/frc/robot/subsystems/shooter/Shooter.java index d12c69d,38b55a5..d0553c4 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@@ -10,14 -12,9 +12,15 @@@ import com.ctre.phoenix6.hardware.Talon import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import au.grapplerobotics.ConfigurationFailedException; +import au.grapplerobotics.LaserCan; +import au.grapplerobotics.interfaces.LaserCanInterface.Measurement; +import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; +import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest; +import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; + import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@@ -31,27 -32,50 +34,49 @@@ public class Shooter extends SubsystemB // Goal Velocity / Double theCircumfrence private double shooterTargetSpeed = 0; - private double feederPower = 0; - public double shooterPower = 0.5; - private double feederPower = 0; - + + public boolean shooterAtMaxSpeed = false; + public boolean ballDetected = false; //Velocity in rotations per second VelocityVoltage voltageRequest = new VelocityVoltage(0); - private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); + private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); + + double powerModifier; + // for tracking current phase: used to adjust the setting + private FlywheelPhase phase; + + private double torqueCurrentDebounceTime = 0.5; + + VelocityDutyCycle velocityDutyCycle = new VelocityDutyCycle(0); + TorqueCurrentFOC torqueCurrentFOC = new TorqueCurrentFOC(0); + + /* + * Debouncers take in certain statement. If it is false: it checks for how long (the first parameter) + * If it is long enough then return True. + */ + private Debouncer torqueCurrentDebouncer = new Debouncer(torqueCurrentDebounceTime, DebounceType.kFalling); + + @AutoLogOutput private long launchCount = 0; + private boolean lastTorqueCurrentControl = false; + + public enum FlywheelPhase { + BANG_BANG, + CONSTANT_TORQUE, + START_UP + } + public Shooter(){ - TalonFXConfiguration config = new TalonFXConfiguration(); - config.Slot0.kP = ShooterConstants.kP; //tune p value - config.Slot0.kI = ShooterConstants.kI; - config.Slot0.kD = ShooterConstants.kD; - config.Slot0.kV = ShooterConstants.kV; //Maximum rps = 100 --> 12V/100rps - - config.TorqueCurrent.PeakReverseTorqueCurrent = ShooterConstants.SHOOTER_MAX_TORQUE_CURRENT; // we are making this a BANG BANG controller for talon fx - config.MotorOutput.PeakForwardDutyCycle = ShooterConstants.MOTOR_BANG_BANG_MAX; // bang bang up - // tune this - config.MotorOutput.PeakReverseDutyCycle = ShooterConstants.MOTOR_BANG_BANG_MIN; // bang bang down + updateInputs(); + + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Slot0.kP = 0.15; //tune p value + config.Slot0.kI = 0; + config.Slot0.kD = 0.0; + config.Slot0.kV = 0.125; //Maximum rps = 100 --> 12V/100rps shooterMotorLeft.getConfigurator().apply(config); shooterMotorRight.getConfigurator().apply(config); diff --cc src/main/java/frc/robot/subsystems/turret/Turret.java index f856892,0954052..dcacc86 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@@ -2,16 -2,9 +2,13 @@@ package frc.robot.subsystems.turret import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +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.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; - import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.sim.TalonFXSimState; @@@ -50,22 -32,23 +48,23 @@@ public class Turret extends SubsystemBa /* ---------------- Constants ---------------- */ - private static final double MIN_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MIN_ANGLE); - private static final double MAX_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MAX_ANGLE); + 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 = 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_VEL_RAD_PER_SEC = 600; private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0; - private static final double VERSA_RATIO = 5.0; - private static final double TURRET_RATIO = 140.0 / 10.0; - private static final double GEAR_RATIO = VERSA_RATIO * TURRET_RATIO; + private static final double GEAR_RATIO = TurretConstants.TURRET_GEAR_RATIO; - private static final PIDController positionPID = new PIDController(15, 0, 0.25); - //private static final PIDController longVelocityPID = new PIDController(15, 0, 1.0); - private static final PIDController velocityPID = new PIDController(0.0, 0.0, 0.0); + private static final PIDController positionPID = new PIDController(15.0, 0, 0.0); + // private static final PIDController positionPID = new PIDController(15, 0, 0.25); + private static final PIDController velocityPID = new PIDController(0.2, 0.0, 0.0); + private final CANcoder encoderLeft = new CANcoder(0, Constants.SUBSYSTEM_CANIVORE_CAN); + private final CANcoder encoderRight = new CANcoder(1, Constants.SUBSYSTEM_CANIVORE_CAN); + private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02 + , 0.02); + private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); @@@ -106,9 -93,10 +109,9 @@@ private final MechanismLigament2d ligament = root.append(new MechanismLigament2d("barrel", 30, 0)); // 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.8, 0); + private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt * 2.0, 0); private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0); - /* ---------------- Constructor ---------------- */ public Turret() { @@@ -245,18 -235,17 +274,26 @@@ lastFrameVelocity = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()); - // var request = velocityRequest.withVelocity(Units.radiansToRotations(targetVelocity)).withEnableFOC(false); Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue()); - Logger.recordOutput("Turret/velocitySetpoint", targetVelocity / GEAR_RATIO); + Logger.recordOutput("Turret/setpointDeg", Units.radiansToDegrees(setpoint.position)); + + // --- Visualization --- + ligament.setAngle(Units.radiansToDegrees(getPositionRad())); + + updateInputs(); + Logger.processInputs("Turret", inputs); + + // SmartDashboard.putNumber("Encoder Left", encoderLeft.get()); + // SmartDashboard.putNumber("Encoder Right", encoderRight.get()); + Logger.recordOutput("Turret/setpointDeg", Units.radiansToDegrees(motorGoalRotations) / GEAR_RATIO); + //Logger.recordOutput("Turret/velocitySetpoint", targetVelocity / GEAR_RATIO); + + // --- Position + velocity feedforward (MA-style) --- + // motor.setControl(request); + // motor.clearStickyFaults(); + + // --- Visualization --- + ligament.setAngle(Units.radiansToDegrees(getPositionRad())); } /* ---------------- Simulation ---------------- */ diff --cc src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 10fee61,992172b..b5a1ac0 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@@ -1,28 -1,28 +1,32 @@@ package frc.robot.subsystems.turret; + import edu.wpi.first.math.geometry.Rotation3d; + import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; + import frc.robot.constants.Constants; + import frc.robot.constants.swerve.DriveConstants; public class TurretConstants { - public static double MAX_ANGLE = 180; - public static double MIN_ANGLE = -180; + public static double MAX_ANGLE = 200; + public static double MIN_ANGLE = -200; public static double MAX_VELOCITY = 10000000; // m/s public static double MAX_ACCELERATION = 10000000; // m/s^2 - public static double TURRET_WIDTH = Units.feetToMeters(1.0); + public static double TURRET_WIDTH = Units.inchesToMeters(6.4); public static double TURRET_RADIUS = TURRET_WIDTH / 2; - public static double ROTATIONAL_VELOCITY_CONSTANT = 0.2; + public static double TURRET_TEETH_COUNT = 140.0; // the turret teeth count + public static double TURRET_GEAR_RATIO = 36.81818182; + public static int LEFT_ENCODER_TEETH = 15; // gear teeth + public static int RIGHT_ENCODER_TEETH = 22; // read above + public static int ENCODER_COUNT_TOTAL = 8192; // how many intervals it can have, like clicks on a clock chat gpt explained to me - public static Translation2d DISTANCE_FROM_ROBOT_CENTER = new Translation2d(0, 0); + public static double LEFT_ENCODER_OFFSET = 9.52; // degrees 9.52 rot + public static double RIGHT_ENCODER_OFFSET = 6.53; // degrees 6.53 rot - Translation3d TURRET_TO_CENTER_OF_ROBOT = new Translation3d(Units.inchesToMeters(4.875), -Units.inchesToMeters(26.5/2 - 11.375), 0.32); - Translation3d CAMERA_TO_CENTER_OF_TURRET = new Translation3d(0.177, Units.inchesToMeters(1.0/8.0), 0.325); - Rotation3d CAMERA_ROTATION = new Rotation3d(Units.degreesToRadians(-10), Units.degreesToRadians(-13), Units.degreesToRadians(-25)); + public static Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters + public static double CRT_TOLERANCE = 0.01; - } + }