Team2890/Mothman
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Tweaked Trapezoidal Profile, removed assistfuelCommand from right trigger

Browse Source
This commit is contained in:
Mehooliu
2026-03-03 19:35:30 -05:00
parent 670de7dd90
commit 420fd90b12
5 changed files with 26 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/main/java/frc/robot/Constants.java
View File

@@ -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),

6
src/main/java/frc/robot/Robot.java
View File

@@ -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

7
src/main/java/frc/robot/RobotContainer.java
View File

@@ -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()));

9
src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View File

@@ -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)));
}