Camellia swerve

src/main/java/com/spikes2212/command/drivetrains/swervedrivetrains/commands/SwerveDrive.java

@@ -0,0 +1,113 @@
+package com.spikes2212.command.drivetrains.swervedrivetrains.commands;
+
+import com.spikes2212.command.drivetrains.swervedrivetrains.drivetrains.SwerveDrivetrain;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import java.util.function.Supplier;
+
+public class SwerveDrive extends Command {
+
+    private final SwerveDrivetrain swerveDrivetrain;
+    private final Supplier<Double> xSpeed;
+    private final Supplier<Double> ySpeed;
+    private final Supplier<Double> rotationSpeed;
+
+    private final boolean fieldRelative;
+    private final boolean usePID;
+    private final boolean limitSpeed;
+    private final boolean limitAcceleration;
+
+    private final SlewRateLimiter xRateLimiter;
+    private final SlewRateLimiter yRateLimiter;
+    private final SlewRateLimiter rotationRateLimiter;
+
+    public SwerveDrive(SwerveDrivetrain swerveDrivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
+                       Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID, boolean limitSpeed,
+                       double xRateLimit, double yRateLimit, double rotationRateLimit) {
+        this.swerveDrivetrain = swerveDrivetrain;
+        this.xSpeed = xSpeed;
+        this.ySpeed = ySpeed;
+        this.rotationSpeed = rotationSpeed;
+        this.fieldRelative = fieldRelative;
+        this.usePID = usePID;
+        this.limitSpeed = limitSpeed;
+        xRateLimiter = new SlewRateLimiter(xRateLimit);
+        yRateLimiter = new SlewRateLimiter(yRateLimit);
+        rotationRateLimiter = new SlewRateLimiter(rotationRateLimit);
+        limitAcceleration = true;
+    }
+
+    public SwerveDrive(SwerveDrivetrain swerveDrivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
+                       Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID, boolean limitSpeed) {
+        this.swerveDrivetrain = swerveDrivetrain;
+        this.xSpeed = xSpeed;
+        this.ySpeed = ySpeed;
+        this.rotationSpeed = rotationSpeed;
+        this.fieldRelative = fieldRelative;
+        this.usePID = usePID;
+        this.limitSpeed = limitSpeed;
+        xRateLimiter = new SlewRateLimiter(0);
+        yRateLimiter = new SlewRateLimiter(0);
+        rotationRateLimiter = new SlewRateLimiter(0);
+        limitAcceleration = false;
+    }
+
+    public SwerveDrive(SwerveDrivetrain swerveDrivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
+                       Supplier<Double> rotationSpeed, boolean fieldRelative, boolean limitSpeed,
+                       double xRateLimit, double yRateLimit, double rotationRateLimit) {
+        this(swerveDrivetrain, xSpeed, ySpeed, rotationSpeed, fieldRelative, false, limitSpeed, xRateLimit, yRateLimit,
+                rotationRateLimit);
+    }
+
+    public SwerveDrive(SwerveDrivetrain swerveDrivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
+                       Supplier<Double> rotationSpeed, boolean fieldRelative, boolean limitSpeed) {
+        this(swerveDrivetrain, xSpeed, ySpeed, rotationSpeed, fieldRelative, false, limitSpeed);
+    }
+
+    public SwerveDrive(SwerveDrivetrain swerveDrivetrain, double xSpeed, double ySpeed,
+                       double rotationSpeed, boolean fieldRelative, boolean usePID, boolean limitSpeed,
+                       double xRateLimit, double yRateLimit, double rotationRateLimit) {
+        this(swerveDrivetrain, () -> xSpeed, () -> ySpeed, () -> rotationSpeed, fieldRelative, usePID, limitSpeed,
+                xRateLimit, yRateLimit, rotationRateLimit);
+    }
+
+    public SwerveDrive(SwerveDrivetrain swerveDrivetrain, double xSpeed, double ySpeed,
+                       double rotationSpeed, boolean fieldRelative, boolean usePID, boolean limitSpeed) {
+        this(swerveDrivetrain, () -> xSpeed, () -> ySpeed, () -> rotationSpeed, fieldRelative, usePID, limitSpeed);
+    }
+
+    public SwerveDrive(SwerveDrivetrain swerveDrivetrain, double xSpeed, double ySpeed,
+                       double rotationSpeed, boolean fieldRelative, boolean limitSpeed,
+                       double xRateLimit, double yRateLimit, double rotationRateLimit) {
+        this(swerveDrivetrain, () -> xSpeed, () -> ySpeed, () -> rotationSpeed, fieldRelative, false, limitSpeed,
+                xRateLimit, yRateLimit, rotationRateLimit);
+    }
+
+    public SwerveDrive(SwerveDrivetrain swerveDrivetrain, double xSpeed, double ySpeed,
+                       double rotationSpeed, boolean fieldRelative, boolean limitSpeed) {
+        this(swerveDrivetrain, () -> xSpeed, () -> ySpeed, () -> rotationSpeed, fieldRelative, false, limitSpeed);
+    }
+
+    @Override
+    public void execute() {
+        double xSpeed;
+        double ySpeed;
+        double rotationSpeed;
+        if (limitAcceleration) {
+            xSpeed = xRateLimiter.calculate(this.xSpeed.get());
+            ySpeed = yRateLimiter.calculate(this.ySpeed.get());
+            rotationSpeed = rotationRateLimiter.calculate(this.rotationSpeed.get());
+        } else {
+            xSpeed = this.xSpeed.get();
+            ySpeed = this.xSpeed.get();
+            rotationSpeed = this.xSpeed.get();
+        }
+        swerveDrivetrain.drive(xSpeed, ySpeed, rotationSpeed, fieldRelative, usePID, limitSpeed);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        swerveDrivetrain.stop();
+    }
+}

src/main/java/com/spikes2212/command/drivetrains/swervedrivetrains/drivetrains/SwerveDrivetrain.java

@@ -0,0 +1,77 @@
+package com.spikes2212.command.drivetrains.swervedrivetrains.drivetrains;
+
+import com.spikes2212.command.DashboardedSubsystem;
+import com.spikes2212.command.drivetrains.swervedrivetrains.modules.SwerveModule;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+import java.util.function.Supplier;
+
+public class SwerveDrivetrain extends DashboardedSubsystem {
+
+    private final SwerveModule frontLeft;
+    private final SwerveModule frontRight;
+    private final SwerveModule backLeft;
+    private final SwerveModule backRight;
+    private final Supplier<Double> robotAngle;
+    private final Translation2d centerOfRobot;
+    private final double maxSpeed;
+
+    private final SwerveDriveKinematics kinematics;
+
+    public SwerveDrivetrain(String namespaceName, SwerveModule frontLeft, SwerveModule frontRight,
+                            SwerveModule backLeft, SwerveModule backRight, Supplier<Double> angle,
+                            Translation2d centerOfRobot, double maxSpeed) {
+        super(namespaceName);
+        this.frontLeft = frontLeft;
+        this.frontRight = frontRight;
+        this.backLeft = backLeft;
+        this.backRight = backRight;
+        this.robotAngle = angle;
+        this.centerOfRobot = centerOfRobot;
+        this.maxSpeed = maxSpeed;
+        kinematics = new SwerveDriveKinematics(frontLeft.getModulePosition(), frontRight.getModulePosition(),
+                backLeft.getModulePosition(), backRight.getModulePosition());
+    }
+
+    public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative, boolean usePID,
+                      boolean limitSpeed) {
+        ChassisSpeeds speeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotationSpeed,
+                Rotation2d.fromDegrees(robotAngle.get())) : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed);
+        SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds, centerOfRobot);
+        SwerveDriveKinematics.desaturateWheelSpeeds(states, maxSpeed);
+        frontLeft.set(states[0], usePID, limitSpeed);
+        frontRight.set(states[1], usePID, limitSpeed);
+        backLeft.set(states[2], usePID, limitSpeed);
+        backRight.set(states[3], usePID, limitSpeed);
+    }
+
+    public void stop() {
+        frontLeft.stop();
+        frontRight.stop();
+        backLeft.stop();
+        backRight.stop();
+    }
+
+    public void resetRelativeEncoders() {
+        frontLeft.resetRelativeEncoder();
+        frontRight.resetRelativeEncoder();
+        backLeft.resetRelativeEncoder();
+        backRight.resetRelativeEncoder();
+    }
+
+    public double getRobotAngle() {
+        return robotAngle.get();
+    }
+
+    public Rotation2d getRotation2d() {
+        return Rotation2d.fromDegrees(getRobotAngle());
+    }
+
+    @Override
+    public void configureDashboard() {
+    }
+}

src/main/java/com/spikes2212/command/drivetrains/swervedrivetrains/modules/SwerveModule.java

@@ -0,0 +1,100 @@
+package com.spikes2212.command.drivetrains.swervedrivetrains.modules;
+
+import com.spikes2212.command.DashboardedSubsystem;
+import com.spikes2212.control.PIDSettings;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+
+public abstract class SwerveModule extends DashboardedSubsystem {
+
+    protected static final double DEGREES_IN_ROTATIONS = 360;
+    protected static final double DEGREES_TO_REVERSE = 180;
+    protected static final double MAX_DEGREES_NEEDED_TO_ROTATE = 90;
+
+    protected final MotorController driveController;
+    protected final MotorController turnController;
+
+    protected final double offset;
+    protected final double minSpeed;
+
+    protected final PIDSettings turnPIDSettings;
+
+    protected final Translation2d modulePosition;
+
+    public SwerveModule(String namespaceName, MotorController driveController, MotorController turnController,
+                        double offset, boolean driveInverted, boolean turnInverted, double minSpeed,
+                        PIDSettings turnPIDSettings, Translation2d modulePosition) {
+        super(namespaceName);
+        this.driveController = driveController;
+        this.turnController = turnController;
+        this.offset = offset;
+        this.minSpeed = minSpeed;
+        this.turnPIDSettings = turnPIDSettings;
+        this.modulePosition = modulePosition;
+    }
+
+    public SwerveModule(String namespaceName, MotorController driveController, MotorController turnController,
+                        double offset, boolean driveInverted, boolean turnInverted, PIDSettings turnPIDSettings,
+                        Translation2d modulePosition) {
+        this(namespaceName, driveController, turnController, offset, driveInverted, turnInverted, 0, turnPIDSettings,
+                modulePosition);
+    }
+
+    public void set(SwerveModuleState state, boolean usePID, boolean limitSpeed) {
+        if (limitSpeed && state.speedMetersPerSecond < minSpeed) {
+            stop();
+            return;
+        }
+        state = optimize(state);
+        setSpeed(state.speedMetersPerSecond, usePID);
+        setAngle(state.angle.getDegrees());
+    }
+
+    protected abstract void setSpeed(double speed, boolean usePID);
+
+    protected abstract void setAngle(double angle);
+
+    public void stop() {
+        driveController.stopMotor();
+        turnController.stopMotor();
+    }
+
+    public abstract void resetRelativeEncoder();
+
+    public Rotation2d getRotation2d() {
+        return Rotation2d.fromDegrees(getAngle());
+    }
+
+    public abstract double getAngle();
+
+    public abstract double getAbsoluteAngle();
+
+    public abstract double getVelocity();
+
+    public Translation2d getModulePosition() {
+        return modulePosition;
+    }
+
+    private SwerveModuleState optimize(SwerveModuleState state) {
+        double wantedAngle = placeInAppropriate0To360Scope(state.angle.getDegrees());
+        double wantedSpeed = state.speedMetersPerSecond;
+        double error = wantedAngle - getAngle();
+        while (Math.abs(error) > MAX_DEGREES_NEEDED_TO_ROTATE) {
+            wantedAngle = error > 0 ? wantedAngle - DEGREES_TO_REVERSE : wantedAngle + DEGREES_TO_REVERSE;
+            wantedSpeed *= -1;
+        }
+        return new SwerveModuleState(wantedSpeed, Rotation2d.fromDegrees(wantedAngle));
+    }
+
+    private double placeInAppropriate0To360Scope(double wantedAngle) {
+        int rotations = (int) (getAngle() / DEGREES_IN_ROTATIONS);
+        if (getAngle() < 0) rotations--;
+        return wantedAngle + DEGREES_IN_ROTATIONS * rotations;
+    }
+
+    @Override
+    public void configureDashboard() {
+    }
+}

src/main/java/com/spikes2212/command/drivetrains/OdometryDrivetrain.java → src/main/java/com/spikes2212/command/drivetrains/tankdrivetrains/OdometryDrivetrain.java

@@ -1,4 +1,4 @@
-package com.spikes2212.command.drivetrains;
+package com.spikes2212.command.drivetrains.tankdrivetrains;
 
 import com.spikes2212.path.OdometryHandler;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;

src/main/java/com/spikes2212/command/drivetrains/TankDrivetrain.java → src/main/java/com/spikes2212/command/drivetrains/tankdrivetrains/TankDrivetrain.java

@@ -1,4 +1,4 @@
-package com.spikes2212.command.drivetrains;
+package com.spikes2212.command.drivetrains.tankdrivetrains;
 
 import com.spikes2212.command.DashboardedSubsystem;
 import edu.wpi.first.networktables.NetworkTable;

src/main/java/com/spikes2212/command/drivetrains/commands/DriveArcade.java → src/main/java/com/spikes2212/command/drivetrains/tankdrivetrains/commands/DriveArcade.java

@@ -1,6 +1,6 @@
-package com.spikes2212.command.drivetrains.commands;
+package com.spikes2212.command.drivetrains.tankdrivetrains.commands;
 
-import com.spikes2212.command.drivetrains.TankDrivetrain;
+import com.spikes2212.command.drivetrains.tankdrivetrains.TankDrivetrain;
 import edu.wpi.first.wpilibj2.command.Command;
 
 import java.util.function.Supplier;

src/main/java/com/spikes2212/command/drivetrains/commands/DriveArcadeWithPID.java → src/main/java/com/spikes2212/command/drivetrains/tankdrivetrains/commands/DriveArcadeWithPID.java

@@ -1,6 +1,6 @@
-package com.spikes2212.command.drivetrains.commands;
+package com.spikes2212.command.drivetrains.tankdrivetrains.commands;
 
-import com.spikes2212.command.drivetrains.TankDrivetrain;
+import com.spikes2212.command.drivetrains.tankdrivetrains.TankDrivetrain;
 import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;

src/main/java/com/spikes2212/command/drivetrains/commands/DriveArcadeWithVoltages.java → src/main/java/com/spikes2212/command/drivetrains/tankdrivetrains/commands/DriveArcadeWithVoltages.java

@@ -1,12 +1,10 @@
-package com.spikes2212.command.drivetrains.commands;
+package com.spikes2212.command.drivetrains.tankdrivetrains.commands;
 
-import com.spikes2212.command.drivetrains.TankDrivetrain;
+import com.spikes2212.command.drivetrains.tankdrivetrains.TankDrivetrain;
 import edu.wpi.first.wpilibj2.command.Command;
 
 import java.util.function.Supplier;
 
-import java.util.function.Supplier;
-
 /**
  * A command that moves a {@link TankDrivetrain} using linear and rotational voltages.
  *

src/main/java/com/spikes2212/command/drivetrains/commands/DriveCurvature.java → src/main/java/com/spikes2212/command/drivetrains/tankdrivetrains/commands/DriveCurvature.java

@@ -1,6 +1,6 @@
-package com.spikes2212.command.drivetrains.commands;
+package com.spikes2212.command.drivetrains.tankdrivetrains.commands;
 
-import com.spikes2212.command.drivetrains.TankDrivetrain;
+import com.spikes2212.command.drivetrains.tankdrivetrains.TankDrivetrain;
 import edu.wpi.first.wpilibj2.command.Command;
 
 import java.util.function.Supplier;

src/main/java/com/spikes2212/command/drivetrains/commands/DriveTank.java → src/main/java/com/spikes2212/command/drivetrains/tankdrivetrains/commands/DriveTank.java

@@ -1,6 +1,6 @@
-package com.spikes2212.command.drivetrains.commands;
+package com.spikes2212.command.drivetrains.tankdrivetrains.commands;
 
-import com.spikes2212.command.drivetrains.TankDrivetrain;
+import com.spikes2212.command.drivetrains.tankdrivetrains.TankDrivetrain;
 import edu.wpi.first.wpilibj2.command.Command;
 
 import java.util.function.Supplier;

src/main/java/com/spikes2212/command/drivetrains/commands/DriveTankWithPID.java → src/main/java/com/spikes2212/command/drivetrains/tankdrivetrains/commands/DriveTankWithPID.java

@@ -1,6 +1,6 @@
-package com.spikes2212.command.drivetrains.commands;
+package com.spikes2212.command.drivetrains.tankdrivetrains.commands;
 
-import com.spikes2212.command.drivetrains.TankDrivetrain;
+import com.spikes2212.command.drivetrains.tankdrivetrains.TankDrivetrain;
 import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;

src/main/java/com/spikes2212/command/drivetrains/commands/DriveTankWithVoltages.java → src/main/java/com/spikes2212/command/drivetrains/tankdrivetrains/commands/DriveTankWithVoltages.java

@@ -1,6 +1,6 @@
-package com.spikes2212.command.drivetrains.commands;
+package com.spikes2212.command.drivetrains.tankdrivetrains.commands;
 
-import com.spikes2212.command.drivetrains.TankDrivetrain;
+import com.spikes2212.command.drivetrains.tankdrivetrains.TankDrivetrain;
 import edu.wpi.first.wpilibj2.command.Command;
 
 import java.util.function.Supplier;

src/main/java/com/spikes2212/command/drivetrains/commands/FollowPath.java → src/main/java/com/spikes2212/command/drivetrains/tankdrivetrains/commands/FollowPath.java

@@ -1,6 +1,6 @@
-package com.spikes2212.command.drivetrains.commands;
+package com.spikes2212.command.drivetrains.tankdrivetrains.commands;
 
-import com.spikes2212.command.drivetrains.OdometryDrivetrain;
+import com.spikes2212.command.drivetrains.tankdrivetrains.OdometryDrivetrain;
 import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;

src/main/java/com/spikes2212/command/drivetrains/commands/OrientWithPID.java → src/main/java/com/spikes2212/command/drivetrains/tankdrivetrains/commands/OrientWithPID.java

@@ -1,6 +1,6 @@
-package com.spikes2212.command.drivetrains.commands;
+package com.spikes2212.command.drivetrains.tankdrivetrains.commands;
 
-import com.spikes2212.command.drivetrains.TankDrivetrain;
+import com.spikes2212.command.drivetrains.tankdrivetrains.TankDrivetrain;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;