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

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/deploy/pathplanner/autos/3 Coral Cage 5.auto
-6
View File
@@ -22,12 +22,6 @@
"name": "place-coral-left-l4"
}
},
{
"type": "named",
"data": {
"name": "stop"
}
},
{
"type": "path",
"data": {
src/main/deploy/pathplanner/paths/Cage 5 to Reef.path
+18 -7
View File
@@ -8,20 +8,20 @@
},
"prevControl": null,
"nextControl": {
"x": 6.000189393939394,
"y": 1.6364267676767676
"x": 5.959532828282828,
"y": 1.5754419191919191
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 5.47165404040404,
"y": 2.1141414141414145
"x": 5.349684343434343,
"y": 2.2869318181818175
},
"prevControl": {
"x": 5.86940340909091,
"y": 1.7593308080808083
"x": 5.747433712121213,
"y": 1.9321212121212112
},
"nextControl": null,
"isLocked": false,
@@ -30,7 +30,18 @@
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"pointTowardsZones": [
{
"fieldPosition": {
"x": 4.8,
"y": 3.0
},
"rotationOffset": 0.0,
"minWaypointRelativePos": 0.0,
"maxWaypointRelativePos": 1.0,
"name": "Point Towards Zone"
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path
+3 -3
View File
@@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 5.0,
"maxAcceleration": 5.0,
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0,
"rotation": 58.10920819815426
},
"useDefaultConstraints": false
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path
+7 -7
View File
@@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 1.1010732323232322,
"y": 1.00625
"x": 1.286335227272727,
"y": 1.2828125
},
"prevControl": {
"x": 1.786268939393939,
"y": 2.2439551767676766
"x": 1.9715309343434337,
"y": 2.5205176767676765
},
"nextControl": null,
"isLocked": false,
@@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 5.0,
"maxAcceleration": 5.0,
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0,
"rotation": 121.42956561483854
},
"useDefaultConstraints": false
"useDefaultConstraints": true
}
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;
@@ -149,6 +153,13 @@ public class Elevator extends Subsystem {
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());
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR);
@@ -313,6 +324,16 @@ public class Elevator extends Subsystem {
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;
if (currentState == CoordinationState.Waiting) {
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;
}
}