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

Auto align scoring

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-18 19:39:01 -07:00
parent 47646dc12b
commit 2641abe8a4
7 changed files with 180 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+16 -6
View File
@@ -358,11 +358,16 @@ public final class Constants {
public static final class FieldConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
// public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(9.5);
public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5);
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(20);
public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
@@ -402,17 +407,22 @@ public final class Constants {
public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
public static final double GEAR_RATIO_ELEVATOR = -9.0;
//Max for elevator = 50%
public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
public static final double WAITING_POSITION_ELEVATOR = (2 / 3.d) * GEAR_RATIO_ELEVATOR; // TODO: find 2-4 in off the pipe
public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = 2 * GEAR_RATIO_ELEVATOR; // TODO: find on the pipe
public static final double WAITING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
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 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 COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
public static final double SCORING_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
public static final double SCORING_THREE_ENDEFECTOR = 0.375 * GEAR_RATIO_ENDEFECTOR;
public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
src/main/java/frc4388/robot/RobotContainer.java
+30 -8
View File
@@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.utility.controller.XboxController;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.robot.Constants.FieldConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
@@ -40,6 +41,8 @@ import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.GotoLastApril;
import frc4388.robot.commands.LidarAlign;
import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.waitElevatorRefrence;
import frc4388.robot.commands.waitEndefectorRefrence;
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
@@ -109,20 +112,29 @@ public class RobotContainer {
// () -> autoplaybackName.get(), // lastAutoName
// new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
// true, false);
private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
// private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
private Command AprilLidarAlign = new SequentialCommandGroup(
new GotoLastApril(m_robotSwerveDrive, m_vision),
new InstantCommand(() -> System.out.println("Soup")),
new WaitCommand(1),
new LidarAlign(m_robotSwerveDrive, m_lidar)
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
// new InstantCommand(() -> System.out.println("Soup")),
// new WaitCommand(1),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
);
private Command AprilLidarLeft = new SequentialCommandGroup(
AutoGotoPosition.asProxy(),
AprilLidarAlign.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_lidar)
);
private Command AprilLidarRight = new SequentialCommandGroup(
AutoGotoPosition.asProxy(),
AprilLidarAlign.asProxy(),
new InstantCommand(() -> System.out.println("Soup")),
new WaitCommand(1),
new LidarAlign(m_robotSwerveDrive, m_lidar)//,
@@ -153,7 +165,7 @@ public class RobotContainer {
*/
public RobotContainer() {
NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition);
NamedCommands.registerCommand("AutoGotoPosition", AprilLidarAlign);
NamedCommands.registerCommand("april-allign", AprilLidarAlign);
NamedCommands.registerCommand("place-coral", placeCoral);
NamedCommands.registerCommand("grab-coral", grabCoral);
@@ -251,6 +263,13 @@ public class RobotContainer {
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(AprilLidarAlign);
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
// ? /* Operator Buttons */
@@ -272,6 +291,9 @@ public class RobotContainer {
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator));
src/main/java/frc4388/robot/commands/GotoLastApril.java
+10 -2
View File
@@ -28,15 +28,19 @@ public class GotoLastApril extends Command {
SwerveDrive swerveDrive;
Vision vision;
double distance;
Side side;
/**
* Command to drive robot to position.
* @param SwerveDrive m_robotSwerveDrive
*/
public GotoLastApril(SwerveDrive swerveDrive, Vision vision) {
public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.distance = distance;
this.side = side;
// addRequirements(swerveDrive);
}
@@ -50,7 +54,7 @@ public class GotoLastApril extends Command {
public void initialize() {
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.LEFT, AutoConstants.X_OFFSET_TRIM.get());
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, AutoConstants.X_OFFSET_TRIM.get(), distance);
}
double xerr;
@@ -70,6 +74,10 @@ public class GotoLastApril extends Command {
double youtput = yPID.update(yerr);
double rotoutput = rotPID.update(roterr);
xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
Translation2d leftStick = new Translation2d(
Math.max(Math.min(-youtput, 1), -1),
Math.max(Math.min(-xoutput, 1), -1)
src/main/java/frc4388/robot/commands/waitElevatorRefrence.java
+36
View File
@@ -0,0 +1,36 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Elevator;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class waitElevatorRefrence extends Command {
/** Creates a new waitElevatorRefrence. */
private Elevator elevator;
public waitElevatorRefrence(Elevator elevator) {
this.elevator = elevator;
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return elevator.elevatorAtRefrence();
}
}
src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java
+36
View File
@@ -0,0 +1,36 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Elevator;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class waitEndefectorRefrence extends Command {
/** Creates a new waitElevatorRefrence. */
private Elevator elevator;
public waitEndefectorRefrence(Elevator elevator) {
this.elevator = elevator;
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return elevator.endefectorAtRefrence();
}
}
src/main/java/frc4388/robot/subsystems/Elevator.java
+50 -9
View File
@@ -4,6 +4,8 @@
package frc4388.robot.subsystems;
import java.time.Instant;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.controls.PositionVoltage;
@@ -23,6 +25,9 @@ public class Elevator extends SubsystemBase {
private TalonFX elevatorMotor;
private TalonFX endefectorMotor;
private long wait = 0;
private long maxWait = 1000;
private DigitalInput basinBeamBreak;
private DigitalInput endefectorLimitSwitch;
@@ -30,6 +35,7 @@ public class Elevator extends SubsystemBase {
Waiting, // for coral into the though
WatingBeamTriped, //once the beam break trips
Ready, // Has coral in endefector
Hovering, // Has coral in endefector
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
@@ -76,6 +82,7 @@ public class Elevator extends SubsystemBase {
currentState = state;
switch (currentState) {
case Waiting: {
wait = System.currentTimeMillis() + maxWait;
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
break;
@@ -84,7 +91,6 @@ public class Elevator extends SubsystemBase {
case WatingBeamTriped: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
armShuffle();
break;
}
@@ -94,7 +100,19 @@ public class Elevator extends SubsystemBase {
break;
}
case Hovering: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
break;
}
case ScoringThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.SCORING_THREE_ENDEFECTOR);
break;
}
case PrimedFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
break;
@@ -102,13 +120,26 @@ public class Elevator extends SubsystemBase {
case ScoringFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR);
break;
}
}
}
public boolean elevatorAtRefrence() {
double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
return Math.abs(elevatorRefrence - elevatorPosition) <= 0.1;
}
public boolean endefectorAtRefrence() {
double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
double elevatorPosition = endefectorMotor.getPosition().getValueAsDouble();
return Math.abs(elevatorRefrence - elevatorPosition) <= 0.2;
}
// public void driveElevatorStick(Translation2d stick) {
// if (stick.getNorm() > 0.05) {
// elevatorMotor.set(stick.getY());
@@ -116,28 +147,38 @@ public class Elevator extends SubsystemBase {
// }
private void periodicWaiting() {
if (basinBeamBreak.get()) transitionState(CoordinationState.WatingBeamTriped);
if (!basinBeamBreak.get())
transitionState(CoordinationState.WatingBeamTriped);
}
private void periodicWaitingTripped() {
if (basinBeamBreak.get()) transitionState(CoordinationState.Ready);
if (!basinBeamBreak.get() && System.currentTimeMillis() > wait)
transitionState(CoordinationState.Ready);
}
private void periodicReady() {
if (elevatorAtRefrence())
transitionState(CoordinationState.Hovering);
}
private void periodicScoring() {
if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0);
// if (currentState == CoordinationState.Waiting) {
// periodicWaiting();
// } else if (currentState == CoordinationState.WatingBeamTriped) {
// periodicWaitingTripped();
// }
if (currentState == CoordinationState.Waiting) {
periodicWaiting();
} else if (currentState == CoordinationState.WatingBeamTriped) {
periodicWaitingTripped();
} else if (currentState == CoordinationState.Ready) {
periodicReady();
}
// } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
// periodicScoring();
// }