Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into Debug-Autos

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-28 17:09:27 -07:00
parent 526cea9e64 a7052a54d2
commit 6aad044141
5 changed files with 109 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+16 -17
View File
@@ -391,34 +391,33 @@ public final class Constants {
}
public static final class ElevatorConstants {
public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
public static final double GEAR_RATIO = 36.0;
public static final double LEVEL_1 = 0 * GEAR_RATIO;
public static final double LEVEL_2 = 5 * GEAR_RATIO;
public static final double ELEVATOR_MAX_HEIGHT = 5 * GEAR_RATIO;
public static final double ELEVATOR_SPEED_UP = 1 * GEAR_RATIO;
public static final double ELEVATOR_SPEED_DOWN = -1 * GEAR_RATIO;
public static final int BASIN_LIMIT_SWITCH = 0; // TODO: FIND
public static final int ENDEFFECTOR_LIMIT_SWITCH = 1; // TODO: FIND
public static final double GEAR_RATIO_ELEVATOR = 36.0;
public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
public static final double WAITING_POSITION_ELEVATOR = 2 * GEAR_RATIO_ELEVATOR; // TODO: find 4-6 off the ground
public static final double MAX_POSITION_ELEVATOR = 20 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
public static final double GEAR_RATIO_ENDEFECTOR = 100.0;
public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
public static final double SCORING_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
.withKP(1)
.withKI(0)
.withKD(0);
}
public static class EndeffectorConstants {
public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
public static final double GEAR_RATIO = 100.0;
public static final double TOP = 0.25 * GEAR_RATIO;
public static final double MIDDLE = 0.0 * GEAR_RATIO;
public static final double BOTTOM = -0.25 * GEAR_RATIO;
public static final Slot0Configs ENDEFECTOR_PID = new Slot0Configs()
.withKP(1)
.withKI(0)
.withKD(0);
}
}
src/main/java/frc4388/robot/RobotContainer.java
+18 -36
View File
@@ -47,9 +47,10 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
// Subsystems
// import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Vision;
import frc4388.robot.subsystems.Elevator.CoordinationState;
import frc4388.robot.subsystems.Lidar;
import frc4388.robot.subsystems.Elevator;
import frc4388.robot.subsystems.Endeffector;
// import frc4388.robot.subsystems.Endeffector;
import frc4388.robot.subsystems.SwerveDrive;
// Utilites
@@ -74,8 +75,7 @@ public class RobotContainer {
// private final LED m_robotLED = new LED();
public final Vision m_vision = new Vision(m_robotMap.camera);
public final Lidar m_lidar = new Lidar();
public final Elevator m_robotELevator= new Elevator(m_robotMap.elevator);
public final Endeffector m_robotEndeffector = new Endeffector(m_robotMap.endeffector);
public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endefectorLimitSwitch);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
@@ -220,8 +220,21 @@ public class RobotContainer {
.onTrue(AutoGotoPosition);
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(()->{}, m_robotSwerveDrive));
// creates an empty command & requires the swerve drive, subsystems can run only 1 command at a time
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator));
// ? /* Programer Buttons (Controller 3)*/
@@ -239,37 +252,6 @@ public class RobotContainer {
true, false))
.onFalse(new InstantCommand());
/*DPad for Level 1 and 2*/
new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.9)
.onTrue(new InstantCommand(() -> m_robotELevator.PIDLevel2()))
.onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop()));
new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.9)
.onTrue(new InstantCommand(() -> m_robotELevator.PIDLevel1()))
.onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop()));
/*Free Brid Mode With Bummpers*/
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotELevator.elevatorDown()))
.onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotELevator.elevatorUp()))
.onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop()));
/*Endeffector Controls*/
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotEndeffector.PIDTop()))
.onFalse(new InstantCommand(() -> m_robotEndeffector.endEffectorStop()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> m_robotEndeffector.PIDMiddle()))
.onFalse(new InstantCommand(() -> m_robotEndeffector.endEffectorStop()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotEndeffector.PIDBottom()))
.onFalse(new InstantCommand(() -> m_robotEndeffector.endEffectorStop()));;
}
/**
src/main/java/frc4388/robot/RobotMap.java
+5 -3
View File
@@ -18,8 +18,8 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
import edu.wpi.first.wpilibj.DigitalInput;
import frc4388.robot.Constants.ElevatorConstants;
import frc4388.robot.Constants.EndeffectorConstants;
// import edu.wpi.first.wpilibj.motorcontrol.Spark;
// import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
@@ -53,9 +53,11 @@ public class RobotMap {
/* Elevator Subsystem */
public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
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);
/*Endeffector Subsystem*/
public final TalonFX endeffector = new TalonFX(EndeffectorConstants.ENDEFFECTOR_ID.id);
void configureDriveMotorControllers() {}
}
src/main/java/frc4388/robot/subsystems/Elevator.java
+70 -19
View File
@@ -9,52 +9,103 @@ import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ElevatorConstants;
public class Elevator extends SubsystemBase {
/** Creates a new Elevator. */
private TalonFX elevatorMotor;
private TalonFX endefectorMotor;
public Elevator(TalonFX elevatorTalonFX) {
private DigitalInput basinLimitSwitch;
private DigitalInput endefectorLimitSwitch;
public enum CoordinationState {
Waiting, // for coral into the though
Ready, // Has coral in enefector
ScoringThree, // Arm and elevator in the level three position
ScoringFour // Arm and elevator in the level four position
}
private CoordinationState currentState;
public Elevator(TalonFX elevatorTalonFX, TalonFX endefectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endefectorLimitSwitch) {
elevatorMotor = elevatorTalonFX;
endefectorMotor = endefectorTalonFX;
this.basinLimitSwitch = basinLimitSwitch;
this.endefectorLimitSwitch = endefectorLimitSwitch;
elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
endefectorMotor.setNeutralMode(NeutralModeValue.Brake);
elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
endefectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFECTOR_PID);
currentState = CoordinationState.Ready;
}
//PID methods
public void PIDPosition(double position) {
private void PIDPosition(TalonFX motor, double position) {
var request = new PositionVoltage(position);
elevatorMotor.setControl(request);
}
public void PIDLevel1() {
PIDPosition(ElevatorConstants.LEVEL_1);
}
public void PIDLevel2() {
PIDPosition(ElevatorConstants.LEVEL_2);
}
public void elevatorUp() {
elevatorMotor.set(ElevatorConstants.ELEVATOR_SPEED_UP);
}
public void elevatorDown() {
elevatorMotor.set(ElevatorConstants.ELEVATOR_SPEED_UP);
}
public void elevatorStop() {
elevatorMotor.set(0);
}
public void endefectorStop() {
endefectorMotor.set(0);
}
public void transitionState(CoordinationState state) {
currentState = state;
switch (currentState) {
case Waiting: {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
break;
}
case Ready: {
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
break;
}
case ScoringThree: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
break;
}
case ScoringFour: {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
break;
}
}
}
private void periodicWaiting() {
if (basinLimitSwitch.get()) transitionState(CoordinationState.Ready);
}
private void periodicScoring() {
if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
if (currentState == CoordinationState.Waiting) {
periodicWaiting();
} else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
periodicScoring();
}
}
}
src/main/java/frc4388/robot/subsystems/Endeffector.java
-51
View File
@@ -1,51 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.subsystems;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ElevatorConstants;
import frc4388.robot.Constants.EndeffectorConstants;
public class Endeffector extends SubsystemBase {
/** Creates a new Endefector. */
private TalonFX endeffectorMotor;
public Endeffector(TalonFX endffectorTalonFX) {
endeffectorMotor = endffectorTalonFX;
endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
endeffectorMotor.getConfigurator().apply(EndeffectorConstants.ENDEFECTOR_PID);
}
public void PIDPosition(double position) {
var request = new PositionVoltage(position);
endeffectorMotor.setControl(request);
}
public void PIDTop() {
PIDPosition(EndeffectorConstants.TOP);
}
public void PIDMiddle() {
PIDPosition(EndeffectorConstants.MIDDLE);
}
public void PIDBottom() {
PIDPosition(EndeffectorConstants.BOTTOM);
}
public void endEffectorStop() {
endeffectorMotor.set(0);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}