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

Intergrate endefector into elevator for cordination

Browse Source
This commit is contained in:
C4llSiqn
2025-01-27 17:29:25 -07:00
parent a7ac44c25d
commit 2b376d741c
5 changed files with 101 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+15 -16
View File
@@ -390,34 +390,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
+16 -35
View File
@@ -49,7 +49,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
import frc4388.robot.subsystems.Vision;
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 +74,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);
@@ -214,7 +213,20 @@ public class RobotContainer {
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
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionWaiting(), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionReady(), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionScoringThree(), m_robotElevator));
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotElevator.transitionScoringFour(), m_robotElevator));
// ? /* Programer Buttons (Controller 3)*/
@@ -232,37 +244,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
+64 -19
View File
@@ -9,52 +9,97 @@ 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 CordinationState {
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 CordinationState 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 = CordinationState.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 transitionWaiting() {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
currentState = CordinationState.Waiting;
}
private void periodicWaiting() {
if (basinLimitSwitch.get()) transitionReady();
}
public void transitionReady() {
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
currentState = CordinationState.Ready;
}
public void transitionScoringThree() {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
currentState = CordinationState.ScoringThree;
}
private void periodicScoring() {
if (!endefectorLimitSwitch.get()) transitionWaiting();
}
public void transitionScoringFour() {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
currentState = CordinationState.ScoringFour;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
if (currentState == CordinationState.Waiting) {
periodicWaiting();
} else if (currentState == CordinationState.ScoringThree || currentState == CordinationState.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
}
}