From 420fd90b121bdec6ce2390f155a1265f24e16a91 Mon Sep 17 00:00:00 2001 From: Mehooliu Date: Tue, 3 Mar 2026 19:35:30 -0500 Subject: [PATCH] Tweaked Trapezoidal Profile, removed assistfuelCommand from right trigger --- .../neo/modules/physicalproperties.json | 2 +- src/main/java/frc/robot/Constants.java | 22 +++++++++---------- src/main/java/frc/robot/Robot.java | 6 ++++- src/main/java/frc/robot/RobotContainer.java | 7 +++--- .../frc/robot/subsystems/IntakeSubsystem.java | 9 ++++---- 5 files changed, 26 insertions(+), 20 deletions(-) diff --git a/src/main/deploy/swerve/neo/modules/physicalproperties.json b/src/main/deploy/swerve/neo/modules/physicalproperties.json index fa0c1f3..63dee33 100644 --- a/src/main/deploy/swerve/neo/modules/physicalproperties.json +++ b/src/main/deploy/swerve/neo/modules/physicalproperties.json @@ -1,6 +1,6 @@ { "optimalVoltage": 12, - "robotMass": 51.75, + "robotMass": 140, "wheelGripCoefficientOfFriction": 1.85, "currentLimit": { "drive": 40, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b0f7ced..9c643fa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -38,10 +38,10 @@ public final class Constants { private static ShuffleboardTab programmingTab = Shuffleboard.getTab("Programming"); - public static final double ROBOT_MASS = 115 * 0.453592; // 32lbs * kg per pound + public static final double ROBOT_MASS = 140 * 0.453592; // 32lbs * kg per pound public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag - public static final double MAX_SPEED = Units.feetToMeters(14.5); + public static final double MAX_SPEED = Units.feetToMeters(25); // Maximum speed of the robot in meters per second, used to limit acceleration. // public static final class AutonConstants @@ -98,7 +98,7 @@ public final class Constants { * .withWidget(BuiltInWidgets.kNumberBar).getEntry(); */ - public static double INDEXER_AND_RAMP_MOTOR_RPM = 10000; + public static double INDEXER_AND_RAMP_MOTOR_RPM = 20000; // this method called in robot periodic so values updated in elastic are // constantly read and applied to RAMP_MOTOR_SPEED @@ -116,7 +116,7 @@ public final class Constants { * programmingTab.add("Desired Intake RPM", -1000) * .withWidget(BuiltInWidgets.kNumberBar).getEntry(); */ - public static double INTAKE_WHEELS_MOTOR_RPM = -7000; + public static double INTAKE_WHEELS_MOTOR_RPM = -6250; /* * public static void updateIntakeWheelsRPM() { @@ -128,9 +128,9 @@ public final class Constants { public static final int INTAKE_ROTATOR_MOTOR_ID = 51; public static class IntakeRotatorPID { - public static final double INTAKE_ROTATOR_P = 10; - public static final double INTAKE_ROTATOR_I = 0; - public static final double INTAKE_ROTATOR_D = 2; + public static final double INTAKE_ROTATOR_P = .5; + public static final double INTAKE_ROTATOR_I = 0.01; + public static final double INTAKE_ROTATOR_D = 0; } public static final double INTAKE_MOTOR_P = 0.0001; @@ -141,9 +141,9 @@ public final class Constants { public static final double INTAKE_MIDDLE_ENCODER_VALUE = -2.5; public static final double INTAKE_RETRACT_ENCODER_VALUE = -5; - public static final double INTAKE_THROUGHBORE_ENCODER_DEPLOY = .26; - public static final double INTAKE_THROUGHBORE_ENCODER_RETRACT = .64; - public static final double INTAKE_THROUGHBORE_ENCODER_MIDDLE = .54; + public static final double INTAKE_THROUGHBORE_ENCODER_DEPLOY = .13; + public static final double INTAKE_THROUGHBORE_ENCODER_RETRACT = .49; + public static final double INTAKE_THROUGHBORE_ENCODER_MIDDLE = .389; } @@ -178,7 +178,7 @@ public final class Constants { public static final Transform3d ROBOT_TO_FRONT_LEFT_CAM = new Transform3d( new Translation3d(Units.inchesToMeters(23.375), 0, Units.inchesToMeters(13)), - new Rotation3d(0, 0, 0)); + new Rotation3d(0, 74, 0)); public static final Transform3d ROBOT_TO_FRONT_RIGHT_CAM = new Transform3d( new Translation3d(0, 0, 0), diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e330ec4..0b128a3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Constants.IntakeConstants; +import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.TargetingSubsystems; @@ -37,6 +38,7 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; private LEDSubsystem m_LedSubsystem; + private ClimberSubsystem m_ClimberSubsystem; private Timer disabledTimer; private static NetworkTable table; @@ -63,7 +65,7 @@ public class Robot extends TimedRobot { m_LedSubsystem = new LEDSubsystem(); m_robotContainer = new RobotContainer(); - + m_ClimberSubsystem = new ClimberSubsystem(); // Create a timer to disable motor brake a few seconds after disable. This will // let the robot stop // immediately when disabled, but then also let it be pushed more @@ -134,6 +136,7 @@ public class Robot extends TimedRobot { // IntakeSubsystem.resetIntakeRotationEncoder(); m_robotContainer.setMotorBrake(true); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + m_ClimberSubsystem.lowerRobotCommand(); // Print the selected autonomous command upon autonomous init System.out.println("Auto selected: " + m_autonomousCommand); @@ -156,6 +159,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { + m_ClimberSubsystem.lowerRobotCommand(); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c50ddcb..0a4cf0f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -196,7 +196,7 @@ public class RobotContainer { .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand().andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand())); // command for // full shooting system including linear actuators - driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand().andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly())); + driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY).repeatedly()); @@ -205,14 +205,15 @@ public class RobotContainer { driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()); driverXbox.povDown().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY)); driverXbox.povUp().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT)); + //driverXbox.povLeft().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE)); //driverXbox.povDown().onTrue(m_IntakeSubsystem.deployIntakeCommand()); //driverXbox.povUp().onTrue(m_IntakeSubsystem.retractIntakeCommand()); //driverXbox.povRight().whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox)); // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); - driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand() - .andThen(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE))); + driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand()); + //.andThen(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY))); // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(), // () -> -driverXbox.getLeftX())); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index c160024..78adf31 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -30,7 +30,7 @@ public class IntakeSubsystem extends SubsystemBase { private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID, MotorType.kBrushless); - private final TrapezoidProfile.Constraints m_Constraints = new TrapezoidProfile.Constraints(10, 5); + private final TrapezoidProfile.Constraints m_Constraints = new TrapezoidProfile.Constraints(5, 10); private final ProfiledPIDController intakeRotatorProfiledPIDController; private static SparkClosedLoopController intakeRotatorPIDController; @@ -48,6 +48,7 @@ public class IntakeSubsystem extends SubsystemBase { Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P, Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I, Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D, m_Constraints); + intakeRotatorProfiledPIDController.setTolerance(0.05); intakeRotatorConfig.closedLoop .p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P) @@ -72,7 +73,7 @@ public class IntakeSubsystem extends SubsystemBase { public void goToPosition(double goalPosition) { double pidVal = intakeRotatorProfiledPIDController.calculate(encoderValue, goalPosition); - intakeRotatorMotor.setVoltage(-pidVal); + intakeRotatorMotor.setVoltage(-pidVal * 12); } @@ -106,8 +107,8 @@ public class IntakeSubsystem extends SubsystemBase { return runOnce(() -> goToPosition(goalPosition)); } - public Command assistFuelIntakeCommand(double deployedPosition, double retractedPosition) { - return runOnce(() -> goToPositionCommand(retractedPosition).andThen(new WaitCommand(1.5)) + public Command assistFuelIntakeCommand(double deployedPosition, double assistPosition) { + return runOnce(() -> goToPositionCommand(assistPosition).andThen(new WaitCommand(1.5)) .andThen(goToPositionCommand(deployedPosition)).andThen(new WaitCommand(1.5))); }