Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-08 16:28:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Preferred operator controls

Browse Source 
Added preferred operator controls
Added manual controls
Renamed misspelled items
This commit is contained in:
McGrathMaggie
2025-02-25 19:47:57 -07:00
parent ddbae68851
commit b341acaa3d
6 changed files with 69 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+9 -9
View File
@@ -237,7 +237,7 @@ public final class Constants {
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, 1, 0);
public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0);
public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
public static final int LIDAR_DIO_CHANNEL = 7;
@@ -430,21 +430,21 @@ public final class Constants {
public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
//Max for endefector = 60%
public static final double L2_SCORE_ENDEFECTOR = -19;
public static final double L2_SCORE_ENDEFFECTOR = -19;
public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
public static final double DEALGAE_L2_EENDEFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
public static final double PRIMED_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
public static final double DEALGAE_L2_ENDEFFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
.withKP(1)
.withKI(0)
.withKD(0);
public static final Slot0Configs ENDEFECTOR_PID = new Slot0Configs()
public static final Slot0Configs ENDEFFECTOR_PID = new Slot0Configs()
.withKP(1)
.withKI(0)
.withKD(0);
src/main/java/frc4388/robot/RobotContainer.java
+15 -4
View File
@@ -18,6 +18,7 @@ import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.utility.controller.XboxController;
@@ -83,7 +84,7 @@ public class RobotContainer {
// private final LED m_robotLED = new LED();
public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
public final Lidar m_lidar = new Lidar();
public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endefectorLimitSwitch);
public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
@@ -452,7 +453,7 @@ public class RobotContainer {
// Cancel button
new JoystickButton(getButtonBox(), ButtonBox.White)
.onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator));
.onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endeffectorStop();}, m_robotElevator));
// Score prep
// Prime 4
@@ -508,9 +509,12 @@ public class RobotContainer {
.onTrue(AprilLidarAlignL2Right);
//Controller Lower Algae Removal
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
.onTrue(leftAlgaeRemove);
//Controller Upper Algae Removal
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
.onTrue(rightAlgaeRemove);
//Button Box Coral Scoring
new JoystickButton(getButtonBox(), ButtonBox.Five)
@@ -541,7 +545,14 @@ public class RobotContainer {
// Cancel button
new JoystickButton(getButtonBox(), ButtonBox.White)
.onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator));
.onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endeffectorStop();}, m_robotElevator));
//Manual Controls
m_robotElevator.setDefaultCommand(new RunCommand(() -> {
m_robotElevator.manualEndeffectorVel(getDeadbandedOperatorController().getLeftY());
m_robotElevator.manualElevatorVel(getDeadbandedOperatorController().getRightY());
}, m_robotElevator)
.withName("Default Manual Controls"));
}
src/main/java/frc4388/robot/RobotMap.java
+1 -1
View File
@@ -57,7 +57,7 @@ public class RobotMap {
public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
public final DigitalInput endefectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
void configureDriveMotorControllers() {}
src/main/java/frc4388/robot/commands/waitElevatorRefrence.java
+1 -1
View File
@@ -31,6 +31,6 @@ public class waitElevatorRefrence extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return elevator.elevatorAtRefrence();
return elevator.elevatorAtReference();
}
}
src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java
+1 -1
View File
@@ -31,6 +31,6 @@ public class waitEndefectorRefrence extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return elevator.endefectorAtRefrence();
return elevator.endeffectorAtReference();
}
}
src/main/java/frc4388/robot/subsystems/Elevator.java
+42 -34
View File
@@ -24,20 +24,20 @@ import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
public class Elevator extends SubsystemBase {
/** Creates a new Elevator. */
private TalonFX elevatorMotor;
private TalonFX endefectorMotor;
private TalonFX endeffectorMotor;
private long wait = 0;
private long maxWait = 1000;
private double elevatorRefrence = 0;
private double endefectorRefrence = 0;
private double endeffectorRefrence = 0;
private DigitalInput basinBeamBreak;
private DigitalInput endefectorLimitSwitch;
private DigitalInput endeffectorLimitSwitch;
public enum CoordinationState {
Waiting, // for coral into the though
WatingBeamTriped, //once the beam break trips
WatingBeamTripped, //once the beam break trips
Ready, // Has coral in endefector
Hovering, // Has coral in endefector
L2Score,
@@ -53,18 +53,18 @@ public class Elevator extends SubsystemBase {
private CoordinationState currentState;
public Elevator(TalonFX elevatorTalonFX, TalonFX endefectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endefectorLimitSwitch) {
public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch) {
elevatorMotor = elevatorTalonFX;
endefectorMotor = endefectorTalonFX;
endeffectorMotor = endeffectorTalonFX;
this.basinBeamBreak = basinLimitSwitch;
this.endefectorLimitSwitch = endefectorLimitSwitch;
this.endeffectorLimitSwitch = endeffectorLimitSwitch;
elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
endefectorMotor.setNeutralMode(NeutralModeValue.Brake);
endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
endefectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFECTOR_PID);
endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
currentState = CoordinationState.Ready;
}
@@ -72,7 +72,7 @@ public class Elevator extends SubsystemBase {
private void PIDPosition(TalonFX motor, double position) {
if (motor == elevatorMotor) elevatorRefrence = position;
else endefectorRefrence = position;
else endeffectorRefrence = position;
var request = new PositionDutyCycle(position);
motor.setControl(request);
@@ -82,8 +82,8 @@ public class Elevator extends SubsystemBase {
elevatorMotor.set(0);
}
public void endefectorStop() {
endefectorMotor.set(0);
public void endeffectorStop() {
endeffectorMotor.set(0);
}
public void transitionState(CoordinationState state) {
@@ -92,79 +92,79 @@ public class Elevator extends SubsystemBase {
case Waiting: {
wait = System.currentTimeMillis() + maxWait;
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
break;
}
case WatingBeamTriped: {
case WatingBeamTripped: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
break;
}
case Ready: {
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
break;
}
case Hovering: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
break;
}
case L2Score: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.L2_SCORE_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
case PrimedFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR);
break;
}
case ScoringFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
case PrimedThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
break;
}
case ScoringThree: {
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
case BallRemoverL2Primed: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
break;
}
case BallRemoverL2Go: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
case BallRemoverL3Primed: {
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
break;
}
case BallRemoverL3Go: {
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
break;
}
@@ -175,7 +175,7 @@ public class Elevator extends SubsystemBase {
}
public boolean elevatorAtRefrence() {
public boolean elevatorAtReference() {
// double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.5;
@@ -187,11 +187,11 @@ public class Elevator extends SubsystemBase {
return atPos;
}
public boolean endefectorAtRefrence() {
public boolean endeffectorAtReference() {
// double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble();
double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
return Math.abs(endefectorRefrence - endefectorPosition) <= 0.2;
return Math.abs(endeffectorRefrence - endeffectorPosition) <= 0.2;
}
// public void driveElevatorStick(Translation2d stick) {
// if (stick.getNorm() > 0.05) {
@@ -210,12 +210,20 @@ public class Elevator extends SubsystemBase {
// }
private void periodicReady() {
if (elevatorAtRefrence())
if (elevatorAtReference())
transitionState(CoordinationState.Hovering);
}
private void periodicScoring() {
if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
if (!endeffectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
}
public void manualElevatorVel(double velocity) {
elevatorMotor.set(velocity);
}
public void manualEndeffectorVel(double velocity) {
endeffectorMotor.set(velocity);
}
@Override
@@ -223,12 +231,12 @@ public class Elevator extends SubsystemBase {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0);
SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
SmartDashboard.putString("State", currentState.toString());
if (currentState == CoordinationState.Waiting) {
// periodicWaiting();
} else if (currentState == CoordinationState.WatingBeamTriped) {
} else if (currentState == CoordinationState.WatingBeamTripped) {
// periodicWaitingTripped();
} else if (currentState == CoordinationState.Ready) {
periodicReady();