Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-08 16:28:04 -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
+9
View File
@@ -307,6 +307,7 @@ public class SwerveDrive extends Subsystem {
swerveDriveTrain.tareEverything();
robotKnowsWhereItIs = false;
rotTarget = 0;
// vision.resetRotations();
}
@@ -432,6 +433,14 @@ public class SwerveDrive extends Subsystem {
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));
}
}
src/main/java/frc4388/utility/ReefPositionHelper.java
+6 -1
View File
@@ -5,6 +5,7 @@ import java.util.Optional;
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.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc4388.robot.Constants.AutoConstants;
@@ -14,7 +15,8 @@ public class ReefPositionHelper {
public enum Side {
LEFT,
RIGHT,
CENTER
CENTER,
FAR_LEFT
}
public static final Pose2d[] RED_TAGS = {
@@ -83,11 +85,14 @@ public class ReefPositionHelper {
switch(side) {
case LEFT:
return -(AutoConstants.X_SCORING_POSITION_OFFSET);
case FAR_LEFT:
return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8));
case RIGHT:
return (AutoConstants.X_SCORING_POSITION_OFFSET);
case CENTER:
return 0;
}
assert false;
return 0;
}