Skip to content

Commit

Permalink
Merge pull request #94 from 4201VitruvianBots/endEffector
Browse files Browse the repository at this point in the history 
Working cycles without hopper
  • Loading branch information
@gavinskycastle
gavinskycastle authored Feb 20, 2025
2 parents 7179f53 + f9ba66e commit cfed70b
Show file tree
Hide file tree
Showing 13 changed files with 322 additions and 219 deletions.
58 changes: 36 additions & 22 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,11 +32,13 @@
import frc.robot.commands.climber.SetClimberSetpoint;
import frc.robot.commands.elevator.RunElevatorJoystick;
import frc.robot.commands.elevator.SetElevatorSetpoint;
import frc.robot.commands.endEffector.EndEffectorJoystick;
import frc.robot.commands.endEffector.EndEffectorSetpoint;
import frc.robot.commands.swerve.ResetGyro;
import frc.robot.commands.swerve.SwerveCharacterization;
import frc.robot.constants.CLIMBER.CLIMBER_SETPOINT;
import frc.robot.constants.ENDEFFECTOR.PIVOT_SETPOINT;
import frc.robot.constants.ENDEFFECTOR.PIVOT.PIVOT_SETPOINT;
import frc.robot.constants.ENDEFFECTOR.ROLLERS.ROLLER_SPEED;
import frc.robot.constants.FIELD;
import frc.robot.constants.ROBOT;
import frc.robot.constants.ROBOT.SUPERSTRUCTURE_STATES;
Expand All @@ -46,9 +48,6 @@
import frc.robot.generated.AlphaBotConstants;
import frc.robot.generated.V2Constants;
import frc.robot.subsystems.*;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.EndEffector;
import frc.robot.subsystems.HopperIntake;
import frc.robot.subsystems.alphabot.*;
import frc.robot.utils.Robot2d;
import frc.robot.utils.SysIdUtils;
Expand Down Expand Up @@ -140,21 +139,23 @@ private void initializeSubSystems() {
if (ROBOT.robotID.equals(ROBOT.ROBOT_ID.V2)) {
m_swerveDrive = V2Constants.createDrivetrain();
m_elevator = new Elevator();
m_endEffector = new EndEffector();
m_endEffectorPivot = new EndEffectorPivot();
m_climberIntake = new ClimberIntake();
m_climber = new Climber();
m_hopperIntake = new HopperIntake();
} else if (ROBOT.robotID.equals(ROBOT.ROBOT_ID.ALPHABOT)) {
m_swerveDrive = AlphaBotConstants.createDrivetrain();
// m_coralOuttake = new CoralOuttake();
// m_algaeIntake = new AlgaeIntake();
m_endEffectorPivot = new EndEffectorPivot();
m_endEffector = new EndEffector();
} else if (ROBOT.robotID.equals(ROBOT.ROBOT_ID.SIM)) {
m_swerveDrive = V2Constants.createDrivetrain();
m_elevator = new Elevator();
m_endEffectorPivot = new EndEffectorPivot();
m_endEffector = new EndEffector();
m_climber = new Climber();
m_endEffectorPivot = new EndEffectorPivot();
m_climberIntake = new ClimberIntake();
m_climber = new Climber();
m_hopperIntake = new HopperIntake();
} else {
// Most likely, the code will crash later on if you get here
DriverStation.reportError(
Expand Down Expand Up @@ -190,6 +191,10 @@ private void initializeSubSystems() {
m_elevator.setDefaultCommand(
new RunElevatorJoystick(m_elevator, () -> -m_driverController.getLeftY()));
}
if (m_endEffectorPivot != null) {
m_endEffectorPivot.setDefaultCommand(
new EndEffectorJoystick(m_endEffectorPivot, () -> -m_driverController.getRightY()));
}
}

private void initAutoChooser() {
Expand Down Expand Up @@ -272,13 +277,12 @@ private void configureAlphaBotBindings() {
.whileTrue(new EndEffectorSetpoint(m_endEffectorPivot, PIVOT_SETPOINT.L3_L2));
}
if (m_endEffector != null) {
// TODO: Make speeds into enum setpoints
m_driverController
.leftTrigger()
.whileTrue(new RunEndEffectorIntake(m_endEffector, 0.4414)); // intake
m_driverController
.rightTrigger()
.whileTrue(new RunEndEffectorIntake(m_endEffector, -0.4414)); // outtake?
// m_driverController
// .leftTrigger()
// .whileTrue(new RunEndEffectorIntake(m_endEffector, 0.4414)); // intake
// m_driverController
// .rightTrigger()
// .whileTrue(new RunEndEffectorIntake(m_endEffector, -0.4414)); // outtake?
}

if (m_algaeIntake != null) {
Expand All @@ -299,9 +303,9 @@ private void configureV2Bindings() {
.a()
.whileTrue(
new ParallelCommandGroup(
new EndEffectorSetpoint(m_endEffectorPivot, PIVOT_SETPOINT.L3_L2),
new SetElevatorSetpoint(
m_elevator, SUPERSTRUCTURE_STATES.L2, () -> m_selectedGamePiece)));
m_elevator, SUPERSTRUCTURE_STATES.L2, () -> m_selectedGamePiece),
new EndEffectorSetpoint(m_endEffectorPivot, PIVOT_SETPOINT.L3_L2)));
m_driverController
.x()
.whileTrue(
Expand All @@ -310,20 +314,29 @@ private void configureV2Bindings() {
m_driverController
.y()
.whileTrue(
new SetElevatorSetpoint(
m_elevator, SUPERSTRUCTURE_STATES.L4, () -> m_selectedGamePiece));
new ParallelCommandGroup(
new SetElevatorSetpoint(
m_elevator, SUPERSTRUCTURE_STATES.L4, () -> m_selectedGamePiece),
new EndEffectorSetpoint(m_endEffectorPivot, PIVOT_SETPOINT.L4)));
m_driverController
.b()
.whileTrue(
new SetElevatorSetpoint(
m_elevator, SUPERSTRUCTURE_STATES.L3, () -> m_selectedGamePiece));
new ParallelCommandGroup(
new SetElevatorSetpoint(
m_elevator, SUPERSTRUCTURE_STATES.L3, () -> m_selectedGamePiece),
new EndEffectorSetpoint(m_endEffectorPivot, PIVOT_SETPOINT.L3_L2)));
m_driverController.povLeft().whileTrue(new RunClimberIntake(m_climberIntake, 0.25));
}

if (m_endEffector != null) {
m_driverController
.leftTrigger()
.whileTrue(new RunEndEffectorIntake(m_endEffector, 0.4414)); // intake
.whileTrue(
new RunEndEffectorIntake(m_endEffector, ROLLER_SPEED.INTAKE_CORAL_HOPPER)); // intake
m_driverController
.rightTrigger()
.whileTrue(
new RunEndEffectorIntake(m_endEffector, ROLLER_SPEED.OUTTAKE_CORAL_REEF)); // outtake
}

if (m_climber != null) {
Expand Down Expand Up @@ -354,6 +367,7 @@ public void autonomousInit() {

public void teleopInit() {
m_swerveDrive.setNeutralMode(SWERVE.MOTOR_TYPE.ALL, NeutralModeValue.Brake);
m_elevator.teleopInit();
}

public void testInit() {
Expand Down
21 changes: 11 additions & 10 deletions src/main/java/frc/robot/commands/RunEndEffectorIntake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,36 +5,37 @@
package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.constants.ENDEFFECTOR.ROLLERS.ROLLER_SPEED;
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 RunEndEffectorIntake extends Command {

private final EndEffector m_EndEffector;
private final double m_speed;
private final EndEffector m_endEffector;
private final ROLLER_SPEED m_speed;

/** Creates a new RunEndEffectorIntake. */
public RunEndEffectorIntake(EndEffector endeffector, double speed) {
m_EndEffector = endeffector;
public RunEndEffectorIntake(EndEffector endEffector, ROLLER_SPEED speed) {
m_endEffector = endEffector;
m_speed = speed;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(m_EndEffector);
addRequirements(m_endEffector);
}

// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
m_endEffector.setPercentOutput(m_speed.get());
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_EndEffector.setPercentOutput(m_speed);
}
public void execute() {}

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

// Returns true when the command should end.
Expand Down
22 changes: 12 additions & 10 deletions src/main/java/frc/robot/commands/elevator/RunElevatorJoystick.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,8 @@ public class RunElevatorJoystick extends Command {
private final Elevator m_elevator;
private final DoubleSupplier m_joystickY;

private boolean m_elevatorHeld = false;

public RunElevatorJoystick(Elevator elevator, DoubleSupplier joystickY) {
m_elevator = elevator;
m_joystickY = joystickY;
Expand All @@ -22,20 +24,20 @@ public void initialize() {}

@Override
public void execute() {
double joystickYDeadbandOutput = MathUtil.applyDeadband(m_joystickY.getAsDouble(), 0.2);
// Adds a Deadband so joystick Ys below 0.1 won't be registered
double joystickYDeadbandOutput = MathUtil.applyDeadband(m_joystickY.getAsDouble(), 0.1);

if (m_elevator.getClosedLoopControlMode() == CONTROL_MODE.OPEN_LOOP) {
// if (joystickYDeadbandOutput < 0)
// joystickYDeadbandOutput *= CLIMBER.kLimitedPercentOutputMultiplier;
if (joystickYDeadbandOutput != 0.0) {
m_elevatorHeld = false;
m_elevator.setControlMode(CONTROL_MODE.OPEN_LOOP);
m_elevator.setJoystickY(joystickYDeadbandOutput);

if (joystickYDeadbandOutput == 0) {
}
if (joystickYDeadbandOutput == 0) {
m_elevator.setControlMode(CONTROL_MODE.CLOSED_LOOP);
if (!m_elevatorHeld) {
m_elevator.holdElevator();
m_elevatorHeld = true;
}
} else if (m_elevator.getClosedLoopControlMode() == CONTROL_MODE.CLOSED_LOOP) {
m_elevator.setDesiredPosition(m_elevator.getHeightMeters() + joystickYDeadbandOutput * 0.5);

if (joystickYDeadbandOutput == 0) m_elevator.holdElevator();
}
}

Expand Down
9 changes: 4 additions & 5 deletions src/main/java/frc/robot/commands/elevator/ScoreNet.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,10 @@

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.constants.ELEVATOR.ELEVATOR_ACCEL_SETPOINT;
import frc.robot.constants.ELEVATOR.ELEVATOR_SETPOINT;
import frc.robot.constants.ROBOT.CONTROL_MODE;
import frc.robot.subsystems.Elevator;

// TODO rework this ENTIRE COMMAND BECAUSE DO WE REALLY NEED THIS AND SETELEVATORSETPOINT
// MYGOSHASGDHGJSADJGSD
public class ScoreNet extends Command {

private final Elevator m_elevator;
Expand All @@ -20,16 +19,16 @@ public ScoreNet(Elevator elevator, ELEVATOR_ACCEL_SETPOINT acceleration) {

@Override
public void initialize() {
m_elevator.setClosedLoopControlMode(CONTROL_MODE.CLOSED_LOOP_NET);
m_elevator.setControlMode(CONTROL_MODE.CLOSED_LOOP_NET);
}

@Override
public void execute() {}

@Override
public void end(boolean interruped) {
m_elevator.setClosedLoopControlMode(CONTROL_MODE.OPEN_LOOP);
m_elevator.holdElevator();
m_elevator.setControlMode(CONTROL_MODE.CLOSED_LOOP);
m_elevator.setDesiredPosition(ELEVATOR_SETPOINT.START_POSITION.getSetpointMeters());
}

public boolean isFinished() {
Expand Down
26 changes: 9 additions & 17 deletions src/main/java/frc/robot/commands/elevator/SetElevatorSetpoint.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,30 +27,26 @@ public SetElevatorSetpoint(
/* This function is the main bulk of the algae toggle logic for elevator. */
@Override
public void initialize() {
m_elevator.setClosedLoopControlMode(CONTROL_MODE.CLOSED_LOOP);
m_elevator.setControlMode(CONTROL_MODE.CLOSED_LOOP);

/* If we want to be able to algae toggle in the middle of an outtake cycle, we need to put this in execute().
No idea why would you want to do that though */
switch (m_stage) {
case STOWED:
m_elevator.setDesiredPosition(
ELEVATOR.ELEVATOR_SETPOINT.START_POSITION.getSetpointMeters()
/ ELEVATOR.drumRotationsToMeters);
ELEVATOR.ELEVATOR_SETPOINT.START_POSITION.getSetpointMeters());
break;
case L1:
if (m_selectedGamePiece.get() == ROBOT.GAME_PIECE.ALGAE)
m_elevator.setDesiredPosition(
ELEVATOR_SETPOINT.PROCESSOR.getSetpointMeters() / ELEVATOR.drumRotationsToMeters);
m_elevator.setDesiredPosition(ELEVATOR_SETPOINT.PROCESSOR.getSetpointMeters());
// No L1 elevator setpoint right now
break;
case L2:
if (m_selectedGamePiece.get() == ROBOT.GAME_PIECE.ALGAE) {
m_elevator.setDesiredPosition(
ELEVATOR_SETPOINT.ALGAE_REEF_INTAKE_LOWER.getSetpointMeters()
/ ELEVATOR.drumRotationsToMeters);
ELEVATOR_SETPOINT.ALGAE_REEF_INTAKE_LOWER.getSetpointMeters());
} else {
m_elevator.setDesiredPosition(
ELEVATOR_SETPOINT.LEVEL_2.getSetpointMeters() / ELEVATOR.drumRotationsToMeters);
m_elevator.setDesiredPosition(ELEVATOR_SETPOINT.LEVEL_2.getSetpointMeters());
}
break;
case L3:
Expand All @@ -59,17 +55,14 @@ public void initialize() {
ELEVATOR_SETPOINT.ALGAE_REEF_INTAKE_UPPER.getSetpointMeters()
/ ELEVATOR.drumRotationsToMeters);
} else {
m_elevator.setDesiredPosition(
ELEVATOR_SETPOINT.LEVEL_3.getSetpointMeters() / ELEVATOR.drumRotationsToMeters);
m_elevator.setDesiredPosition(ELEVATOR_SETPOINT.LEVEL_3.getSetpointMeters());
}
break;
case L4:
if (m_selectedGamePiece.get() == ROBOT.GAME_PIECE.ALGAE) {
m_elevator.setDesiredPosition(
ELEVATOR_SETPOINT.NET.getSetpointMeters() / ELEVATOR.drumRotationsToMeters);
m_elevator.setDesiredPosition(ELEVATOR_SETPOINT.NET.getSetpointMeters());
} else {
m_elevator.setDesiredPosition(
ELEVATOR_SETPOINT.LEVEL_4.getSetpointMeters() / ELEVATOR.drumRotationsToMeters);
m_elevator.setDesiredPosition(ELEVATOR_SETPOINT.LEVEL_4.getSetpointMeters());
}
break;
default:
Expand All @@ -82,8 +75,7 @@ public void execute() {}

@Override
public void end(boolean interruped) {
m_elevator.holdElevator();
m_elevator.setClosedLoopControlMode(CONTROL_MODE.OPEN_LOOP);
m_elevator.setDesiredPosition(ELEVATOR_SETPOINT.START_POSITION.getSetpointMeters());
}

@Override
Expand Down
22 changes: 11 additions & 11 deletions src/main/java/frc/robot/commands/endEffector/EndEffectorJoystick.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.constants.ENDEFFECTOR;
import frc.robot.constants.ENDEFFECTOR.PIVOT;
import frc.robot.constants.ROBOT;
import frc.robot.subsystems.EndEffectorPivot;
import java.util.function.DoubleSupplier;
Expand All @@ -20,9 +20,11 @@ public class EndEffectorJoystick extends Command {

/** Creates a new EndEffectorJoystick. */
public EndEffectorJoystick(EndEffectorPivot endEffectorPivot, DoubleSupplier joystickY) {
// Use addRequirements() here to declare subsystem dependencies.
m_endEffectorPivot = endEffectorPivot;
m_joystickY = joystickY;

// Use addRequirements() here to declare subsystem dependencies.
addRequirements(m_endEffectorPivot);
}

// Called when the command is initially scheduled.
Expand All @@ -41,26 +43,24 @@ public void execute() {
Degrees.of(
MathUtil.clamp(
m_joystickDeadband * 0.5 + m_endEffectorPivot.getCurrentRotation().in(Degrees),
ENDEFFECTOR.minAngle.in(Degrees),
ENDEFFECTOR.maxAngle.in(Degrees)));
PIVOT.minAngle.in(Degrees),
PIVOT.maxAngle.in(Degrees)));
m_endEffectorPivot.setPosition(rotationSetpoint);
} else if (m_endEffectorPivot.getControlMode() == ROBOT.CONTROL_MODE.OPEN_LOOP) {
if (ENDEFFECTOR.limitOpenLoop) {
if (PIVOT.limitOpenLoop) {
// Upper limit
if (m_endEffectorPivot.getCurrentRotation().in(Degrees)
>= ENDEFFECTOR.maxAngle.in(Degrees) - 1)
if (m_endEffectorPivot.getCurrentRotation().in(Degrees) >= PIVOT.maxAngle.in(Degrees) - 1)
m_joystickDeadband = Math.min(m_joystickDeadband, 0);

// Lower limit
if (m_endEffectorPivot.getCurrentRotation().in(Degrees)
<= ENDEFFECTOR.minAngle.in(Degrees) + 1)
if (m_endEffectorPivot.getCurrentRotation().in(Degrees) <= PIVOT.minAngle.in(Degrees) + 1)
m_joystickDeadband = Math.max(m_joystickDeadband, 0);
}
m_endEffectorPivot.setPercentOutput(m_joystickDeadband * ENDEFFECTOR.joystickMultiplier);
m_endEffectorPivot.setPercentOutput(m_joystickDeadband * PIVOT.joystickMultiplier);
}
} else {
if (m_endEffectorPivot.getControlMode() == ROBOT.CONTROL_MODE.OPEN_LOOP)
m_endEffectorPivot.setPercentOutput(m_joystickDeadband * ENDEFFECTOR.joystickMultiplier);
m_endEffectorPivot.setPercentOutput(m_joystickDeadband * PIVOT.joystickMultiplier);
}
}

Expand Down
Loading

0 comments on commit cfed70b

Please sign in to comment.