Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' into kraken_swerve

Browse Source
This commit is contained in:
Bradley Bickford 2025-01-20 20:12:40 -05:00
commit a96d96fecb
6 changed files with 170 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/main/java/frc/robot/RobotContainer.java
View File

@ -14,12 +14,16 @@ import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.Indexer;
import frc.robot.subsystems.Manipulator;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
public class RobotContainer {
@ -40,6 +44,8 @@ public class RobotContainer {
private CommandXboxController driver;
private CommandXboxController operator;
private SendableChooser<Command> autoChooser;
public RobotContainer() {
arm = new Arm();
@ -58,8 +64,12 @@ public class RobotContainer {
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
autoChooser = AutoBuilder.buildAutoChooser();
configureButtonBindings();
configureNamedCommands();
configureShuffleboard();
}
@ -86,7 +96,7 @@ public class RobotContainer {
);
elevator.setDefaultCommand(
elevator.runElevator(operator::getLeftY)
elevator.runAssistedElevator(operator::getLeftY)
);
indexer.setDefaultCommand(
@ -148,10 +158,22 @@ public class RobotContainer {
);
}
private void configureNamedCommands() {
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
}
//creates tabs and transforms them on the shuffleboard
private void configureShuffleboard() {
ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
Shuffleboard.selectTab(OIConstants.kAutoTab);
autoTab.add("Auto Selection", autoChooser)
.withSize(2, 1)
.withPosition(0, 0)
.withWidget(BuiltInWidgets.kComboBoxChooser);
sensorTab.addDouble("ElevatorPosition", elevator::getEncoderPosition)
.withSize(2, 1)
.withPosition(0, 0)
@ -164,7 +186,7 @@ public class RobotContainer {
}
public Command getAutonomousCommand() {
return new PrintCommand("NO AUTO DEFINED");
return autoChooser.getSelected();
}
//teleop routines
@ -222,4 +244,14 @@ public class RobotContainer {
() -> elevatorFirst
);
}
/*
* A moveManipulator method that will guarantee a safe movement.
* Here in case we need want to skip moveManipulator debugging
*/
@SuppressWarnings("unused")
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
.andThen(arm.goToSetpoint(armPosition, 2));
}
}

25
src/main/java/frc/robot/constants/AutoConstants.java
View File

@ -1,5 +1,13 @@
package frc.robot.constants;
import java.io.IOException;
import org.json.simple.parser.ParseException;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
public class AutoConstants {
@ -15,4 +23,21 @@ public class AutoConstants {
// Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
// TODO This is a constant being managed like a static rewriteable variable
public static RobotConfig kRobotConfig;
public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
new PIDConstants(kPXController, 0, 0),
new PIDConstants(kPYController, 0, 0)
);
static {
try {
kRobotConfig = RobotConfig.fromGUISettings();
} catch (IOException | ParseException e) {
System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?");
e.printStackTrace();
}
}
}

1
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@ -43,6 +43,7 @@ public class ElevatorConstants {
public static final double kElevatorL2AlgaePosition = 0;
public static final double kElevatorL3AlgaePosition = 0;
public static final double kElevatorBracePosition = 0;
public static final double kElevatorMaxHeight = 0;
// 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = 1;

1
src/main/java/frc/robot/constants/OIConstants.java
View File

@ -6,5 +6,6 @@ public class OIConstants {
public static final double kDriveDeadband = 0.05;
public static final String kAutoTab = "Auto Tab";
public static final String kSensorsTab = "Sensors Tab";
}

85
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -4,9 +4,14 @@
package frc.robot.subsystems;
import java.util.Optional;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
import com.pathplanner.lib.auto.AutoBuilder;
import com.studica.frc.AHRS;
import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
@ -15,10 +20,10 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.ADIS16470_IMU;
import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.AutoConstants;
import frc.robot.constants.DrivetrainConstants;
import frc.robot.constants.OIConstants;
@ -30,7 +35,7 @@ public class Drivetrain extends SubsystemBase {
protected MAXSwerveModule m_rearRight;
// The gyro sensor
private ADIS16470_IMU m_gyro;
private AHRS ahrs;
// Odometry class for tracking robot pose
private SwerveDriveOdometry m_odometry;
@ -61,11 +66,41 @@ public class Drivetrain extends SubsystemBase {
DrivetrainConstants.kBackRightChassisAngularOffset
);
m_gyro = new ADIS16470_IMU();
ahrs = new AHRS(NavXComType.kMXP_SPI);
m_odometry = new SwerveDriveOdometry(
DrivetrainConstants.kDriveKinematics,
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
Rotation2d.fromDegrees(ahrs.getAngle()),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
});
AutoBuilder.configure(
this::getPose,
this::resetOdometry,
this::getCurrentChassisSpeeds,
this::driveWithChassisSpeeds,
AutoConstants.kPPDriveController,
AutoConstants.kRobotConfig,
() -> {
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
this
);
}
@Override
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(
Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
@ -74,17 +109,21 @@ public class Drivetrain extends SubsystemBase {
});
}
@Override
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
});
public ChassisSpeeds getCurrentChassisSpeeds() {
return DrivetrainConstants.kDriveKinematics.toChassisSpeeds(
m_frontLeft.getState(),
m_frontRight.getState(),
m_rearLeft.getState(),
m_rearRight.getState()
);
}
public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);
SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(newStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
setModuleStates(newStates);
}
/**
@ -103,7 +142,7 @@ public class Drivetrain extends SubsystemBase {
*/
public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition(
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
@ -144,7 +183,7 @@ public class Drivetrain extends SubsystemBase {
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
Rotation2d.fromDegrees(getGyroValue()))
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@ -194,7 +233,11 @@ public class Drivetrain extends SubsystemBase {
/** Zeroes the heading of the robot. */
public void zeroHeading() {
m_gyro.reset();
ahrs.reset();;
}
public double getGyroValue() {
return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
}
/**
@ -203,7 +246,7 @@ public class Drivetrain extends SubsystemBase {
* @return the robot's heading in degrees, from -180 to 180
*/
public double getHeading() {
return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees();
return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
}
/**
@ -212,6 +255,6 @@ public class Drivetrain extends SubsystemBase {
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
return m_gyro.getRate(IMUAxis.kZ) * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
}
}

45
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -97,8 +97,13 @@ public class Elevator extends SubsystemBase {
return motionTarget > ElevatorConstants.kElevatorBracePosition;
}
//manual command that keeps ouput speed consistent no matter the direction
public Command runElevator(DoubleSupplier speed) {
/**
* A manual translation command that will move the elevator using a consistent velocity disregarding direction
*
* @param speed How fast the elevator moves
* @return Sets motor voltage to move the elevator relative to the speed parameter
*/
public Command runAssistedElevator(DoubleSupplier speed) {
return run(() -> {
double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
@ -108,11 +113,29 @@ public class Elevator extends SubsystemBase {
) + feedForward.calculate(realSpeedTarget);
elevatorMotor1.setVoltage(voltsOut);
}).until(bottomLimitSwitch::get);
}).until(
() -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kElevatorMaxHeight);
}
/**
* A manual translation command that uses feed forward calculation to maintain position
*
* @param speed The speed at which the elevator translates
* @return Sets motor voltage to translate the elevator and maintain position
*/
public Command runManualElevator(double speed) {
return run(() -> {
elevatorMotor1.set(speed);
});
}
//go to setpoint command
/**
* Moves the elevator to a target destination (setpoint)
*
* @param setpoint Target destination of the subsystem
* @param timeout Time to achieve the setpoint before quitting
* @return Sets motor voltage to achieve the target destination
*/
public Command goToSetpoint(double setpoint, double timeout) {
return run(() -> {
double voltsOut = positionController.calculate(
@ -126,7 +149,21 @@ public class Elevator extends SubsystemBase {
).withTimeout(timeout);
}
/**
* Returns the current encoder position
*
* @return Current encoder position
*/
public double getEncoderPosition() {
return encoder.getPosition();
}
/**
* Returns the value of the bottom limit switch on the elevator (false = disabled, true = enabled)
*
* @return The value of bottomLimitSwitch
*/
public boolean getBottomLimitSwitch() {
return bottomLimitSwitch.get();
}
}