From 80114f92ca558e61d9ffd437916539e6b7f94b51 Mon Sep 17 00:00:00 2001 From: WesleyWong-972 Date: Sat, 7 Feb 2026 16:28:59 -0800 Subject: [PATCH] n --- src/main/java/frc/robot/RobotContainer.java | 13 +- .../java/frc/robot/constants/IdConstants.java | 6 +- .../controls/PS5ControllerDriverConfig.java | 58 +++++---- .../java/frc/robot/subsystems/hood/Hood.java | 30 +++-- .../robot/subsystems/hood/HoodConstants.java | 37 +++++- .../frc/robot/subsystems/shooter/Shooter.java | 118 ++++++++++++------ .../subsystems/turret/TurretConstants.java | 7 +- 7 files changed, 182 insertions(+), 87 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9773ebd..54926c3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,6 +33,7 @@ import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.drivetrain.GyroIOPigeon2; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.turret.Turret; +import frc.robot.subsystems.hood.Hood; import frc.robot.util.PathGroupLoader; import frc.robot.util.Vision.DetectedObject; import frc.robot.util.Vision.Vision; @@ -52,6 +53,7 @@ public class RobotContainer { private Vision vision = null; private Turret turret = null; private Shooter shooter = null; + private Hood hood = null; private Climb climb = null; private Command auto = new DoNothing(); @@ -65,7 +67,7 @@ public class RobotContainer { * Different robots may have different subsystems. */ public RobotContainer(RobotId robotId) { - climb = new Climb(); + // climb = new Climb(); // dispatch on the robot switch (robotId) { case TestBed1: @@ -84,12 +86,11 @@ public class RobotContainer { case Vivace: case Phil: case Vertigo: - vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS); - turret = new Turret(); + // vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS); shooter = new Shooter(); - + hood = new Hood(); drive = new Drivetrain(vision, new GyroIOPigeon2()); - driver = new PS5ControllerDriverConfig(drive, shooter, turret); + driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood); operator = new Operator(drive); // Detected objects need access to the drivetrain @@ -123,7 +124,7 @@ public class RobotContainer { LiveWindow.setEnabled(false); SmartDashboard.putData("Shutdown Orange Pis", new ShutdownAllPis()); - SmartDashboard.putData("Aim at thingy", new AimAtPose(drive, turret, new Pose2d(FieldConstants.field.getTagPose(26).get().getTranslation().toTranslation2d(), Rotation2d.kZero))); + //SmartDashboard.putData("Aim at thingy", new AimAtPose(drive, turret, new Pose2d(FieldConstants.field.getTagPose(26).get().getTranslation().toTranslation2d(), Rotation2d.kZero))); } /** diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java index d7a0259..8d50b89 100644 --- a/src/main/java/frc/robot/constants/IdConstants.java +++ b/src/main/java/frc/robot/constants/IdConstants.java @@ -23,8 +23,8 @@ public class IdConstants { public static final int TURRET_MOTOR_ID = 20; // Shooter - public static final int SHOOTER_LEFT_ID = 22; - public static final int SHOOTER_RIGHT_ID = 23; + public static final int SHOOTER_LEFT_ID = 10; + public static final int SHOOTER_RIGHT_ID =4; public static final int FEEDER_ID = 21; // Climb @@ -32,5 +32,5 @@ public class IdConstants { public static final int CLIMB_MOTOR_RIGHT = 49; // Hood - public static final int HOOD_ID = 50; + public static final int HOOD_ID = 11; } diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 37cd9d3..3ac13db 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -23,6 +23,8 @@ import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.turret.Turret; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.hood.HoodConstants; import lib.controllers.PS5Controller; import lib.controllers.PS5Controller.PS5Axis; import lib.controllers.PS5Controller.PS5Button; @@ -35,16 +37,18 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { private final BooleanSupplier slowModeSupplier = ()->false; private Shooter shooter; private Turret turret; + private Hood hood; private Pose2d alignmentPose = null; private Command turretAutoShoot; private Command simpleTurretAutoShoot; private TurretJoyStickAim turretJoyStickAim; - public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret) { + public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood) { super(drive); this.shooter = shooter; this.turret = turret; + this.hood = hood; } public void configureControls() { @@ -80,32 +84,32 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { // })); //driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY))).onFalse(new InstantCommand(() -> shooter.setShooter(0))); - driver.get(PS5Button.SQUARE).onTrue( - new InstantCommand(()->{ - if (simpleTurretAutoShoot != null && simpleTurretAutoShoot.isScheduled()){ - simpleTurretAutoShoot.cancel(); - } else{ - simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain(), shooter); - CommandScheduler.getInstance().schedule(simpleTurretAutoShoot); - } - }) - ); - - driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> shooter.setFeeder(1))).onFalse( - new InstantCommand(()->{ - shooter.setFeeder(0); - }) - ); - - driver.get(PS5Button.CIRCLE).onTrue( - new InstantCommand(()->{ - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(180)), 0); - }) - ).onFalse( - new InstantCommand(()->{ - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(-170)), 0); - }) - ); + // driver.get(PS5Button.SQUARE).onTrue( + // new InstantCommand(()->{ + // if (simpleTurretAutoShoot != null && simpleTurretAutoShoot.isScheduled()){ + // simpleTurretAutoShoot.cancel(); + // } else{ + // simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain(), shooter); + // CommandScheduler.getInstance().schedule(simpleTurretAutoShoot); + // } + // }) + // ); + + + driver.get(PS5Button.SQUARE).onTrue(new InstantCommand(() -> hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0))); + driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0))); + driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0))); + + + // driver.get(PS5Button.CIRCLE).onTrue( + // new InstantCommand(()->{ + // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(180)), 0); + // }) + // ).onFalse( + // new InstantCommand(()->{ + // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(-170)), 0); + // }) + // ); // driver.get(PS5Button.CROSS).onTrue( // new InstantCommand(()->{ diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java index 157379a..53744dc 100644 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -2,6 +2,7 @@ package frc.robot.subsystems.hood; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; @@ -16,12 +17,14 @@ 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.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; 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 TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.SUBSYSTEM_CANIVORE_CAN); private double MIN_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MIN_ANGLE); private double MAX_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MAX_ANGLE); @@ -29,9 +32,9 @@ public class Hood extends SubsystemBase implements HoodIO{ 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 double GEAR_RATIO = HoodConstants.HOOD_GEAR_RATIO; - private static final PIDController positionPID = new PIDController(15, 0, 0.25); + private static final PIDController positionPID = new PIDController(6, 0, 0.2); private final TrapezoidProfile profile = new TrapezoidProfile( new TrapezoidProfile.Constraints( @@ -41,15 +44,16 @@ public class Hood extends SubsystemBase implements HoodIO{ private State setpoint = new State(); - private Rotation2d goalAngle = Rotation2d.kZero; + private Rotation2d goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)); private double goalVelocityRadPerSec = 0.0; - private static final double kP = 15.0; - private static final double kD = 0.2; + private static double kP = 2.0; + private static double kD = 0.2; private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged(); public Hood(){ + motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * GEAR_RATIO); motor.setNeutralMode(NeutralModeValue.Coast); motor.getConfigurator().apply( @@ -62,11 +66,15 @@ public class Hood extends SubsystemBase implements HoodIO{ motor.getConfigurator().apply( new com.ctre.phoenix6.configs.TalonFXConfiguration() { { - MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; } }); setpoint = new State(getPositionRad(), 0.0); + + SmartDashboard.putData("max", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0))); + SmartDashboard.putData("medium", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0))); + SmartDashboard.putData("min", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0))); } public double getPositionRad(){ @@ -80,6 +88,7 @@ public class Hood extends SubsystemBase implements HoodIO{ @Override public void periodic() { + updateInputs(); State goalState = new State( MathUtil.clamp(goalAngle.getRadians(), MIN_ANGLE_RAD, MAX_ANGLE_RAD), goalVelocityRadPerSec); @@ -107,7 +116,12 @@ public class Hood extends SubsystemBase implements HoodIO{ Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue()); Logger.recordOutput("Hood/velocitySetpoint", targetVelocity / GEAR_RATIO); - } + Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(setpoint.position) / GEAR_RATIO); + + SmartDashboard.putData("Hood PID", positionPID); + + SmartDashboard.putNumber("Turret Position Deg", Units.radiansToDegrees(getPositionRad()) / GEAR_RATIO); + } @Override public void updateInputs() { diff --git a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java index b2d4938..07928fe 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java @@ -1,8 +1,39 @@ package frc.robot.subsystems.hood; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; + public class HoodConstants { - public static final double MIN_ANGLE = 60.0; - public static final double MAX_ANGLE = 90.0; + public static final double HOOD_GEAR_RATIO = 64; + + public static final double MASS = 2.46; // kilograms + public static final double LENGTH = 0.138 * 2; // meters + public static final double MOI = 0.0489969498; // kg*m^2 <-- We got this on Onshape + + public static final double CENTER_OF_MASS_LENGTH = 0.138; // meters + + // public static final double MAX_VELOCITY = 5; // rad/s + public static final double MAX_VELOCITY = 0.30; // rad/s + public static final double MAX_ACCELERATION = 30; // rad/s^2 + + public static final double MAX_ANGLE = 82; // degrees + public static final double MIN_ANGLE = 58.5; // degrees + + public static final double START_ANGLE = 90-22.9; // degrees + + // Arena dimensions + public static final double TARGET_HEIGHT = 2.44; // meters + public static final double SHOOTER_HEIGHT = 0.51; // meters + + public static final Translation2d TRANSLATION_TARGET = new Translation2d(0, 0); + public static final Rotation2d ROTATION_TARGET_ANGLE = new Rotation2d(); + // Other + public static final double INITIAL_VELOCTIY = 14.9; // meters per second + + // Testing purposes + public static final double START_DISTANCE = 2; // meters - public static final double GEAR_RATIO = 70.0; + // Calibration Purposes + public static final double CURRENT_SPIKE_THRESHHOLD = 10.0; // amps } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9f5e340..6d7ab09 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,16 +1,20 @@ package frc.robot.subsystems.shooter; -import java.lang.annotation.Target; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; + +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.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,74 +24,116 @@ import frc.robot.subsystems.shooter.ShooterIO.ShooterIOInputs; public class Shooter extends SubsystemBase implements ShooterIO { - private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.RIO_CAN); - private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.RIO_CAN); - - private TalonFX feederMotor = new TalonFX(IdConstants.FEEDER_ID, Constants.RIO_CAN); + private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.SUBSYSTEM_CANIVORE_CAN); + private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.SUBSYSTEM_CANIVORE_CAN); //rotations/sec // Goal Velocity / Double theCircumfrence private double shooterTargetSpeed = 0; - private double feederPower = 0; - public double shooterPower = 1.0; + public double shooterPower = 0.5; //Velocity in rotations per second VelocityVoltage voltageRequest = new VelocityVoltage(0); private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); + // for tracking current phase: used to adjust the setting + private FlywheelPhase phase; + + private double torqueCurrentControlTolerance = 20.0; + private double torqueCurrentDebounceTime = 0.025; + private double atGoalStateDebounceTime = 0.2; + private Debouncer torqueCurrentDebouncer = new Debouncer(torqueCurrentDebounceTime, DebounceType.kFalling); + private Debouncer atGoalStateDebouncer = new Debouncer(atGoalStateDebounceTime, DebounceType.kFalling); + private boolean atGoal = false; + @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 = 0.2; //tune p value + config.Slot0.kP = 676767.0; //tune p value config.Slot0.kI = 0; config.Slot0.kD = 0; config.Slot0.kV = 0.12; //Maximum rps = 100 --> 12V/100rps + + config.TorqueCurrent.PeakForwardTorqueCurrent = 40; // this is the constant torque velocity + config.TorqueCurrent.PeakReverseTorqueCurrent = 0; // we are making this a BANG BANG controller for talon fx + config.MotorOutput.PeakForwardDutyCycle = 1.0; + config.MotorOutput.PeakReverseDutyCycle = 0.0; + shooterMotorLeft.getConfigurator().apply(config); shooterMotorRight.getConfigurator().apply(config); - shooterMotorLeft.getConfigurator().apply( + shooterMotorRight.getConfigurator().apply( new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast) ); - shooterMotorRight.getConfigurator().apply( + shooterMotorLeft.getConfigurator().apply( new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) ); - feederMotor.getConfigurator().apply( - new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast) - ); + // set start up for phase initially: + phase = FlywheelPhase.START_UP; SmartDashboard.putData("Turn on shooter", new InstantCommand(() -> setShooter(ShooterConstants.SHOOTER_VELOCITY))); - SmartDashboard.putData("Turn on feeder", new InstantCommand(() -> setFeeder(ShooterConstants.FEEDER_RUN_POWER))); SmartDashboard.putData("Turn ALL off", new InstantCommand(() -> deactivateShooterAndFeeder())); SmartDashboard.putData("Turn off Shooter", new InstantCommand(() -> setShooter(0))); - SmartDashboard.putData("Turn off feeder", new InstantCommand(() -> setFeeder(0))); } 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(-shooterPower); - // shooterMotorRight.set(-shooterPower); - feederMotor.set(feederPower); + // updateInputs(); + // runVelocity(Units.rotationsToRadians(getShooterVelcoity())); + // SmartDashboard.putNumber("Shot Power", shooterPower); + // shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower); + + // if (phase == FlywheelPhase.BANG_BANG) { // shooter target speed is in RPS + // // Duty-cycle bang-bang (apply to both) + // shooterMotorLeft.setControl(new VelocityDutyCycle(shooterTargetSpeed)); // spin as fast as possible shooter target speed + // shooterMotorRight.setControl(new VelocityDutyCycle(shooterTargetSpeed)); // same here + // } else { + // // Torque-current bang-bang + // shooterMotorLeft.setControl(new VelocityTorqueCurrentFOC(shooterTargetSpeed)); // apply constant torque current + // shooterMotorRight.setControl(new VelocityTorqueCurrentFOC(shooterTargetSpeed)); // again + // } + + shooterMotorLeft.set(-shooterPower); + shooterMotorRight.set(-shooterPower); + } + + /** Run closed loop at the specified velocity. */ + private void runVelocity(double velocityRadsPerSec) { + boolean inTolerance = + Math.abs(Units.rotationsToRadians(getShooterVelcoity()) - velocityRadsPerSec) + <= torqueCurrentControlTolerance; + boolean torqueCurrentControl = torqueCurrentDebouncer.calculate(inTolerance); + atGoal = atGoalStateDebouncer.calculate(inTolerance); + + if (!torqueCurrentControl && lastTorqueCurrentControl) { + launchCount++; + } + lastTorqueCurrentControl = torqueCurrentControl; + + phase = + torqueCurrentControl + ? FlywheelPhase.BANG_BANG + : FlywheelPhase.CONSTANT_TORQUE; + shooterTargetSpeed = Units.radiansToRotations(velocityRadsPerSec); + Logger.recordOutput("Shooter/Setpoint", velocityRadsPerSec); } + public void deactivateShooterAndFeeder() { - setFeeder(0); setShooter(0); System.out.println("Shooter deactivated"); } - public void setFeeder(double power){ - System.out.println("VELOCITY: " + getShooterVelcoity()); - feederPower = power; - } public void setShooter(double velocityRPS) { shooterTargetSpeed = velocityRPS; @@ -98,10 +144,6 @@ public class Shooter extends SubsystemBase implements ShooterIO { this.shooterPower = power; } - public double getFeederVelocity() { - return inputs.feederVelocity; - } - public double getShooterVelcoity() { return inputs.leftShooterVelocity; // assuming they are the same rn } @@ -109,9 +151,7 @@ public class Shooter extends SubsystemBase implements ShooterIO { public void updateInputs() { inputs.leftShooterVelocity = shooterMotorLeft.getVelocity().getValueAsDouble(); inputs.rightShooterVelocity = shooterMotorRight.getVelocity().getValueAsDouble(); - inputs.feederVelocity = feederMotor.getVelocity().getValueAsDouble(); inputs.leftShooterCurrent = shooterMotorLeft.getStatorCurrent().getValueAsDouble(); - inputs.rightShooterVelocity = shooterMotorRight.getStatorCurrent().getValueAsDouble(); - inputs.feederVelocity = feederMotor.getStatorCurrent().getValueAsDouble(); + inputs.rightShooterCurrent = shooterMotorRight.getStatorCurrent().getValueAsDouble(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index ece6543..4a01beb 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.turret; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; public class TurretConstants { public static double MAX_ANGLE = 180; @@ -21,4 +23,7 @@ public class TurretConstants { public static double LEFT_ENCODER_OFFSET = 0; // degrees public static double RIGHT_ENCODER_OFFSET = 0; // degrees -} + + public static Translation2d DISTANCE_FROM_ROBOT_CENTER = new Translation2d(0,0); + +} \ No newline at end of file -- 2.39.5