Skip to content

Commit 26b10cc

Browse files
1939DriverStation1939DriverStation
committed
Practice Day
1 parent ecd5e98 commit 26b10cc

File tree

15 files changed

+146
-60
lines changed

15 files changed

+146
-60
lines changed

‎src/main/java/frc/robot/BuildConstants.java‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,12 @@ public final class BuildConstants {
77
public static final String MAVEN_GROUP = "";
88
public static final String MAVEN_NAME = "Reefscape2025";
99
public static final String VERSION = "unspecified";
10-
public static final int GIT_REVISION = 161;
11-
public static final String GIT_SHA = "ac421b4493163359fe03320f29a161e4e51cc5a8";
12-
public static final String GIT_DATE = "2025-03-06 01:32:45 CST";
10+
public static final int GIT_REVISION = 167;
11+
public static final String GIT_SHA = "ecd5e98507db43c25aa59df26ed8be2b3f5078df";
12+
public static final String GIT_DATE = "2025-03-06 08:12:25 CST";
1313
public static final String GIT_BRANCH = "feature/indexing";
14-
public static final String BUILD_DATE = "2025-03-06 03:20:17 CST";
15-
public static final long BUILD_UNIX_TIME = 1741252817056L;
14+
public static final String BUILD_DATE = "2025-03-06 17:55:48 CST";
15+
public static final long BUILD_UNIX_TIME = 1741305348569L;
1616
public static final int DIRTY = 1;
1717

1818
private BuildConstants(){}

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 12 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -64,8 +64,8 @@ private void configureBindings () {
6464
() -> -driver.getLeftY(),
6565
() -> -driver.getLeftX(),
6666
() -> -driver.getRightX(),
67-
this.driver.rightBumper(),
68-
this.driver.leftBumper()
67+
this.driver.leftBumper(),
68+
this.driver.rightBumper()
6969
)
7070
);
7171

@@ -76,15 +76,18 @@ private void configureBindings () {
7676
elevatorSetpoints.and(this.operator.a()).onTrue(new SetpointElevator(this.elevator, 0.55));
7777
elevatorSetpoints.and(this.operator.b()).onTrue(new SetpointElevator(this.elevator, 0.97));
7878
elevatorSetpoints.and(this.operator.y()).onTrue(new SetpointElevator(this.elevator, 1.60));
79-
elevatorSetpoints.and(this.operator.povUp()).onTrue(new SetpointElevator(this.elevator, 0.0)); //TODO Calculate Barge Height
79+
elevatorSetpoints.and(this.operator.povUp()).onTrue(new SetpointElevator(this.elevator, 1.64)); //TODO Calculate Barge Height
8080
elevatorSetpoints.and(this.operator.povLeft()).onTrue(new SetpointElevator(this.elevator, 0.0)); //TODO Calculate Processer Height
81-
elevatorSetpoints.and(this.operator.povRight()).onTrue(new SetpointElevator(this.elevator, 0.0)); //TODO Calculate Reef High Height
82-
elevatorSetpoints.and(this.operator.povDown()).onTrue(new SetpointElevator(this.elevator, 0.0)); //TODO Calculate Reef Low Height
81+
elevatorSetpoints.and(this.operator.povRight()).onTrue(new SetpointElevator(this.elevator, 0.76)); //TODO Calculate Reef High Height
82+
elevatorSetpoints.and(this.operator.povDown()).onTrue(new SetpointElevator(this.elevator, 0.37)); //TODO Calculate Reef Low Height
8383

84-
this.operator.rightBumper().onTrue(new LoadCoral(funnel, endEffector, 5.0, -10.0));
85-
this.operator.leftBumper().whileTrue(new AlgaeMove(endEffector, SetPointConstants.ALGAE_INTAKE_SPEED));
86-
this.operator.rightTrigger().onTrue(new ScoreCoral(endEffector, SetPointConstants.CORAL_OUTTAKE_SPEED));
87-
this.operator.leftTrigger().whileTrue(new AlgaeMove(endEffector, SetPointConstants.ALGAE_OUTTAKE_SPEED));
84+
this.operator.rightBumper().toggleOnTrue(new LoadCoral(funnel, endEffector, 10.0, -15.0));
85+
this.operator.rightTrigger().toggleOnTrue(new ScoreCoral(endEffector, SetPointConstants.CORAL_OUTTAKE_SPEED));
86+
87+
this.operator.leftBumper().whileTrue(new PivotWrist(this.endEffector, 150.0));
88+
this.operator.leftTrigger().whileTrue(new ScoreAlgae(this.endEffector));
89+
90+
this.operator.start().whileTrue(new GroundIntakeAlgae(this.elevator, this.endEffector));
8891

8992
//new Trigger(this.endEffector::isManual).whileTrue(new ManualEndEffector(this.endEffector, () -> this.operator.getRightX() * 3.0, ));
9093

@@ -99,18 +102,6 @@ private void configureBindings () {
99102
new ManualEndEffector(endEffector, () -> (0.34 + (0.017 * 50.0)), () -> 0.0, () -> 0.0)
100103
);
101104
*/
102-
103-
/**
104-
this.endEffector.setDefaultCommand(
105-
new ManualEndEffector(
106-
this.endEffector,
107-
() -> this.driver.getLeftX() * 3.0,
108-
//() -> (this.driver.getRightTriggerAxis() - this.driver.getLeftTriggerAxis()) * 3.0,
109-
() -> 0.0,
110-
() -> (this.driver.getRightTriggerAxis() - this.driver.getLeftTriggerAxis()) * 4.0
111-
)
112-
);
113-
*/
114105
}
115106

116107
public Command getAutonomousCommand() {

‎src/main/java/frc/robot/SetPointConstants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -13,5 +13,5 @@ public class SetPointConstants {
1313
public static final double FUNNEL_INTAKE_SPEED = 1.5;
1414
public static final double FUNNEL_OUTTAKE_SPEED = -1.5;
1515
public static final double FUNNEL_STUCK_SPEED = -1.65;
16-
public static final double CORAL_OUTTAKE_SPEED = -5.0;
16+
public static final double CORAL_OUTTAKE_SPEED = 20.0;
1717
}

‎src/main/java/frc/robot/commands/AlgaeScore.java‎

Lines changed: 0 additions & 14 deletions
This file was deleted.

‎src/main/java/frc/robot/commands/AutomaticEndEffectorBack.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@ public void initialize () {
2727
@Override
2828
public boolean isFinished () {
2929

30-
return (this.coralIntakePosition - this.endEffector.getCoralIntakePosition()) > 5.5;
30+
return (this.coralIntakePosition - this.endEffector.getCoralIntakePosition()) > 4.0;
3131
}
3232

3333
@Override

‎src/main/java/frc/robot/commands/GroundIntakeAlgae.java‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
package frc.robot.commands;
2+
3+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
4+
import frc.robot.subsystems.elevator.Elevator;
5+
import frc.robot.subsystems.end_effector.EndEffector;
6+
7+
public class GroundIntakeAlgae extends SequentialCommandGroup {
8+
9+
public GroundIntakeAlgae (Elevator elevator, EndEffector endEffector) {
10+
11+
this.addCommands(
12+
new SetpointElevator(elevator, -0.15),
13+
new PivotWrist(endEffector, 175.0)
14+
);
15+
}
16+
}

‎src/main/java/frc/robot/commands/PivotWrist.java‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
package frc.robot.commands;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.end_effector.EndEffector;
5+
6+
public class PivotWrist extends Command {
7+
8+
private final EndEffector endEffector;
9+
private final double position;
10+
11+
public PivotWrist (EndEffector endEffector, double position) {
12+
13+
this.endEffector = endEffector;
14+
this.position = position;
15+
this.addRequirements(this.endEffector);
16+
}
17+
18+
@Override
19+
public void initialize () {
20+
21+
this.endEffector.setAlgaeWristPosition(position);
22+
this.endEffector.setAlgaeIntakeVoltage(2.0);
23+
}
24+
25+
@Override
26+
public void end (boolean interrupted) {
27+
28+
this.endEffector.setAlgaeWristPosition(0.0);
29+
}
30+
}

‎src/main/java/frc/robot/commands/ScoreAlgae.java‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
package frc.robot.commands;
2+
import edu.wpi.first.wpilibj2.command.Command;
3+
import edu.wpi.first.wpilibj2.command.Commands;
4+
import edu.wpi.first.wpilibj2.command.WaitCommand;
5+
import frc.robot.subsystems.end_effector.EndEffector;
6+
7+
public class ScoreAlgae extends Command {
8+
9+
private final EndEffector endEffector;
10+
11+
public ScoreAlgae(EndEffector endEffector) {
12+
13+
this.endEffector = endEffector;
14+
this.addRequirements(this.endEffector);
15+
}
16+
17+
@Override
18+
public void initialize () {
19+
20+
this.endEffector.setAlgaeWristPosition(0.0);
21+
this.endEffector.setAlgaeIntakeVoltage(-3.0);
22+
}
23+
24+
@Override
25+
public void end (boolean interrupted) {
26+
27+
this.endEffector.setAlgaeWristPosition(0.0);
28+
this.endEffector.setAlgaeIntakeVoltage(0.0);
29+
}
30+
}

‎src/main/java/frc/robot/commands/ScoreCoral.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ public void initialize () {
2626
@Override
2727
public boolean isFinished () {
2828

29-
return (this.endEffector.getCoralIntakePosition() - this.coralPosition) > 50.0;
29+
return (this.endEffector.getCoralIntakePosition() - this.coralPosition) > 25.0;
3030
}
3131

3232
@Override

‎src/main/java/frc/robot/commands/swerve/Drive.java‎

Lines changed: 22 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,14 @@
22

33
import java.util.function.DoubleSupplier;
44

5+
import edu.wpi.first.math.controller.PIDController;
6+
import edu.wpi.first.math.controller.ProfiledPIDController;
7+
import edu.wpi.first.math.geometry.Pose2d;
8+
import edu.wpi.first.math.geometry.Rotation2d;
9+
import edu.wpi.first.math.geometry.Translation2d;
510
import edu.wpi.first.math.kinematics.ChassisSpeeds;
11+
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
12+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
613
import edu.wpi.first.wpilibj2.command.Command;
714
import edu.wpi.first.wpilibj2.command.Commands;
815
import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -15,9 +22,9 @@ public class Drive extends Command {
1522
private final Swerve swerve;
1623
private SwerveInputStream activeInputStream;
1724
private SwerveInputStream driverInputStream;
18-
private SwerveInputStream precisionInputStream;
25+
private SwerveInputStream visionInputStream;
1926

20-
public Drive (Swerve swerve, DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier omega, Trigger drive, Trigger precision) {
27+
public Drive (Swerve swerve, DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier omega, Trigger drive, Trigger vision) {
2128

2229
this.swerve = swerve;
2330

@@ -29,14 +36,8 @@ public Drive (Swerve swerve, DoubleSupplier vx, DoubleSupplier vy, DoubleSupplie
2936
.cubeTranslationControllerAxis(true)
3037
.allianceRelativeControl(true);
3138

32-
this.precisionInputStream = new SwerveInputStream(this.swerve.getSwerveDrive(), vx, vy, omega)
33-
.deadband(ControllerConstants.SWERVE_DEADBAND)
34-
.scaleTranslation(0.4)
35-
.cubeTranslationControllerAxis(true)
36-
.allianceRelativeControl(true);
37-
3839
drive.onTrue(Commands.runOnce(() -> this.activeInputStream = driverInputStream));
39-
precision.onTrue(Commands.runOnce(() -> this.activeInputStream = precisionInputStream));
40+
vision.onTrue(Commands.runOnce(() -> this.activeInputStream = visionInputStream));
4041

4142
this.activeInputStream = driverInputStream;
4243

@@ -46,8 +47,18 @@ public Drive (Swerve swerve, DoubleSupplier vx, DoubleSupplier vy, DoubleSupplie
4647
@Override
4748
public void execute () {
4849

50+
if (this.activeInputStream == this.visionInputStream) {
51+
52+
double xDistance = 12.35 - this.swerve.getPose().getX();
53+
double yDistance = 3.00 - this.swerve.getPose().getY();
54+
55+
this.swerve.driveToPose(new Translation2d(xDistance, yDistance).times(2));
56+
} else {
57+
58+
this.swerve.driveFieldOriented(this.activeInputStream.get());
59+
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(this.activeInputStream.get(), this.swerve.getSwerveDrive().getOdometryHeading());
60+
}
61+
4962
// TODO Heading Lock
50-
this.swerve.driveFieldOriented(this.activeInputStream.get());
51-
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(this.activeInputStream.get(), this.swerve.getSwerveDrive().getOdometryHeading());
5263
}
5364
}

0 commit comments

Comments
 (0)