diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a572cc8..d9cb192 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,7 @@ package frc.robot; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.Pigeon2; -import com.ctre.phoenixpro.hardware.TalonFX; +import com.ctre.phoenix6.hardware.Pigeon2; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.PowerDistribution; @@ -29,11 +27,11 @@ import frc.robot.intake.IntakeSubsystem; import frc.robot.localization.LocalizationSubsystem; import frc.robot.managers.AutoRotate; -import frc.robot.swerve.SwerveModule; -import frc.robot.swerve.SwerveSubsystem; import frc.robot.util.scheduling.LifecycleSubsystemManager; import frc.robot.wrist.Positions; import frc.robot.wrist.WristSubsystem; +import frc.swerve.SwerveSubsystem; + import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.NT4Publisher; @@ -55,35 +53,11 @@ public class Robot extends LoggedRobot { private final RumbleControllerSubsystem rumbleController = new RumbleControllerSubsystem(new XboxController(1)); - private final SwerveModule frontLeft = - new SwerveModule( - Config.SWERVE_FL_CONSTANTS, - new TalonFX(Config.SWERVE_FL_DRIVE_MOTOR_ID), - new TalonFX(Config.SWERVE_FL_STEER_MOTOR_ID), - new CANCoder(Config.SWERVE_FL_CANCODER_ID)); - private final SwerveModule frontRight = - new SwerveModule( - Config.SWERVE_FR_CONSTANTS, - new TalonFX(Config.SWERVE_FR_DRIVE_MOTOR_ID), - new TalonFX(Config.SWERVE_FR_STEER_MOTOR_ID), - new CANCoder(Config.SWERVE_FR_CANCODER_ID)); - private final SwerveModule backLeft = - new SwerveModule( - Config.SWERVE_BL_CONSTANTS, - new TalonFX(Config.SWERVE_BL_DRIVE_MOTOR_ID), - new TalonFX(Config.SWERVE_BL_STEER_MOTOR_ID), - new CANCoder(Config.SWERVE_BL_CANCODER_ID)); - private final SwerveModule backRight = - new SwerveModule( - Config.SWERVE_BR_CONSTANTS, - new TalonFX(Config.SWERVE_BR_DRIVE_MOTOR_ID), - new TalonFX(Config.SWERVE_BR_STEER_MOTOR_ID), - new CANCoder(Config.SWERVE_BR_CANCODER_ID)); + private final FmsSubsystem fmsSubsystem = new FmsSubsystem(); private final ImuSubsystem imu = new ImuSubsystem(new Pigeon2(Config.PIGEON_ID)); - private final SwerveSubsystem swerve = - new SwerveSubsystem(imu, frontRight, frontLeft, backRight, backLeft); + private final SwerveSubsystem swerve = new SwerveSubsystem(imu); private final IntakeSubsystem intake = new IntakeSubsystem(new CANSparkMax(Config.INTAKE_MOTOR_ID, MotorType.kBrushless)); private final WristSubsystem wrist = @@ -194,8 +168,7 @@ private void configureButtonBindings() { () -> driveController.getSidewaysPercentage() == 0 && driveController.getForwardPercentage() == 0 - && driveController.getThetaPercentage() == 0) - .onFalse(swerve.disableXSwerveCommand()); + && driveController.getThetaPercentage() == 0); // operator home wrist operatorController.back().onTrue(wrist.getHomeCommand()); diff --git a/src/main/java/frc/robot/autos/Autobalance.java b/src/main/java/frc/robot/autos/Autobalance.java index b253533..e2dfa6e 100644 --- a/src/main/java/frc/robot/autos/Autobalance.java +++ b/src/main/java/frc/robot/autos/Autobalance.java @@ -13,9 +13,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.fms.FmsSubsystem; import frc.robot.imu.ImuSubsystem; -import frc.robot.swerve.SwerveSubsystem; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.SubsystemPriority; +import frc.swerve.SwerveSubsystem; public class Autobalance extends LifecycleSubsystem { private final SwerveSubsystem swerve; diff --git a/src/main/java/frc/robot/config/Config.java b/src/main/java/frc/robot/config/Config.java index 91c5db0..6191800 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -5,12 +5,9 @@ package frc.robot.config; import com.pathplanner.lib.auto.PIDConstants; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import frc.robot.swerve.SwerveCorner; -import frc.robot.swerve.SwerveModuleConstants; public class Config { public static final String SERIAL_NUMBER = System.getenv("serialnum"); @@ -38,27 +35,18 @@ public class Config { public static final int SWERVE_FL_DRIVE_MOTOR_ID = 2; public static final int SWERVE_FL_STEER_MOTOR_ID = 3; public static final int SWERVE_FL_CANCODER_ID = 10; - public static final SwerveModuleConstants SWERVE_FL_CONSTANTS = - new SwerveModuleConstants( - Rotation2d.fromDegrees(284.68), SwerveCorner.FRONT_LEFT, false, false); + public static final int SWERVE_FR_DRIVE_MOTOR_ID = 4; public static final int SWERVE_FR_STEER_MOTOR_ID = 5; public static final int SWERVE_FR_CANCODER_ID = 11; - public static final SwerveModuleConstants SWERVE_FR_CONSTANTS = - new SwerveModuleConstants( - Rotation2d.fromDegrees(259.89), SwerveCorner.FRONT_RIGHT, false, false); + public static final int SWERVE_BL_DRIVE_MOTOR_ID = 6; public static final int SWERVE_BL_STEER_MOTOR_ID = 7; public static final int SWERVE_BL_CANCODER_ID = 12; - public static final SwerveModuleConstants SWERVE_BL_CONSTANTS = - new SwerveModuleConstants( - Rotation2d.fromDegrees(35.60), SwerveCorner.BACK_LEFT, false, false); + public static final int SWERVE_BR_DRIVE_MOTOR_ID = 8; public static final int SWERVE_BR_STEER_MOTOR_ID = 9; public static final int SWERVE_BR_CANCODER_ID = 13; - public static final SwerveModuleConstants SWERVE_BR_CONSTANTS = - new SwerveModuleConstants( - Rotation2d.fromDegrees(116.37), SwerveCorner.BACK_RIGHT, false, false); public static final int WRIST_MOTOR_ID = 16; public static final double WRIST_GEARING = 1.0 / 96.0; diff --git a/src/main/java/frc/robot/imu/ImuSubsystem.java b/src/main/java/frc/robot/imu/ImuSubsystem.java index 17e6155..6baf610 100644 --- a/src/main/java/frc/robot/imu/ImuSubsystem.java +++ b/src/main/java/frc/robot/imu/ImuSubsystem.java @@ -4,7 +4,6 @@ package frc.robot.imu; -import com.ctre.phoenix.sensors.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -12,6 +11,8 @@ import frc.robot.util.scheduling.SubsystemPriority; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.hardware.Pigeon2; + public class ImuSubsystem extends LifecycleSubsystem { private final Pigeon2 imu; diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java index 35a5a05..fa98b60 100644 --- a/src/main/java/frc/robot/localization/LocalizationSubsystem.java +++ b/src/main/java/frc/robot/localization/LocalizationSubsystem.java @@ -11,9 +11,10 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.fms.FmsSubsystem; import frc.robot.imu.ImuSubsystem; -import frc.robot.swerve.SwerveSubsystem; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.SubsystemPriority; +import frc.swerve.SwerveSubsystem; + import org.littletonrobotics.junction.Logger; public class LocalizationSubsystem extends LifecycleSubsystem { diff --git a/src/main/java/frc/robot/managers/AutoRotate.java b/src/main/java/frc/robot/managers/AutoRotate.java index b480fcf..f403262 100644 --- a/src/main/java/frc/robot/managers/AutoRotate.java +++ b/src/main/java/frc/robot/managers/AutoRotate.java @@ -7,9 +7,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.fms.FmsSubsystem; -import frc.robot.swerve.SwerveSubsystem; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.SubsystemPriority; +import frc.swerve.SwerveSubsystem; + import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/swerve/CustomSlotGains.java b/src/main/java/frc/swerve/CustomSlotGains.java new file mode 100644 index 0000000..f7a3934 --- /dev/null +++ b/src/main/java/frc/swerve/CustomSlotGains.java @@ -0,0 +1,17 @@ +// 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 frc.swerve; + +import com.ctre.phoenix6.configs.Slot0Configs; + +public class CustomSlotGains extends Slot0Configs { + public CustomSlotGains(double kP, double kI, double kD, double kV, double kS) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kV = kV; + this.kS = kS; + } +}