mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-09 00:38:02 -06:00
end of day one
This commit is contained in:
C4llSiqn
@@ -54,6 +54,7 @@ import frc4388.utility.Gains;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.ReefPositionHelper;
|
||||
import frc4388.utility.Trim;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
|
||||
/**
|
||||
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
|
||||
@@ -193,7 +194,7 @@ public final class Constants {
|
||||
.withKS(0).withKV(0.124);
|
||||
|
||||
public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
|
||||
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(2.5, 0.2, 1);
|
||||
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1);
|
||||
}
|
||||
|
||||
public static final class Configurations {
|
||||
@@ -298,7 +299,11 @@ public final class Constants {
|
||||
}
|
||||
|
||||
public static final class AutoConstants {
|
||||
public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
|
||||
// public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
|
||||
public static final Gains XY_GAINS = new Gains(8,0,0.0);
|
||||
// public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
|
||||
// public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
|
||||
// public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
|
||||
// public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
|
||||
|
||||
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
|
||||
@@ -316,24 +321,37 @@ public final class Constants {
|
||||
public static final double MIN_XY_PID_OUTPUT = 0.0;
|
||||
public static final double MIN_ROT_PID_OUTPUT = 0.0;
|
||||
|
||||
public static final double VELOCITY_THRESHHOLD = 0.005;
|
||||
public static final double VELOCITY_THRESHHOLD = 0.01;
|
||||
|
||||
// X is tangent to reef side
|
||||
// Y is normal to reef side
|
||||
public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field
|
||||
public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1);
|
||||
public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18);
|
||||
|
||||
public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
|
||||
public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
|
||||
// public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
|
||||
public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
|
||||
public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP;
|
||||
// public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5);
|
||||
|
||||
public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
|
||||
public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
|
||||
public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
|
||||
public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1);
|
||||
|
||||
public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
|
||||
public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
|
||||
public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6);
|
||||
public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5);
|
||||
|
||||
public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
|
||||
public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE;
|
||||
|
||||
// public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
|
||||
// public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
|
||||
// // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
|
||||
|
||||
// public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
|
||||
// public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
|
||||
|
||||
// public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
|
||||
// public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
|
||||
|
||||
// public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
|
||||
|
||||
public static final int L4_DRIVE_TIME = 250; //Milliseconds
|
||||
// public static final int L3_DRIVE_TIME = 500;
|
||||
@@ -437,7 +455,8 @@ public final class Constants {
|
||||
public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
|
||||
public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
|
||||
|
||||
public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
|
||||
|
||||
@@ -38,6 +38,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
// Commands
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
@@ -49,6 +50,7 @@ import frc4388.robot.commands.DriveUntilLiDAR;
|
||||
import frc4388.robot.commands.GotoLastApril;
|
||||
import frc4388.robot.commands.LidarAlign;
|
||||
import frc4388.robot.commands.MoveForTimeCommand;
|
||||
import frc4388.robot.commands.MoveUntilSuply;
|
||||
import frc4388.robot.commands.WhileTrueCommand;
|
||||
import frc4388.robot.commands.waitElevatorRefrence;
|
||||
import frc4388.robot.commands.waitEndefectorRefrence;
|
||||
@@ -148,6 +150,48 @@ public class RobotContainer {
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
|
||||
waitDebuger.asProxy(),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
waitDebuger.asProxy(),
|
||||
new ParallelDeadlineGroup(
|
||||
new WaitCommand(1),
|
||||
new MoveUntilSuply(
|
||||
m_robotSwerveDrive,
|
||||
new Translation2d(0,-0.5),
|
||||
new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
|
||||
),
|
||||
new InstantCommand(m_robotSwerveDrive::softStop, m_robotElevator),
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
|
||||
|
||||
new ConditionalCommand(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
() -> m_robotElevator.hasCoral()),
|
||||
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
/* private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
|
||||
), () -> m_robotElevator.isL4Primed()),
|
||||
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
|
||||
waitDebuger.asProxy(),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
@@ -204,6 +248,53 @@ public class RobotContainer {
|
||||
|
||||
|
||||
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
); */
|
||||
|
||||
private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
|
||||
|
||||
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
|
||||
), () -> m_robotElevator.isL4Primed()),
|
||||
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
|
||||
waitDebuger.asProxy(),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
waitDebuger.asProxy(),
|
||||
new ParallelDeadlineGroup(
|
||||
new WaitCommand(1),
|
||||
new MoveUntilSuply(
|
||||
m_robotSwerveDrive,
|
||||
new Translation2d(0,-0.5),
|
||||
new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
|
||||
),
|
||||
new InstantCommand(m_robotSwerveDrive::softStop, m_robotElevator),
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
|
||||
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
|
||||
|
||||
new ConditionalCommand(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
() -> m_robotElevator.hasCoral()),
|
||||
|
||||
|
||||
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
@@ -408,7 +499,7 @@ public class RobotContainer {
|
||||
new Translation2d(0, 1),
|
||||
new Translation2d(), 300, true
|
||||
|
||||
), new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)),
|
||||
), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)),
|
||||
new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
|
||||
|
||||
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
|
||||
@@ -421,7 +512,7 @@ public class RobotContainer {
|
||||
NamedCommands.registerCommand("prepare-l4", new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Hovering)),
|
||||
// new waitElevatorRefrence(m_robotElevator),
|
||||
new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)),
|
||||
// new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour))
|
||||
));
|
||||
|
||||
@@ -545,14 +636,15 @@ public class RobotContainer {
|
||||
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new DriveUntilLiDAR(m_robotSwerveDrive,
|
||||
new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
|
||||
.onTrue( new DriveUntilLiDAR(m_robotSwerveDrive,
|
||||
new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
|
||||
// .onTrue(new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.HALF_ROBOT_SIZE, Side.CENTER, true));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
.onTrue(thrustIntake.asProxy());
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_reefLidar));
|
||||
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_reefLidar));
|
||||
|
||||
|
||||
// ? /* Operator Buttons */
|
||||
|
||||
@@ -55,11 +55,14 @@ public class RobotMap {
|
||||
/* Elevator Subsystem */
|
||||
public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
|
||||
public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
|
||||
|
||||
|
||||
public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
|
||||
public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
|
||||
public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
|
||||
|
||||
void configureDriveMotorControllers() {}
|
||||
void configureDriveMotorControllers() {
|
||||
// endeffector.saf
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -45,6 +45,10 @@ public class DriveUntilLiDAR extends Command {
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return Math.abs(m_lidar.getDistance()) < mindistance;
|
||||
if (Math.abs(m_lidar.getDistance()) < mindistance) {
|
||||
swerveDrive.softStop();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -50,8 +50,8 @@ public class GotoLastApril extends Command {
|
||||
this.vision = vision;
|
||||
this.distance = distance;
|
||||
this.side = side;
|
||||
this.waitVelocityZero = waitVelocityZero;
|
||||
// addRequirements(swerveDrive);
|
||||
this.waitVelocityZero = waitVelocityZero && false;
|
||||
addRequirements(swerveDrive);
|
||||
}
|
||||
|
||||
|
||||
@@ -62,6 +62,23 @@ public class GotoLastApril extends Command {
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
// double kP = AutoConstants.P_XY_GAINS.get();
|
||||
// double kI = AutoConstants.I_XY_GAINS.get();
|
||||
// double kD = AutoConstants.D_XY_GAINS.get();
|
||||
// xPID = new PID(new Gains(
|
||||
// kP,
|
||||
// kI,
|
||||
// kD),
|
||||
// 0);
|
||||
// yPID = new PID(new Gains(
|
||||
// kP,
|
||||
// kI,
|
||||
// kD),
|
||||
// 0);
|
||||
|
||||
// System.out.println("kP: "+kP);
|
||||
// System.out.println("kI: "+kI);
|
||||
// System.out.println("kD: "+kD);
|
||||
xPID.initialize();
|
||||
yPID.initialize();
|
||||
this.targetpos = ReefPositionHelper.getNearestPosition(
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.time.Instant;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.TimesNegativeOne;
|
||||
|
||||
// Command to repeat a joystick movement for a specific time.
|
||||
public class MoveUntilSuply extends Command {
|
||||
private final SwerveDrive swerveDrive;
|
||||
private final Translation2d leftStick;
|
||||
private final Translation2d rightStick;
|
||||
private final Supplier<Boolean> truth;
|
||||
private final boolean robotRelative;
|
||||
|
||||
public MoveUntilSuply(
|
||||
SwerveDrive swerveDrive,
|
||||
Translation2d leftStick,
|
||||
Translation2d rightStick,
|
||||
Supplier<Boolean> truth,
|
||||
boolean robotRelative) {
|
||||
addRequirements(swerveDrive);
|
||||
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.leftStick = leftStick;
|
||||
this.rightStick = rightStick;
|
||||
this.truth = truth;
|
||||
this.robotRelative = robotRelative;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return truth.get();
|
||||
}
|
||||
}
|
||||
@@ -162,7 +162,7 @@ public class Elevator extends Subsystem {
|
||||
|
||||
case PrimedFour: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR);
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
@@ -255,6 +255,10 @@ public class Elevator extends Subsystem {
|
||||
// }
|
||||
// }
|
||||
|
||||
public boolean getEndeffectorLimit() {
|
||||
return endeffectorLimitSwitch.get();
|
||||
}
|
||||
|
||||
private void periodicWaiting() {
|
||||
if (!basinBeamBreak.get())
|
||||
transitionState(CoordinationState.Ready);
|
||||
@@ -363,7 +367,7 @@ public class Elevator extends Subsystem {
|
||||
}
|
||||
|
||||
public boolean hasCoral() {
|
||||
return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get();
|
||||
return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get();
|
||||
}
|
||||
|
||||
public boolean readyToMove() {
|
||||
|
||||
Reference in New Issue
Block a user