Skip to content

Commit 5565949

Browse files
mwitcpalekmwitcpalek
committed
add funnel, intial drive practice bugs
1 parent 69a8811 commit 5565949

File tree

10 files changed

+78
-28
lines changed

10 files changed

+78
-28
lines changed

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

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -145,11 +145,11 @@ public RobotContainer() {
145145
}
146146

147147
private void configureTelemetry() {
148-
telemetryService.register(driveSubsystem);
149-
telemetryService.register(coralSubsystem);
150-
telemetryService.register(algaeSubsystem);
151-
telemetryService.register(elevatorSubsystem);
152-
elevatorIO.registerWith(telemetryService);
148+
driveSubsystem.registerWith(telemetryService);
149+
coralSubsystem.registerWith(telemetryService);
150+
algaeSubsystem.registerWith(telemetryService);
151+
elevatorSubsystem.registerWith(telemetryService);
152+
funnelSubsystem.registerWith(telemetryService);
153153
telemetryService.start();
154154
}
155155

@@ -161,7 +161,7 @@ private void configureDriverBindings() {
161161
// Reset Gyro Command
162162
new JoystickButton(driveJoystick, Button.M_SWC.id).onTrue(new ResetGyroCommand(driveSubsystem));
163163
new JoystickButton(driveJoystick, Button.M_SWH.id)
164-
.onTrue(new ScoreReefManualCommand(robotStateSubsystem));
164+
.onTrue(new ScoreReefManualCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem));
165165
}
166166

167167
private void configureOperatorBindings() {
@@ -177,7 +177,9 @@ private void configureOperatorBindings() {
177177

178178
// Stow
179179
new JoystickButton(xboxController, XboxController.Button.kBack.value)
180-
.onTrue(new StowCommand(robotStateSubsystem));
180+
.onTrue(
181+
new StowCommand(
182+
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
181183

182184
// zero elevator, slated for removal
183185
new JoystickButton(xboxController, XboxController.Button.kX.value)

‎src/main/java/frc/robot/commands/robotState/ScoreReefManualCommand.java‎

Lines changed: 18 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,33 +1,44 @@
11
package frc.robot.commands.robotState;
22

33
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.coral.CoralSubsystem;
5+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
46
import frc.robot.subsystems.robotState.RobotStateSubsystem;
57
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
68

79
public class ScoreReefManualCommand extends Command {
8-
public RobotStateSubsystem robotStateSubsystem;
9-
public RobotStates robotState;
10+
private RobotStateSubsystem robotStateSubsystem;
11+
private ElevatorSubsystem elevatorSubsystem;
12+
private RobotStates startingRobotState;
13+
private boolean startingElevatorFinished;
1014

11-
public ScoreReefManualCommand(RobotStateSubsystem robotStateSubsystem) {
15+
public ScoreReefManualCommand(
16+
RobotStateSubsystem robotStateSubsystem,
17+
ElevatorSubsystem elevatorSubsystem,
18+
CoralSubsystem coralSubsystem) {
1219
this.robotStateSubsystem = robotStateSubsystem;
20+
this.elevatorSubsystem = elevatorSubsystem;
21+
addRequirements(elevatorSubsystem, coralSubsystem);
1322
}
1423

1524
@Override
1625
public void initialize() {
17-
robotState = robotStateSubsystem.getState();
26+
startingRobotState = robotStateSubsystem.getState();
27+
startingElevatorFinished = elevatorSubsystem.isFinished();
1828
robotStateSubsystem.setIsAuto(false);
1929
robotStateSubsystem.setGetAlgaeOnCycle(false);
2030
robotStateSubsystem.toPrepCoral();
2131
}
2232

2333
@Override
2434
public boolean isFinished() {
25-
if (robotState == RobotStates.PRESTAGE || robotState == RobotStates.STOW) {
35+
if (startingRobotState == RobotStates.PRESTAGE || startingRobotState == RobotStates.STOW) {
2636
return robotStateSubsystem.getState() == RobotStates.REEF_ALIGN_CORAL;
2737
}
28-
if (robotState == RobotStates.REEF_ALIGN_CORAL) {
38+
if (startingRobotState == RobotStates.REEF_ALIGN_CORAL) {
2939
return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD
30-
|| robotStateSubsystem.getState() == RobotStates.LOADING_CORAL;
40+
|| robotStateSubsystem.getState() == RobotStates.LOADING_CORAL
41+
|| !startingElevatorFinished;
3142
}
3243
return false;
3344
}

‎src/main/java/frc/robot/commands/robotState/StowCommand.java‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,22 @@
11
package frc.robot.commands.robotState;
22

33
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
5+
import frc.robot.subsystems.coral.CoralSubsystem;
6+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
47
import frc.robot.subsystems.robotState.RobotStateSubsystem;
58
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
69

710
public class StowCommand extends Command {
811
RobotStateSubsystem robotState;
912

10-
public StowCommand(RobotStateSubsystem robotState) {
13+
public StowCommand(
14+
RobotStateSubsystem robotState,
15+
ElevatorSubsystem elevatorSubsystem,
16+
CoralSubsystem coralSubsystem,
17+
BiscuitSubsystem biscuitSubsystem) {
1118
this.robotState = robotState;
19+
addRequirements(elevatorSubsystem, coralSubsystem, biscuitSubsystem);
1220
}
1321

1422
@Override

‎src/main/java/frc/robot/constants/ElevatorConstants.java‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@ public class ElevatorConstants {
3939

4040
// Setpoints
4141
// Idle
42-
public static final Angle kFunnelSetpoint = Rotations.of(2.40430);
42+
public static final Angle kFunnelSetpoint = Rotations.of(2.03125); // was 2.40430
4343
public static final Angle kStowSetpoint = kFunnelSetpoint;
4444

4545
// Algae removal
@@ -54,8 +54,9 @@ public class ElevatorConstants {
5454

5555
// Coral score
5656
public static final Angle kL1CoralSetpoint = Rotations.of(13.04053);
57-
public static final Angle kL2CoralSetpoint = Rotations.of(19.62793);
58-
public static final Angle kL3CoralSetpoint = Rotations.of(30.42969);
57+
public static final Angle kL2CoralSetpoint = Rotations.of(21.0786); // 19.62793 -> 21.0786
58+
public static final Angle kL3CoralSetpoint =
59+
Rotations.of(31.0901); // was 30.42969 -> 31.7505 -> 31.0901
5960
public static final Angle kL4CoralSetpoint = Rotations.of(48.28076);
6061

6162
public static final Angle kPrestageSetpoint = kL3CoralSetpoint;

‎src/main/java/frc/robot/constants/FunnelConstants.java‎

Lines changed: 16 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,21 +1,25 @@
11
package frc.robot.constants;
22

3+
import com.ctre.phoenix6.configs.CommutationConfigs;
34
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
5+
import com.ctre.phoenix6.configs.ExternalFeedbackConfigs;
46
import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
57
import com.ctre.phoenix6.configs.MotorOutputConfigs;
68
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
79
import com.ctre.phoenix6.configs.TalonFXSConfiguration;
10+
import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue;
811
import com.ctre.phoenix6.signals.ForwardLimitSourceValue;
912
import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
13+
import com.ctre.phoenix6.signals.MotorArrangementValue;
1014
import com.ctre.phoenix6.signals.NeutralModeValue;
1115
import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
1216
import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
1317

1418
public class FunnelConstants {
15-
public static final double kFunnelPercentOutput = 0;
19+
public static final double kFunnelPercentOutput = 0.5;
1620

17-
public static int FunnelFxsId = 0;
18-
public static final int kFunnelBeamCounts = 3;
21+
public static int FunnelFxsId = 40;
22+
public static final int kFunnelBeamCounts = 1;
1923

2024
public static TalonFXSConfiguration getFXSConfig() {
2125
TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration();
@@ -55,6 +59,15 @@ public static TalonFXSConfiguration getFXSConfig() {
5559
.withNeutralMode(NeutralModeValue.Coast);
5660
fxsConfig.MotorOutput = motorOut;
5761

62+
CommutationConfigs commutation =
63+
new CommutationConfigs().withMotorArrangement(MotorArrangementValue.Minion_JST);
64+
fxsConfig.Commutation = commutation;
65+
66+
ExternalFeedbackConfigs external =
67+
new ExternalFeedbackConfigs()
68+
.withExternalFeedbackSensorSource(ExternalFeedbackSensorSourceValue.Commutation);
69+
fxsConfig.ExternalFeedback = external;
70+
5871
return fxsConfig;
5972
}
6073
}

‎src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
import net.jafama.FastMath;
1919
import org.littletonrobotics.junction.Logger;
2020
import org.slf4j.LoggerFactory;
21+
import org.strykeforce.telemetry.TelemetryService;
2122
import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
2223
import org.strykeforce.telemetry.measurable.Measure;
2324

@@ -347,6 +348,12 @@ public enum DriveStates {
347348
SAFE_HOLD
348349
}
349350

351+
@Override
352+
public void registerWith(TelemetryService telemetryService) {
353+
io.registerWith(telemetryService);
354+
super.registerWith(telemetryService);
355+
}
356+
350357
@Override
351358
public Set<Measure> getMeasures() {
352359
return Set.of(

‎src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,8 +63,9 @@ public void periodic() {
6363
org.littletonrobotics.junction.Logger.processInputs("ElevatorInputs", inputs);
6464

6565
// Log outputs
66-
Logger.recordOutput("Elevator/setpoints", setpoint);
66+
Logger.recordOutput("Elevator/setpoint", setpoint.in(Rotations));
6767
Logger.recordOutput("Elevator/state", currState);
68+
Logger.recordOutput("Elevator/isFinished", isFinished());
6869

6970
switch (currState) {
7071
case ZEROING -> {

‎src/main/java/frc/robot/subsystems/funnel/FunnelIOFXS.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ public void setPct(double percentOutput) {
4848
public void updateInputs(FunnelIOInputs inputs) {
4949
BaseStatusSignal.refreshAll(curVelocity, curRevLimit);
5050
inputs.velocity = curVelocity.getValue();
51-
inputs.isRevBeamBroken = curRevLimit.getValue().value == 1;
51+
inputs.isRevBeamBroken = curRevLimit.getValue().value == 0;
5252
}
5353

5454
@Override

‎src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,8 @@ public FunnelState getState() {
2828
}
2929

3030
public boolean hasCoral() {
31-
return true;
32-
// return curState == FunnelState.HasSeenCoral;
31+
// return true;
32+
return curState == FunnelState.HasSeenCoral;
3333
}
3434

3535
@Override
@@ -82,11 +82,11 @@ public void setPercent(double pct) {
8282
io.setPct(pct);
8383
}
8484

85-
public void StartMotor() {
85+
public void startMotor() {
8686
setPercent(FunnelConstants.kFunnelPercentOutput);
8787
}
8888

89-
public void StopMotor() {
89+
public void stopMotor() {
9090
setPercent(0.0);
9191
}
9292

‎src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -230,7 +230,9 @@ public void toStow() {
230230

231231
public void toPrepCoral() {
232232
if (curState == RobotStates.REEF_ALIGN_CORAL) {
233-
toPlaceCoral();
233+
if (elevatorSubsystem.isFinished()) {
234+
toPlaceCoral();
235+
}
234236
} else if (isAuto) {
235237
toReefAlign(false, false);
236238
} else {
@@ -309,6 +311,7 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
309311

310312
public void toPlaceCoral() {
311313
coralSubsystem.eject();
314+
funnelSubsystem.ClearCoral();
312315
scoringTimer.stop();
313316
scoringTimer.reset();
314317
scoringTimer.start();
@@ -569,6 +572,9 @@ public void periodic() {
569572
}
570573

571574
case FUNNEL_LOAD -> {
575+
if (elevatorSubsystem.isFinished()) {
576+
funnelSubsystem.startMotor();
577+
}
572578
if (funnelSubsystem.hasCoral()) {
573579
coralLoc = CoralLoc.TRANSFER;
574580
setState(RobotStates.LOADING_CORAL);
@@ -579,6 +585,7 @@ public void periodic() {
579585
coralLoc = CoralLoc.CORAL;
580586
// biscuitSubsystem.setPosition(BiscuitConstants.kPrestageSetpoint);
581587
elevatorSubsystem.setPosition(ElevatorConstants.kPrestageSetpoint);
588+
funnelSubsystem.stopMotor();
582589

583590
setState(RobotStates.PRESTAGE, true);
584591
}

0 commit comments

Comments
 (0)