Skip to content

Commit

Permalink
Made basic command (Needs testing)
Browse files Browse the repository at this point in the history
  • Loading branch information
Ronan-B committed Jan 26, 2025
1 parent 080e191 commit 7546002
Show file tree
Hide file tree
Showing 4 changed files with 15 additions and 12 deletions.
7 changes: 5 additions & 2 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
import frc.robot.commands.ResetGyro;
import frc.robot.commands.RunAlgaeIntake;
import frc.robot.commands.RunCoralOuttake;
import frc.robot.commands.RunEndEffectorIntake;
import frc.robot.commands.SwerveCharacterization;
import frc.robot.constants.ROBOT;
import frc.robot.constants.SWERVE;
Expand All @@ -30,6 +31,7 @@
import frc.robot.subsystems.AlgaeIntake;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.CoralOuttake;
import frc.robot.subsystems.EndEffector;
import frc.robot.utils.SysIdUtils;
import frc.robot.utils.Telemetry;

Expand All @@ -45,7 +47,7 @@ public class RobotContainer {
private final Telemetry m_telemetry = new Telemetry();
private final CoralOuttake m_coralOuttake = new CoralOuttake();
private final AlgaeIntake m_algaeIntake = new AlgaeIntake();

private final EndEffector m_endEffector = new EndEffector();
// Replace with CommandPS4Controller or CommandJoystick if needed
private final Joystick leftJoystick = new Joystick(USB.leftJoystick);
private final SendableChooser<Command> m_sysidChooser = new SendableChooser<>();
Expand Down Expand Up @@ -173,8 +175,9 @@ private void configureBindings() {
m_driverController
.rightBumper()
.whileTrue(new RunCoralOuttake(m_coralOuttake, -0.15)); // intake
m_driverController.x().whileTrue(new RunAlgaeIntake(m_algaeIntake, 0.5)); // outtake
m_driverController.x().whileTrue(new RunAlgaeIntake(m_algaeIntake, 0.5)); // ou,ttake
m_driverController.y().whileTrue(new RunAlgaeIntake(m_algaeIntake, -0.5)); // intake
m_driverController.leftTrigger().whileTrue(new RunEndEffectorIntake(m_endEffector, 0.4414)); //
}

/**
Expand Down
16 changes: 8 additions & 8 deletions src/main/java/frc/robot/commands/RunEndEffectorIntake.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,20 +5,20 @@
package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.AlgaeIntake;
import frc.robot.subsystems.EndEffector;

/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class RunAlgaeIntake extends Command {
public class RunEndEffectorIntake extends Command {

private final AlgaeIntake m_algaeIntake;
private final EndEffector m_EndEffector;
private final double m_speed;

/** Creates a new SetAlgaeIntakeSpeed. */
public RunAlgaeIntake(AlgaeIntake algae, double speed) {
m_algaeIntake = algae;
public RunEndEffectorIntake(EndEffector endeffector, double speed) {
m_EndEffector = endeffector;
m_speed = speed;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(m_algaeIntake);
addRequirements(m_EndEffector);
}

// Called when the command is initially scheduled.
Expand All @@ -28,13 +28,13 @@ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_algaeIntake.setPercentOutput(m_speed);
m_EndEffector.setPercentOutput(m_speed);
}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_algaeIntake.setPercentOutput(0);
m_EndEffector.setPercentOutput(0);
}

// Returns true when the command should end.
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/constants/CAN.java
Original file line number Diff line number Diff line change
Expand Up @@ -22,5 +22,5 @@ public class CAN {

public static final int coralOuttakeMotor = 30;
public static final int algaeMotor = 31;
public static final int EndEffector = 35;
public static final int endEffector = 35;
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/EndEffector.java
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
import frc.robot.utils.CtreUtils;

public class EndEffector extends SubsystemBase {
private final TalonFX m_endEffectorMotor = new TalonFX(CAN.EndEffector);
private final TalonFX m_endEffectorMotor = new TalonFX(CAN.endEffector);

/** Creates a new EndEffector. */
public EndEffector() {
Expand Down

0 comments on commit 7546002

Please sign in to comment.