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

March 5th driver practice

Browse Source
This commit is contained in:
Michael Mikovsky
2025-03-08 17:11:56 -07:00
parent c58a2406ac
commit a264e466bc
8 changed files with 74 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -1
View File
@@ -427,10 +427,11 @@ public final class Constants {
public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
//Max for endefector = 60%
public static final double L2_SCORE_ELEVATOR = -5;
public static final double L2_LEAVE_ELEVATOR = -11;
public static final double L2_SCORE_ENDEFFECTOR = -19;
public static final double COMPLETLY_DOWN_ENDEFFECTOR = 9001;
public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
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;
src/main/java/frc4388/robot/RobotContainer.java
+6 -2
View File
@@ -274,6 +274,8 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
@@ -293,6 +295,8 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
@@ -326,7 +330,7 @@ public class RobotContainer {
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_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
@@ -354,7 +358,7 @@ public class RobotContainer {
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
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_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
src/main/java/frc4388/robot/subsystems/Elevator.java
+23 -2
View File
@@ -48,6 +48,9 @@ public class Elevator extends Subsystem {
private boolean disableAutoIntake = false;
private boolean seededZeroEndefector = false;
private boolean seededZeroElevator = false;
private DigitalInput basinBeamBreak;
private DigitalInput endeffectorLimitSwitch;
private DigitalInput intakeIR;
@@ -58,6 +61,7 @@ public class Elevator extends Subsystem {
Ready, // Has coral in endefector
Hovering, // Has coral in endefector
L2Score,
L2ScoreLeave,
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
@@ -116,7 +120,7 @@ public class Elevator extends Subsystem {
case Waiting: {
wait = System.currentTimeMillis() + maxWait;
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
led.setMode(LEDConstants.WAITING_PATTERN);
break;
}
@@ -129,7 +133,7 @@ public class Elevator extends Subsystem {
}
case Ready: {
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
led.setMode(LEDConstants.DOWN_PATTERN);
break;
@@ -148,6 +152,13 @@ public class Elevator extends Subsystem {
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case L2ScoreLeave: {
PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
led.setMode(LEDConstants.SCORING_PATTERN);
break;
}
case PrimedFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
@@ -312,6 +323,16 @@ public class Elevator extends Subsystem {
SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
SmartDashboard.putString("State", currentState.toString());
if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) {
endeffectorMotor.setPosition(0);
seededZeroEndefector = !seededZeroEndefector;
}
if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) {
elevatorMotor.setPosition(0);
seededZeroElevator = !seededZeroElevator;
}
if (disableAutoIntake) return;
src/main/java/frc4388/robot/subsystems/Vision.java
+15 -5
View File
@@ -9,6 +9,7 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import java.time.Instant;
import java.util.List;
import java.util.Optional;
@@ -124,6 +125,8 @@ public class Vision extends Subsystem {
rotations -= 180;
}
private Instant lastVisionTime = null;
private void update() {
isTagProcessed = false;
@@ -176,20 +179,27 @@ public class Vision extends Subsystem {
lastLatency = latency / cams;
if(isTagProcessed){
Instant now = Instant.now();
// double lastAngle = lastVisionPose.getRotation().getDegrees();
double curAngle = Yaw/cams;
// if(lastAngle - curAngle >= 90){
// subtractRotation();
// }else if(lastAngle - curAngle <= -90){
// addRotation();
// if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) > 1){
// double lastAngle = lastVisionPose.getRotation().getDegrees();
// if(lastAngle - curAngle >= 80){
// addRotation();
// }else if(lastAngle - curAngle <= -80){
// subtractRotation();
// }
// }
// SmartDashboard.putNumber("curAngle", curAngle);
// SmartDashboard.putNumber("Rotations", rotations);
lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle));
lastVisionTime = now;
}
}