From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Wed, 18 Feb 2026 21:16:28 +0000 (-0800) Subject: fixes X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=3366e497920b4431add251546136cd6e87c20034;p=FRC2026.git fixes --- diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index efcd7c2..e4fdaef 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,8 +13,10 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.commands.DoNothing; import frc.robot.commands.drive_comm.DefaultDriveCommand; import frc.robot.commands.vision.ShutdownAllPis; @@ -56,6 +58,8 @@ public class RobotContainer { private Command auto = new DoNothing(); + // Auto Command selection + private final SendableChooser autoChooser = new SendableChooser<>(); // Controllers are defined here private BaseDriverConfig driver = null; @@ -67,7 +71,12 @@ public class RobotContainer { * Different robots may have different subsystems. */ public RobotContainer(RobotId robotId) { - // climb = new Climb(); + // display the current robot id on smartdashboard + SmartDashboard.putString("RobotID", robotId.toString()); + + // Filling the SendableChooser on SmartDashboard + autoChooserInit(); + // dispatch on the robot switch (robotId) { case TestBed1: @@ -189,8 +198,32 @@ public class RobotContainer { } } - public Command getAutoCommand(){ - return auto; + // Autos + + /** + * Initialize the SendableChooser on the SmartDashbboard. + * Fill the SendableChooser with available Commands. + */ + public void autoChooserInit() { + // add the options to the Chooser + autoChooser.setDefaultOption("Do nothing", new DoNothing()); + autoChooser.addOption("Do nada", new DoNothing()); + autoChooser.addOption("Spin my wheels", new DoNothing()); + autoChooser.addOption("Hello world", new InstantCommand(() -> System.out.println("Hello world"))); + + // put the Chooser on the SmartDashboard + SmartDashboard.putData("Auto chooser", autoChooser); + } + + /** + * Gets the auto command from SmartDashboard + * @return + */ + public Command getAutoCommand() { + // get the selected Command + Command autoSelected = autoChooser.getSelected(); + + return autoSelected; } public void logComponents(){ diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java index f171d9a..426a75b 100644 --- a/src/main/java/frc/robot/constants/IdConstants.java +++ b/src/main/java/frc/robot/constants/IdConstants.java @@ -19,8 +19,10 @@ public class IdConstants { // LEDs public static final int CANDLE_ID = 1; - // Motor ID + // Turret public static final int TURRET_MOTOR_ID = 20; + public static final int TURRET_ENCODER_LEFT_ID = 0; + public static final int TURRET_ENCODER_RIGHT_ID = 1; // Shooter public static final int SHOOTER_LEFT_ID = 10; diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java new file mode 100644 index 0000000..b4643d6 --- /dev/null +++ b/src/main/java/frc/robot/constants/ShotInterpolation.java @@ -0,0 +1,27 @@ +package frc.robot.constants; + +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +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(); + + public static final InterpolatingDoubleTreeMap exitVelocityMap = 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); + + hoodAngleMap.put(0.0, Units.degreesToRadians(90)); + hoodAngleMap.put(1.0, Units.degreesToRadians(90)); + + //TODO: find actual values from video motion + exitVelocityMap.put(1.0, 2.0); + exitVelocityMap.put(2.0, 4.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java index 89ac487..70ade0d 100644 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -31,8 +31,6 @@ public class Hood extends SubsystemBase implements HoodIO{ private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02); - private double FEEDFORWARD_KV = 0.12; - private Rotation2d goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)); private double goalVelocityRadPerSec = 0.0; private double lastFilteredRad = 0.0; @@ -116,7 +114,7 @@ public class Hood extends SubsystemBase implements HoodIO{ motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.radiansToRotations(MIN_ANGLE_RAD) * GEAR_RATIO, Units.radiansToRotations(MAX_ANGLE_RAD) * GEAR_RATIO); // Multiply goal velocity by kV - double velocityCompensation = goalVelocityRadPerSec * FEEDFORWARD_KV; + double velocityCompensation = goalVelocityRadPerSec * HoodConstants.FEEDFORWARD_KV; // Set control with feedforward motor.setControl(mmVoltageRequest diff --git a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java index 03343b0..e38e680 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java @@ -14,4 +14,6 @@ public class HoodConstants { public static final double MAX_ANGLE = 82; // degrees public static final double MIN_ANGLE = 58.5; // degrees + + public static final double FEEDFORWARD_KV = 0.12; } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ad29439..2e4423a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -104,6 +104,6 @@ public class Shooter extends SubsystemBase implements ShooterIO { */ @AutoLogOutput(key="Shooter/TargetSpeed") public double getTargetVelocity(){ - return Units.rotationsToRadians(shooterTargetSpeed); + return shooterTargetSpeed; } } diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index 0d26d1c..17d72a7 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -10,7 +10,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID); private double power = 0.0; - public int ballCount = 0; + private int ballCount = 0; private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged(); private boolean wasAboveThreshold = false; diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index f8d8885..c686cef 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -48,8 +48,8 @@ public class Turret extends SubsystemBase implements TurretIO{ /* ---------------- Hardware ---------------- */ private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.SUBSYSTEM_CANIVORE_CAN); - private final CANcoder encoderLeft = new CANcoder(0, Constants.SUBSYSTEM_CANIVORE_CAN); - private final CANcoder encoderRight = new CANcoder(1, Constants.SUBSYSTEM_CANIVORE_CAN); + private final CANcoder encoderLeft = new CANcoder(IdConstants.TURRET_ENCODER_LEFT_ID, Constants.SUBSYSTEM_CANIVORE_CAN); + private final CANcoder encoderRight = new CANcoder(IdConstants.TURRET_ENCODER_RIGHT_ID, Constants.SUBSYSTEM_CANIVORE_CAN); private TalonFXSimState simState; private SingleJointedArmSim turretSim;