diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json new file mode 100644 index 0000000..3693eda --- /dev/null +++ b/src/main/deploy/swerve/controllerproperties.json @@ -0,0 +1,8 @@ +{ + "angleJoystickRadiusDeadband": 0.5, + "heading": { + "p": 0, + "i": 0, + "d": 0 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json new file mode 100644 index 0000000..fce52d2 --- /dev/null +++ b/src/main/deploy/swerve/modules/backleft.json @@ -0,0 +1,27 @@ +{ + "location": { + "front": -12.75, + "left": 12.75 + }, + "absoluteEncoderOffset": 0, + "drive": { + "type": "talonfx", + "id": 2, + "canbus": null + }, + "angle": { + "type": "talonsrx", + "id": 6, + "canbus": null + }, + "encoder": { + "type": "talonsrx_pwm", + "id": 0, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": true + }, + "absoluteEncoderInverted": false +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json new file mode 100644 index 0000000..364179d --- /dev/null +++ b/src/main/deploy/swerve/modules/backright.json @@ -0,0 +1,27 @@ +{ + "location": { + "front": -12.75, + "left": -12.75 + }, + "absoluteEncoderOffset": 0, + "drive": { + "type": "talonfx", + "id": 3, + "canbus": null + }, + "angle": { + "type": "talonsrx", + "id": 7, + "canbus": null + }, + "encoder": { + "type": "talonsrx_pwm", + "id": 0, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": true + }, + "absoluteEncoderInverted": false +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json new file mode 100644 index 0000000..ceb3a4e --- /dev/null +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -0,0 +1,27 @@ +{ + "location": { + "front": 12.75, + "left": 12.75 + }, + "absoluteEncoderOffset": 0, + "drive": { + "type": "talonfx", + "id": 0, + "canbus": null + }, + "angle": { + "type": "talonsrx", + "id": 4, + "canbus": null + }, + "encoder": { + "type": "talonsrx_pwm", + "id": 0, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + }, + "absoluteEncoderInverted": false +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json new file mode 100644 index 0000000..2f79c1f --- /dev/null +++ b/src/main/deploy/swerve/modules/frontright.json @@ -0,0 +1,27 @@ +{ + "location": { + "front": 12.75, + "left": -12.75 + }, + "absoluteEncoderOffset": 0, + "drive": { + "type": "talonfx", + "id": 1, + "canbus": null + }, + "angle": { + "type": "talonsrx", + "id": 5, + "canbus": null + }, + "encoder": { + "type": "talonsrx_pwm", + "id": 0, + "canbus": null + }, + "inverted": { + "drive": false, + "angle": false + }, + "absoluteEncoderInverted": false +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json new file mode 100644 index 0000000..2460c38 --- /dev/null +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -0,0 +1,16 @@ +{ + "optimalVoltage": 12, + "wheelGripCoefficientOfFriction": 1.19, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json new file mode 100644 index 0000000..0ef1681 --- /dev/null +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -0,0 +1,16 @@ +{ + "drive": { + "p": 0, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + }, + "angle": { + "p": 0, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json new file mode 100644 index 0000000..fab8811 --- /dev/null +++ b/src/main/deploy/swerve/swervedrive.json @@ -0,0 +1,14 @@ +{ + "imu": { + "type": "pigeon2", + "id": 0, + "canbus": null + }, + "invertedIMU": false, + "modules": [ + "frontleft.json", + "frontright.json", + "backleft.json", + "backright.json" + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7d86c41..027dfd6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,7 +1,35 @@ package frc.robot; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import java.io.File; + +import com.pathplanner.lib.util.PIDConstants; + +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.wpilibj.Filesystem; + public class Constants { public static class SwerveK { - + public static final int driveMotorID = 0; + public static final int turnMotorID = 1; + public static final int encoderID = 2; + + public static final Measure wheelDiameter = Inches.of(3); + public static final Measure driveBaseRadius = Meters.of(0.4579874); + + public static final double steerGearRatio = 12.8; //! Figure Out; + public static final double driveGearRatio = 6; //! Figure Out + + public static final Measure> maxModuleSpeed = MetersPerSecond.of(0); //! Tune + + public static final PIDConstants translationConstants = new PIDConstants(1, 1, 1); //! Tune + public static final PIDConstants rotationConstants = new PIDConstants(1, 1, 1); //! Tune + + public static final File swerveDirectory = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/swerve"); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 11b8784..08ba675 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,11 +5,15 @@ package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; +import frc.robot.subsystems.Swerve; public class Robot extends TimedRobot { - @Override - public void robotInit() { + private final Swerve swerve = new Swerve(); + + public Robot() { + } + } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index dce5ba5..0e21ca4 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,5 +1,122 @@ package frc.robot.subsystems; -public class Swerve { +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import java.io.IOException; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.SwerveK; +import swervelib.SwerveDrive; +import swervelib.math.SwerveMath; +import swervelib.parser.SwerveParser; +import swervelib.telemetry.SwerveDriveTelemetry; +import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; + +/** + * ? Need to tune current limits, pidfproperties, controllerproperties, maybe find wheel grip coefficient of friction + */ +public class Swerve extends SubsystemBase { + + private final SwerveDrive swerveDrive; + // double maximumSpeed = Units.feetToMeters(4.5); + + /** + * + * @param directory Directory of swerve drive config files. + */ + public Swerve() { + double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(SwerveK.steerGearRatio); + double driveConversionFactor = SwerveMath.calculateMetersPerRotation(SwerveK.driveGearRatio, SwerveK.wheelDiameter.in(Inches)); + + SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; + SwerveParser parser = null; + try { + parser = new SwerveParser(SwerveK.swerveDirectory); + } catch (IOException e) { + e.printStackTrace(); + throw new RuntimeException("Swerve directory not found."); + } + swerveDrive = parser.createSwerveDrive(SwerveK.maxModuleSpeed.in(MetersPerSecond), angleConversionFactor, driveConversionFactor); + } + + public void setupPathPlanner() { + AutoBuilder.configureHolonomic( + this::getPose, + this::resetOdometry, + this::getRobotVelocity, + this::setChassisSpeeds, + new HolonomicPathFollowerConfig(SwerveK.translationConstants, + SwerveK.rotationConstants, + SwerveK.maxModuleSpeed.in(MetersPerSecond), + SwerveK.driveBaseRadius.in(Meters), + null), + () -> { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } return false; + } + ,this); + } + + /** + * + * @param TranslationX Translation in the X direction + * @param TranslationY Translation in the Y direction + * @param angularVelocity Angular Velocity to set + * @return + */ + public Command driveCommand(double TranslationX, double TranslationY, double angularVelocity) { + return runOnce(() -> drive( + new Translation2d(TranslationX * swerveDrive.getMaximumVelocity(), TranslationY * swerveDrive.getMaximumVelocity()), + angularVelocity * swerveDrive.getMaximumAngularVelocity(), + true, false)); + } + + public Command turnCommand(Measure targetAngle, Measure currentAngle, boolean fieldRelative) { + return runOnce(() -> turn(targetAngle, currentAngle, fieldRelative)); + } + + public void turn(Measure targetAngle, Measure currentAngle, boolean fieldRelative) { + drive(getPose().getTranslation(), getTurningAngle(targetAngle, currentAngle).in(Degrees), fieldRelative, false); + } + + public Measure getTurningAngle(Measure desiredAngle, Measure currentHeading) { + double angle = (desiredAngle.minus(currentHeading).plus(Degrees.of(540))).in(Degrees); + angle = (angle % 360) - 180; + return Degrees.of(angle); + } + + public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { + swerveDrive.drive(translation, rotation, fieldRelative, isOpenLoop); + } + + public void resetOdometry(Pose2d initialHolonomicPose) { + swerveDrive.resetOdometry(initialHolonomicPose); + } + + public Pose2d getPose() { + return swerveDrive.getPose(); + } + + public ChassisSpeeds getRobotVelocity() { + return swerveDrive.getRobotVelocity(); + } + + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { + swerveDrive.setChassisSpeeds(chassisSpeeds); + } }