diff --git a/src/main/java/frc/robot/Commands/Desaccumuler.java b/src/main/java/frc/robot/Commands/Desaccumuler.java index b99aa0f..45ac278 100644 --- a/src/main/java/frc/robot/Commands/Desaccumuler.java +++ b/src/main/java/frc/robot/Commands/Desaccumuler.java @@ -22,9 +22,16 @@ public class Desaccumuler extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(!accumulateur.photocell() || !accumulateur.photocell2()){accumulateur.desaccumule(0.1);} - - } + /*if(accumulateur.photocell() || accumulateur.photocell2()){ + accumulateur.desaccumule(0); + accumulateur.masterslave2(); + + } + else{ + */ accumulateur.desaccumule(0.4); + accumulateur.masterslave2(); + } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { @@ -33,6 +40,6 @@ public class Desaccumuler extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return /*accumulateur.photocell()==true || accumulateur.photocell2()==true*/ false; } } diff --git a/src/main/java/frc/robot/Commands/Lancer.java b/src/main/java/frc/robot/Commands/Lancer.java index bd42d4a..248310e 100644 --- a/src/main/java/frc/robot/Commands/Lancer.java +++ b/src/main/java/frc/robot/Commands/Lancer.java @@ -27,13 +27,18 @@ public class Lancer extends Command { @Override public void execute() { lanceur.lance(); - accumulateur.Petitlanceur(0.7); + accumulateur.Petitlanceur(0.9); accumulateur.desaccumule(0.2); + lanceur.masterslave(); + accumulateur.rouesbleue(0.7); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { lanceur.lance(0); + accumulateur.desaccumule(0); + accumulateur.Petitlanceur(0); + accumulateur.rouesbleue(0); } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 842a52d..5be0daa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,7 +6,14 @@ package frc.robot; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; + import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -18,13 +25,50 @@ import frc.robot.Commands.FollowAprilTag; import frc.robot.Commands.LEDactive; import frc.robot.Commands.Lancer; import frc.robot.Subsystems.Accumulateur; +import frc.robot.Subsystems.CommandSwerveDrivetrain; import frc.robot.Subsystems.LED; //import frc.robot.Subsystems.Drive; import frc.robot.Subsystems.Lanceur; import frc.robot.Subsystems.Limelight3G; +import frc.robot.generated.TunerConstants; //import com.pathplanner.lib.auto.AutoBuilder; //import com.pathplanner.lib.auto.NamedCommands; public class RobotContainer { + private double MaxSpeed = TunerConstants.kSpeedAt12VoltsMps; // kSpeedAt12VoltsMps desired top speed + private double MaxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity + + /* Setting up bindings for necessary control of the swerve drive platform */ // My joystick + private final CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain + + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.15).withRotationalDeadband(MaxAngularRate * 0.15) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // I want field-centric + // driving in open loop + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + + private final Telemetry logger = new Telemetry(MaxSpeed); + private void configureBindings() { + drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically + drivetrain.applyRequest(() -> drive.withVelocityX(-manette.getLeftY() * MaxSpeed*0.25) // Drive forward with + // negative Y (forward) + .withVelocityY(-manette.getLeftX() * MaxSpeed*0.25) // Drive left with negative X (left) + .withRotationalRate(-manette.getRightX() * MaxAngularRate*0.25) // Drive counterclockwise with negative X (left) + )); + + manette.a().whileTrue(drivetrain.applyRequest(() -> brake)); + manette.b().whileTrue(drivetrain + .applyRequest(() -> point.withModuleDirection(new Rotation2d(-manette.getLeftY(), -manette.getLeftX())))); + + // reset the field-centric heading on left bumper press + manette.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative())); + + if (Utils.isSimulation()) { + drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); + } + drivetrain.registerTelemetry(logger::telemeterize); + } + // private final SendableChooser autoChooser; Lanceur lanceur= new Lanceur(); Accumulateur accumulateur = new Accumulateur(); @@ -43,23 +87,23 @@ public class RobotContainer { dashboard.addCamera("limelight3", "limelight3","limelight.local:5800") .withSize(3,4) .withPosition(2,0); - configureBindings(); + final double MaxSpeed = TunerConstants.kSpeedAt12VoltsMps; // kSpeedAt12VoltsMps desired top speed + final double MaxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity + + /* Setting up bindings for necessary control of the swerve drive platform */ // My joystick + + //drive.setDefaultCommand(new RunCommand(()->{ // drive.drive(-MathUtil.applyDeadband(manette.getRightX(),0.2), MathUtil.applyDeadband(-manette.getRightY(),0.2), MathUtil.applyDeadband(-manette.getRightX(), 0.2)); //},drive)); // dashboard.add("autochooser",autoChooser) //.withSize(1,1) // .withPosition(8,0); - } - - private void configureBindings() { - manette.x().whileTrue(new Lancer(lanceur,accumulateur)); manette.leftBumper().toggleOnTrue(new FollowAprilTag(limelight3G, lanceur)); manette.a().whileTrue(new Desaccumuler(accumulateur)); manette.y().whileTrue(new LEDactive(accumulateur, led)); } - public Command getAutonomousCommand() { return null; // return autoChooser.getSelected(); diff --git a/src/main/java/frc/robot/Subsystems/Accumulateur.java b/src/main/java/frc/robot/Subsystems/Accumulateur.java index 3c89b97..4c9fda5 100644 --- a/src/main/java/frc/robot/Subsystems/Accumulateur.java +++ b/src/main/java/frc/robot/Subsystems/Accumulateur.java @@ -47,6 +47,7 @@ public class Accumulateur extends SubsystemBase { } public void masterslave2(){ rouesbleues.follow(strap); + rouesbleues.setInverted(true); } public void Petitlanceur(double vitesse){ roueRouge1.set(vitesse); @@ -55,6 +56,9 @@ public class Accumulateur extends SubsystemBase { public void desaccumule(){ desaccumule(vitesse.getDouble(0.9)); } + public void rouesbleue(double vitesse){ + rouesbleues.set(vitesse); + } @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/Subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/Subsystems/CommandSwerveDrivetrain.java new file mode 100644 index 0000000..4931dd9 --- /dev/null +++ b/src/main/java/frc/robot/Subsystems/CommandSwerveDrivetrain.java @@ -0,0 +1,83 @@ +package frc.robot.Subsystems; + +import java.util.function.Supplier; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +/** + * Class that extends the Phoenix SwerveDrivetrain class and implements + * subsystem so it can be used in command-based projects easily. + */ +public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean hasAppliedOperatorPerspective = false; + + public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { + super(driveTrainConstants, OdometryUpdateFrequency, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + } + public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { + super(driveTrainConstants, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + } + + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } + + @Override + public void periodic() { + /* Periodically try to apply the operator perspective */ + /* If we haven't applied the operator perspective before, then we should apply it regardless of DS state */ + /* This allows us to correct the perspective in case the robot code restarts mid-match */ + /* Otherwise, only check and apply the operator perspective if the DS is disabled */ + /* This ensures driving behavior doesn't change until an explicit disable event occurs during testing*/ + if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance().ifPresent((allianceColor) -> { + this.setOperatorPerspectiveForward( + allianceColor == Alliance.Red ? RedAlliancePerspectiveRotation + : BlueAlliancePerspectiveRotation); + hasAppliedOperatorPerspective = true; + }); + } + } +} diff --git a/src/main/java/frc/robot/Subsystems/Lanceur.java b/src/main/java/frc/robot/Subsystems/Lanceur.java index 1055cde..2246ea7 100644 --- a/src/main/java/frc/robot/Subsystems/Lanceur.java +++ b/src/main/java/frc/robot/Subsystems/Lanceur.java @@ -61,7 +61,7 @@ public class Lanceur extends SubsystemBase { lanceur1.set(vitesse); } public void lance(){ - lance(vitesse.getDouble(0.2)); + lance(vitesse.getDouble(0.9)); } public void tourelRotation(double x, double y, double rotation){ tourelle.set(rotation); diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java new file mode 100644 index 0000000..c7e3c2c --- /dev/null +++ b/src/main/java/frc/robot/Telemetry.java @@ -0,0 +1,110 @@ +package frc.robot; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class Telemetry { + private final double MaxSpeed; + + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public Telemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + } + + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + + /* Robot speeds for general checking */ + private final NetworkTable driveStats = inst.getTable("Drive"); + private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); + private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); + private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); + private final DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish(); + + /* Keep a reference of the last pose to calculate the speeds */ + private Pose2d m_lastPose = new Pose2d(); + private double lastTime = Utils.getCurrentTimeSeconds(); + + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; + + /* Accept the swerve drive state and telemeterize it to smartdashboard */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the pose */ + Pose2d pose = state.Pose; + fieldTypePub.set("Field2d"); + fieldPub.set(new double[] { + pose.getX(), + pose.getY(), + pose.getRotation().getDegrees() + }); + + /* Telemeterize the robot's general speeds */ + double currentTime = Utils.getCurrentTimeSeconds(); + double diffTime = currentTime - lastTime; + lastTime = currentTime; + Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation(); + m_lastPose = pose; + + Translation2d velocities = distanceDiff.div(diffTime); + + speed.set(velocities.getNorm()); + velocityX.set(velocities.getX()); + velocityY.set(velocities.getY()); + odomPeriod.set(state.OdometryPeriod); + + /* Telemeterize the module's states */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + } + } +} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java new file mode 100644 index 0000000..c31bedc --- /dev/null +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -0,0 +1,167 @@ +package frc.robot.generated; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; + +import edu.wpi.first.math.util.Units; +import frc.robot.Subsystems.CommandSwerveDrivetrain; + +// Generated by the Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class TunerConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.2) + .withKS(0).withKV(1.5).withKA(0); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(3).withKI(0).withKD(0) + .withKS(0).withKV(0).withKA(0); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final double kSlipCurrentA = 150.0; + + // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration cancoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // Theoretical free speed (m/s) at 12v applied output; + // This needs to be tuned to your individual robot + public static final double kSpeedAt12VoltsMps = 5.21; + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.5714285714285716; + + private static final double kDriveGearRatio = 6.122448979591837; + private static final double kSteerGearRatio = 21.428571428571427; + private static final double kWheelRadiusInches = 2; + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final String kCANbusName = "swerve"; + private static final int kPigeonId = 13; + + + // These are only used for simulation + private static final double kSteerInertia = 0.00001; + private static final double kDriveInertia = 0.001; + // Simulated voltage necessary to overcome friction + private static final double kSteerFrictionVoltage = 0.25; + private static final double kDriveFrictionVoltage = 0.25; + + private static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANbusName(kCANbusName) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withWheelRadius(kWheelRadiusInches) + .withSlipCurrent(kSlipCurrentA) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) + .withSpeedAt12VoltsMps(kSpeedAt12VoltsMps) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage) + .withFeedbackSource(SteerFeedbackType.FusedCANcoder) + .withCouplingGearRatio(kCoupleRatio) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withCANcoderInitialConfigs(cancoderInitialConfigs); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 2; + private static final int kFrontLeftSteerMotorId = 6; + private static final int kFrontLeftEncoderId = 9; + private static final double kFrontLeftEncoderOffset = -0.046142578125; + private static final boolean kFrontLeftSteerInvert = true; + + private static final double kFrontLeftXPosInches = 11.625; + private static final double kFrontLeftYPosInches = 11.625; + + // Front Right + private static final int kFrontRightDriveMotorId = 4; + private static final int kFrontRightSteerMotorId = 5; + private static final int kFrontRightEncoderId = 12; + private static final double kFrontRightEncoderOffset = -0.116455078125; + private static final boolean kFrontRightSteerInvert = true; + + private static final double kFrontRightXPosInches = 11.625; + private static final double kFrontRightYPosInches = -11.625; + + // Back Left + private static final int kBackLeftDriveMotorId = 3; + private static final int kBackLeftSteerMotorId = 7; + private static final int kBackLeftEncoderId = 10; + private static final double kBackLeftEncoderOffset = -0.046630859375; + private static final boolean kBackLeftSteerInvert = true; + + private static final double kBackLeftXPosInches = -11.625; + private static final double kBackLeftYPosInches = 11.625; + + // Back Right + private static final int kBackRightDriveMotorId = 18; + private static final int kBackRightSteerMotorId = 8; + private static final int kBackRightEncoderId = 11; + private static final double kBackRightEncoderOffset = -0.016357421875; + private static final boolean kBackRightSteerInvert = true; + + private static final double kBackRightXPosInches = -11.625; + private static final double kBackRightYPosInches = -11.625; + + + private static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, Units.inchesToMeters(kFrontLeftXPosInches), Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide) + .withSteerMotorInverted(kFrontLeftSteerInvert); + private static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, Units.inchesToMeters(kFrontRightXPosInches), Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide) + .withSteerMotorInverted(kFrontRightSteerInvert); + private static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, Units.inchesToMeters(kBackLeftXPosInches), Units.inchesToMeters(kBackLeftYPosInches), kInvertLeftSide) + .withSteerMotorInverted(kBackLeftSteerInvert); + private static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, Units.inchesToMeters(kBackRightXPosInches), Units.inchesToMeters(kBackRightYPosInches), kInvertRightSide) + .withSteerMotorInverted(kBackRightSteerInvert); + + public static final CommandSwerveDrivetrain DriveTrain = new CommandSwerveDrivetrain(DrivetrainConstants, FrontLeft, + FrontRight, BackLeft, BackRight); +}