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

Calibration

Browse Source
This commit is contained in:
Michael Mikovsky
2025-03-08 13:22:14 -07:00
parent 63808a652e
commit c58a2406ac
6 changed files with 110 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+5 -4
View File
@@ -382,6 +382,7 @@ public final class Constants {
public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED;
public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW;
public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK;
public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW;
public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES;
public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES;
@@ -418,8 +419,8 @@ public final class Constants {
public static final double HOVERING_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 DEALGAE_L2_ELEVATOR = -23.5;
public static final double DEALGAE_L3_ELEVATOR = -33.75;
public static final double DEALGAE_L2_ELEVATOR = -4;
public static final double DEALGAE_L3_ELEVATOR = -26.5;
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;
@@ -429,8 +430,8 @@ public final class Constants {
public static final double L2_SCORE_ENDEFFECTOR = -19;
public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
public static final double DEALGAE_L2_ENDEFFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_DOWN_ENDEFFECTOR = 9001;
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
src/main/java/frc4388/robot/RobotContainer.java
+49 -18
View File
@@ -299,31 +299,62 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command leftAlgaeRemove = new SequentialCommandGroup(
// private Command leftAlgaeRemove = new SequentialCommandGroup(
// new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
// new waitEndefectorRefrence(m_robotElevator),
// new waitElevatorRefrence(m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
// new LidarAlign(m_robotSwerveDrive, m_reefLidar),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
// new MoveForTimeCommand(m_robotSwerveDrive,
// new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
// new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
// );
private Command lowerAlgaeRemove = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
waitDebuger.asProxy(),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new Translation2d(1,0), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME / 2, true),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME * 2, true),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command rightAlgaeRemove = new SequentialCommandGroup(
// private Command rightAlgaeRemove = new SequentialCommandGroup(
// new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
// new waitEndefectorRefrence(m_robotElevator),
// new waitElevatorRefrence(m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
// new LidarAlign(m_robotSwerveDrive, m_reefLidar),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
// new MoveForTimeCommand(m_robotSwerveDrive,
// new Translation2d(1,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
// new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
// );
private Command upperAlgaeRemove = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
waitDebuger.asProxy(),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME, true),
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
@@ -536,11 +567,11 @@ public class RobotContainer {
// Lower algae removal
new JoystickButton(getButtonBox(), ButtonBox.Eight)
.onTrue(leftAlgaeRemove);
.onTrue(lowerAlgaeRemove);
// Upper algae removal
new JoystickButton(getButtonBox(), ButtonBox.Four)
.onTrue(rightAlgaeRemove);
.onTrue(upperAlgaeRemove);
// Cancel button
@@ -574,8 +605,8 @@ public class RobotContainer {
new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> {operatorManualMode = !operatorManualMode;}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking()));
// new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking()));
// Auto Scoring
new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode)
@@ -598,11 +629,11 @@ public class RobotContainer {
//Controller Lower Algae Removal
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180 && !operatorManualMode)
.onTrue(leftAlgaeRemove);
.onTrue(lowerAlgaeRemove);
//Controller Upper Algae Removal
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0 && !operatorManualMode)
.onTrue(rightAlgaeRemove);
.onTrue(upperAlgaeRemove);
// ? /* Programer Buttons (Controller 3)*/
src/main/java/frc4388/robot/subsystems/Elevator.java
+12 -12
View File
@@ -145,62 +145,62 @@ public class Elevator extends Subsystem {
case L2Score: {
PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case PrimedFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR);
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case ScoringFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case PrimedThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case ScoringThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case BallRemoverL2Primed: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case BallRemoverL2Go: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case BallRemoverL3Primed: {
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
break;
}
case BallRemoverL3Go: {
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+10 -1
View File
@@ -307,6 +307,7 @@ public class SwerveDrive extends Subsystem {
swerveDriveTrain.tareEverything();
robotKnowsWhereItIs = false;
rotTarget = 0;
// vision.resetRotations();
}
@@ -431,7 +432,15 @@ public class SwerveDrive extends Subsystem {
public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
if(curPose.isPresent() && lastPose.isPresent()){
this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
// double rotDiff = curPose.get().getRotation().getDegrees() - lastPose.get().getRotation().getDegrees();
// if(rotDiff >= 180){
// vision.subtractRotation();
// }else if(rotDiff <= -180){
// vision.addRotation();
// }
SmartDashboard.putNumber("Speed", lastOdomSpeed);
}
}
src/main/java/frc4388/robot/subsystems/Vision.java
+28 -1
View File
@@ -110,6 +110,20 @@ public class Vision extends Subsystem {
// cameras[0].
}
public double rotations = 0;
public void resetRotations(){
rotations = 0;
}
public void addRotation(){
rotations += 180;
}
public void subtractRotation(){
rotations -= 180;
}
private void update() {
isTagProcessed = false;
@@ -162,7 +176,20 @@ public class Vision extends Subsystem {
lastLatency = latency / cams;
if(isTagProcessed){
lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(Yaw/cams));
// double lastAngle = lastVisionPose.getRotation().getDegrees();
double curAngle = Yaw/cams;
// if(lastAngle - curAngle >= 90){
// subtractRotation();
// }else if(lastAngle - curAngle <= -90){
// addRotation();
// }
// SmartDashboard.putNumber("curAngle", curAngle);
// SmartDashboard.putNumber("Rotations", rotations);
lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle));
}
}