Skip to content

Commit 84d7f64

Browse files
StarbuckBaristaStarbuckBarista
authored
Merge pull request #17 from FIRST1939/feature/indexing
Tuned values for Coral Load-Maaz Hawa
2 parents cccfa12 + 26b10cc commit 84d7f64

27 files changed

+253
-304
lines changed

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

Lines changed: 6 additions & 6 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 = 153;
11-
public static final String GIT_SHA = "25a16d9f5c438eaeecd3aef28f0dcd901c3e2589";
12-
public static final String GIT_DATE = "2025-03-05 17:21:55 CST";
13-
public static final String GIT_BRANCH = "feature/yagsl";
14-
public static final String BUILD_DATE = "2025-03-05 17:25:08 CST";
15-
public static final long BUILD_UNIX_TIME = 1741217108101L;
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";
13+
public static final String GIT_BRANCH = "feature/indexing";
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/Robot.java‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -48,8 +48,10 @@ public Robot () {
4848

4949
setUseTiming(false);
5050
String logPath = LogFileUtil.findReplayLog();
51-
Logger.setReplaySource(new WPILOGReader(logPath));
52-
Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
51+
Logger.addDataReceiver(new NT4Publisher());
52+
System.out.println("test");
53+
//Logger.setReplaySource(new WPILOGReader(logPath));
54+
//Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
5355
}
5456

5557
Logger.start();

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

Lines changed: 21 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@
55
package frc.robot;
66

77
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
8-
import frc.robot.commands.ConfirmAlliance;
98
import frc.robot.commands.swerve.Drive;
109
import frc.robot.commands.swerve.ZeroGyro;
1110
import frc.robot.subsystems.swerve.Swerve;
@@ -64,60 +63,45 @@ private void configureBindings () {
6463
swerve,
6564
() -> -driver.getLeftY(),
6665
() -> -driver.getLeftX(),
67-
() -> -driver.getRightX()
66+
() -> -driver.getRightX(),
67+
this.driver.leftBumper(),
68+
this.driver.rightBumper()
6869
)
6970
);
7071

71-
// this.driverTwo.leftBumper().whileTrue(new CoralScore(endEffector));
72-
// this.driverTwo.leftTrigger().whileTrue(new AlgaeOuttake(endEffector, SetPointConstants.ALGAE_OUTTAKE_SPEED));
73-
// this.driverTwo.rightTrigger().whileTrue(new AlgaeIntake(endEffector, SetPointConstants.ALGAE_INTAKE_SPEED));
74-
//this.driverTwo.povUp().whileTrue(new ElevatorMove(elevator, () -> this.driverTwo.getRightY() * SetPointConstants.ELEVATOR_MAXIMUM_MANUAL_SPEED));
75-
//this.driverTwo.povDown().whileTrue(new ElevatorMove(elevator, () -> this.driverTwo.getRightY() * -(SetPointConstants.ELEVATOR_MAXIMUM_MANUAL_SPEED)));
76-
//this.driverTwo.a().onTrue(new ElevatorMove(elevator, ()-> .5 ));
77-
//this.driverTwo.leftTrigger().whileTrue(new AlgaeMove(endEffector, SetPointConstants.ALGAE_OUTTAKE_SPEED));
78-
//this.driverTwo.rightTrigger().whileTrue(new AlgaeMove(endEffector, SetPointConstants.ALGAE_INTAKE_SPEED));
72+
new Trigger(this.elevator::isManual).whileTrue(new ManualElevator(this.elevator, () -> -this.operator.getRightY() * 3.0));
73+
Trigger elevatorSetpoints = new Trigger(this.elevator::isManual).negate();
7974

80-
SmartDashboard.putData(CommandScheduler.getInstance());
75+
elevatorSetpoints.and(this.operator.x()).onTrue(new SetpointElevator(this.elevator, -0.015));
76+
elevatorSetpoints.and(this.operator.a()).onTrue(new SetpointElevator(this.elevator, 0.55));
77+
elevatorSetpoints.and(this.operator.b()).onTrue(new SetpointElevator(this.elevator, 0.97));
78+
elevatorSetpoints.and(this.operator.y()).onTrue(new SetpointElevator(this.elevator, 1.60));
79+
elevatorSetpoints.and(this.operator.povUp()).onTrue(new SetpointElevator(this.elevator, 1.64)); //TODO Calculate Barge Height
80+
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.76)); //TODO Calculate Reef High Height
82+
elevatorSetpoints.and(this.operator.povDown()).onTrue(new SetpointElevator(this.elevator, 0.37)); //TODO Calculate Reef Low Height
8183

82-
new Trigger(this.elevator::isManual).whileTrue(new ManualElevator(this.elevator, () -> -this.driver.getRightY() * 3.0));
83-
Trigger elevatorSetpoints = new Trigger(this.elevator::isManual).negate();
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));
8486

85-
elevatorSetpoints.and(this.driver.x()).onTrue(new SetpointElevator(this.elevator, -0.015));
86-
elevatorSetpoints.and(this.driver.a()).onTrue(new SetpointElevator(this.elevator, 0.55));
87-
elevatorSetpoints.and(this.driver.b()).onTrue(new SetpointElevator(this.elevator, 0.97));
88-
elevatorSetpoints.and(this.driver.y()).onTrue(new SetpointElevator(this.elevator, 1.60));
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));
91+
92+
//new Trigger(this.endEffector::isManual).whileTrue(new ManualEndEffector(this.endEffector, () -> this.operator.getRightX() * 3.0, ));
8993

9094
/**
9195
new Trigger(this.funnel::isManual).and(this.driver.leftBumper()).whileTrue(new ManualFunnel(this.funnel, SetPointConstants.FUNNEL_OUTTAKE_SPEED));
9296
new Trigger(this.funnel::isManual).and(this.driver.rightBumper()).whileTrue(new ManualFunnel(this.funnel, SetPointConstants.FUNNEL_INTAKE_SPEED));
9397
new Trigger(this.funnel::isManual).negate().and(this.driver.rightBumper()).onTrue(new AutomaticFunnel(this.funnel, SetPointConstants.FUNNEL_INTAKE_SPEED, SetPointConstants.FUNNEL_STUCK_SPEED)); // TODO Funnel Intake Deadline
9498
*/
9599

96-
this.driver.rightBumper().whileTrue(
97-
new LoadCoral(funnel, endEffector, 25.0, -5.0)
98-
);
99-
100-
this.driver.rightTrigger().whileTrue(
101-
new ManualEndEffector(endEffector, () -> 2.5, () -> 0.0, () -> 0.0)
102-
);
103-
104100
/**
105101
this.endEffector.setDefaultCommand(
106102
new ManualEndEffector(endEffector, () -> (0.34 + (0.017 * 50.0)), () -> 0.0, () -> 0.0)
107103
);
108104
*/
109-
110-
/**
111-
this.endEffector.setDefaultCommand(
112-
new ManualEndEffector(
113-
this.endEffector,
114-
() -> this.driver.getLeftX() * 3.0,
115-
//() -> (this.driver.getRightTriggerAxis() - this.driver.getLeftTriggerAxis()) * 3.0,
116-
() -> 0.0,
117-
() -> (this.driver.getRightTriggerAxis() - this.driver.getLeftTriggerAxis()) * 4.0
118-
)
119-
);
120-
*/
121105
}
122106

123107
public Command getAutonomousCommand() {

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

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,10 +7,11 @@ public class SetPointConstants {
77
public static final double CORAL_OUTTAKE_HEIGHT_L3 = 1.25;
88
public static final double CORAL_OUTTAKE_HEIGHT_L4 = 1.5;
99
public static final double ELEVATOR_MAXIMUM_MANUAL_SPEED = 0.0;
10-
public static final double ALGAE_INTAKE_SPEED = -2.0;
11-
public static final double ALGAE_OUTTAKE_SPEED = 2.0;
10+
public static final double ALGAE_INTAKE_SPEED = 2.0;
11+
public static final double ALGAE_OUTTAKE_SPEED = -2.0;
1212
public static final double CORAL_INTAKE_SPEED = 0.0;
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 = 20.0;
1617
}

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

Lines changed: 7 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -12,23 +12,18 @@ public AlgaeMove(EndEffector endEffector, double algaeouttakespeed) {
1212

1313
this.endEffector=endEffector;
1414
this.algaeouttakespeed = algaeouttakespeed;
15+
1516
this.addRequirements(this.endEffector);
1617
}
1718

1819
@Override
19-
//Start
20-
public void execute () {
21-
endEffector.runVoltage(0, algaeouttakespeed,0);
20+
public void initialize () {
21+
22+
this.endEffector.setAlgaeIntakeVoltage(algaeouttakespeed);
2223
}
2324
@Override
24-
public void end (boolean interrupted)
25-
{
26-
endEffector.runVoltage(0, 0,0);
25+
public void end (boolean interrupted) {
26+
27+
this.endEffector.setAlgaeIntakeVoltage(0.0);
2728
}
28-
2929
}
30-
31-
32-
33-
34-

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

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,12 @@
11
package frc.robot.commands;
2-
32
import edu.wpi.first.wpilibj2.command.Command;
43
import frc.robot.subsystems.end_effector.EndEffector;
54

65
public class AutomaticEndEffectorBack extends Command {
76

87
private final EndEffector endEffector;
98
private final double backVoltage;
9+
private double coralIntakePosition;
1010

1111
public AutomaticEndEffectorBack (EndEffector endEffector, double inVoltage) {
1212

@@ -19,13 +19,15 @@ public AutomaticEndEffectorBack (EndEffector endEffector, double inVoltage) {
1919
@Override
2020
public void initialize () {
2121

22+
this.coralIntakePosition = this.endEffector.getCoralIntakePosition();
2223
this.endEffector.setCoralIntakeVelocity(backVoltage);
24+
2325
}
2426

2527
@Override
2628
public boolean isFinished () {
27-
28-
return !this.endEffector.getCoralIntakeBeambreak();
29+
30+
return (this.coralIntakePosition - this.endEffector.getCoralIntakePosition()) > 4.0;
2931
}
3032

3133
@Override

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

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

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

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

‎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/LoadCoral.java‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,20 +2,21 @@
22

33

44
import edu.wpi.first.wpilibj2.command.Commands;
5-
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
5+
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
66
import frc.robot.SetPointConstants;
77
import frc.robot.subsystems.end_effector.EndEffector;
88
import frc.robot.subsystems.funnel.Funnel;
99

10-
public class LoadCoral extends ParallelCommandGroup{
10+
public class LoadCoral extends ParallelDeadlineGroup{
1111

1212
public LoadCoral (Funnel funnel, EndEffector endEffector, double coralInSpeed, double coralBackSpeed) {
13-
this.addCommands(
14-
new AutomaticFunnel(funnel, SetPointConstants.FUNNEL_INTAKE_SPEED, SetPointConstants.FUNNEL_STUCK_SPEED),
13+
14+
super(
1515
Commands.sequence(
1616
new AutomaticEndEffectorIn(endEffector, coralInSpeed),
1717
new AutomaticEndEffectorBack(endEffector, coralBackSpeed)
18-
)
18+
),
19+
new AutomaticFunnel(funnel, SetPointConstants.FUNNEL_INTAKE_SPEED, SetPointConstants.FUNNEL_STUCK_SPEED)
1920
);
2021
}
2122
}

0 commit comments

Comments
 (0)