Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 08:48:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

l2 baby

Browse Source
This commit is contained in:
C4llSiqn
2025-02-21 20:22:07 -07:00
parent c9a947d3b8
commit 0914601699
4 changed files with 108 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+7 -3
View File
@@ -234,7 +234,10 @@ public final class Constants {
public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0);
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, 1, 0);
public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
public static final int LIDAR_DIO_CHANNEL = 7;
@@ -361,8 +364,6 @@ public final class Constants {
public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5);
;;;;;;;;;;;;;;;;;;;;;;;;;;;;
// public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(17);
@@ -418,12 +419,15 @@ public final class Constants {
public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
public static final double SCORING_THREE_ELEVATOR = -9.25;
public static final double DEALGAE_L2_ELEVATOR = -23.5;
public static final double DEALGAE_L3_ELEVATOR = -33.75;
public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
public static final double GEAR_RATIO_ENDEFECTOR = -100.0;
public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
//Max for endefector = 60%
public static final double L2_SCORE_ENDEFECTOR = -19;
public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
public static final double DEALGAE_L2_EENDEFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
src/main/java/frc4388/robot/RobotContainer.java
+43 -4
View File
@@ -314,11 +314,20 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
.onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
.onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
.onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270)
.onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
@@ -360,6 +369,23 @@ public class RobotContainer {
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(AprilLidarAlignL3);
//Trims
new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
// Lower coral removal
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
.onTrue(new SequentialCommandGroup(
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
@@ -372,6 +398,19 @@ public class RobotContainer {
new Translation2d(0,1), new Translation2d(), 500, true)
));
// Upper coral removal
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
.onTrue(new SequentialCommandGroup(
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true)
));
// ? /* Programer Buttons (Controller 3)*/
src/main/java/frc4388/robot/commands/GotoLastApril.java
+4 -1
View File
@@ -3,6 +3,7 @@ package frc4388.robot.commands;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
@@ -54,7 +55,9 @@ public class GotoLastApril extends Command {
public void initialize() {
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, AutoConstants.X_OFFSET_TRIM.get(), distance);
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side,
Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()),
distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get()));
}
double xerr;
src/main/java/frc4388/robot/subsystems/Elevator.java
+54 -22
View File
@@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ElevatorConstants;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
public class Elevator extends SubsystemBase {
/** Creates a new Elevator. */
@@ -28,6 +29,9 @@ public class Elevator extends SubsystemBase {
private long wait = 0;
private long maxWait = 1000;
private double elevatorRefrence = 0;
private double endefectorRefrence = 0;
private DigitalInput basinBeamBreak;
private DigitalInput endefectorLimitSwitch;
@@ -36,6 +40,7 @@ public class Elevator extends SubsystemBase {
WatingBeamTriped, //once the beam break trips
Ready, // Has coral in endefector
Hovering, // Has coral in endefector
L2Score,
PrimedThree, // Arm and elevator Waiting to score in the level 3 position
ScoringThree, // Arm and elevator in the level three position
PrimedFour, // Arm and elevator Waiting to score in the level 4 position
@@ -66,6 +71,9 @@ public class Elevator extends SubsystemBase {
//PID methods
private void PIDPosition(TalonFX motor, double position) {
if (motor == elevatorMotor) elevatorRefrence = position;
else endefectorRefrence = position;
var request = new PositionDutyCycle(position);
motor.setControl(request);
}
@@ -106,39 +114,57 @@ public class Elevator extends SubsystemBase {
break;
}
case L2Score: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.L2_SCORE_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
case PrimedFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
break;
}
case ScoringFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR);
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
case PrimedThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR);
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFECTOR);
break;
}
case ScoringThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
case BallRemoverL2Primed: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
break;
}
case BallRemoverL2Go: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR);
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
case BallRemoverL3Primed: {
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
break;
}
case BallRemoverL3Go: {
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
@@ -150,17 +176,22 @@ public class Elevator extends SubsystemBase {
}
public boolean elevatorAtRefrence() {
double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
// double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.1;
if (atPos) {
SmartDashboard.putNumber("Elevator Refrence", elevatorRefrence);
SmartDashboard.putNumber("Elevator Pos", elevatorPosition);
}
return Math.abs(elevatorRefrence - elevatorPosition) <= 0.1;
return atPos;
}
public boolean endefectorAtRefrence() {
double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
double elevatorPosition = endefectorMotor.getPosition().getValueAsDouble();
// double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble();
return Math.abs(elevatorRefrence - elevatorPosition) <= 0.2;
return Math.abs(endefectorRefrence - endefectorPosition) <= 0.2;
}
// public void driveElevatorStick(Translation2d stick) {
// if (stick.getNorm() > 0.05) {
@@ -173,14 +204,14 @@ public class Elevator extends SubsystemBase {
transitionState(CoordinationState.Ready);
}
private void periodicWaitingTripped() {
if (!basinBeamBreak.get() && System.currentTimeMillis() > wait)
transitionState(CoordinationState.Ready);
}
// private void periodicWaitingTripped() {
// if (!basinBeamBreak.get() && System.currentTimeMillis() > wait)
// transitionState(CoordinationState.Ready);
// }
private void periodicReady() {
// if (elevatorAtRefrence())
// transitionState(CoordinationState.Hovering);
if (elevatorAtRefrence())
transitionState(CoordinationState.Hovering);
}
private void periodicScoring() {
@@ -193,13 +224,14 @@ public class Elevator extends SubsystemBase {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0);
SmartDashboard.putString("State", currentState.toString());
if (currentState == CoordinationState.Waiting) {
// periodicWaiting();
periodicWaiting();
} else if (currentState == CoordinationState.WatingBeamTriped) {
// periodicWaitingTripped();
} else if (currentState == CoordinationState.Ready) {
// periodicReady();
periodicReady();
}
// } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
// periodicScoring();