Add swerve

.gitignore

@@ -2,3 +2,7 @@
 /build/
 /.idea/
 bin/
+
+*.project
+/.settings/
+*.classpath

examples/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=..
+eclipse.preferences.version=1

examples/robot/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../..
+eclipse.preferences.version=1

examples/robot/basic-robot/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

examples/robot/basic-trigger/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

examples/robot/pnuematics-system/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

examples/robot/robot-manual-mode-control/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

examples/robot/tankdrive-modes/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

examples/robot/tankdrive-system-custom/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

examples/robot/tankdrive-system-xbox/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

examples/robot/tankdrive-system/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

examples/robot/tankdrive/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

examples/vision/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../..
+eclipse.preferences.version=1

examples/vision/basic-vision/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

examples/vision/cv-2020-vision/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

examples/vision/cv-colorfilter/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

examples/vision/vision-control/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=../../..
+eclipse.preferences.version=1

flashlib.core.robot/src/main/java/com/flash3388/flashlib/robot/systems/drive/DrivetrainProperties.java

@@ -0,0 +1,26 @@
+package com.flash3388.flashlib.robot.systems.drive;
+
+public class DrivetrainProperties {
+    private final double mLength;
+    private final double mWidth;
+    private final double mDiagonal;
+
+    public DrivetrainProperties(double length, double width) {
+        mLength = length;
+        mWidth = width;
+
+        mDiagonal = Math.sqrt(width * width + length * length);
+    }
+
+    public double getDiagonal() {
+        return mDiagonal;
+    }
+
+    public double getWidth() {
+        return mWidth;
+    }
+
+    public double getLength() {
+        return mLength;
+    }
+}

flashlib.core.robot/src/main/java/com/flash3388/flashlib/robot/systems/drive/PidSwerveWheel.java

@@ -0,0 +1,67 @@
+package com.flash3388.flashlib.robot.systems.drive;
+
+import java.util.function.DoubleSupplier;
+
+import com.flash3388.flashlib.io.devices.SpeedController;
+import com.flash3388.flashlib.math.Mathf;
+import com.flash3388.flashlib.robot.control.PidController;
+import com.jmath.vectors.Vector2;
+
+public class PidSwerveWheel implements SwerveWheel {
+
+    private final PidController mPidController;
+    private final SpeedController mForwardController;
+    private final SpeedController mRotationController;
+    private final DoubleSupplier mAngleDegreeSupplier;
+
+    public PidSwerveWheel(PidController pidController, SpeedController forwardController,
+            SpeedController rotationController, DoubleSupplier angleDegreeSupplier) {
+        mPidController = pidController;
+        mForwardController = forwardController;
+        mRotationController = rotationController;
+        mAngleDegreeSupplier = angleDegreeSupplier;
+
+        resetPid();
+    }
+
+    public PidSwerveWheel(SpeedController forwardController, SpeedController rotationController,
+            DoubleSupplier angleDegreeSupplier, double kp, double ki, double kd, double kf) {
+        this(new PidController(kp, kd, kd, kf), forwardController, rotationController, angleDegreeSupplier);
+    }
+
+    public void resetPid() {
+        mPidController.reset();
+    }
+
+    @Override
+    public void move(Vector2 motionVector) {
+        double forwardSpeed = motionVector.magnitude();
+        double targetAngle = motionVector.angle();
+        double currentAngle = Mathf.translateAngle(mAngleDegreeSupplier.getAsDouble());
+
+        double nextAngle = calcNextAngle(targetAngle, currentAngle);
+        double pidOut = mPidController.applyAsDouble(currentAngle, nextAngle);
+
+        rotate(pidOut);
+        move(forwardSpeed);
+    }
+
+    @Override
+    public void rotate(double speed) {
+        mRotationController.set(speed);
+    }
+
+    @Override
+    public void stop() {
+        mForwardController.stop();
+        mRotationController.stop();
+    }
+
+    private double calcNextAngle(double targetAngle, double currentAngle) {
+        targetAngle = Mathf.translateAngle(targetAngle);
+        currentAngle = Mathf.translateAngle(currentAngle);
+        double distance = (targetAngle - currentAngle + 180 ) % 360 - 180;
+
+        return currentAngle + distance;
+    }
+}

flashlib.core.robot/src/main/java/com/flash3388/flashlib/robot/systems/drive/SwerveDriveSystem.java

@@ -0,0 +1,88 @@
+package com.flash3388.flashlib.robot.systems.drive;
+
+import com.flash3388.flashlib.math.Mathf;
+import com.flash3388.flashlib.scheduling.Subsystem;
+import com.jmath.vectors.Vector2;
+
+public class SwerveDriveSystem extends Subsystem implements HolonomicDrive {
+
+    private final DrivetrainProperties mDrivetrain;
+    private final SwerveWheel mFrontRightWheel;
+    private final SwerveWheel mFrontLeftWheel;
+    private final SwerveWheel mRearRightWheel;
+    private final SwerveWheel mRearLeftWheel;
+
+    public SwerveDriveSystem(DrivetrainProperties drivetrain, SwerveWheel frontRightWheel,
+            SwerveWheel frontLeftWheel, SwerveWheel rearRightWheel, SwerveWheel rearLeftWheel) {
+        mDrivetrain = drivetrain;
+        mFrontRightWheel = frontRightWheel;
+        mFrontLeftWheel = frontLeftWheel;
+        mRearRightWheel = rearRightWheel;
+        mRearLeftWheel = rearLeftWheel;
+    }
+
+    public SwerveDriveSystem(SwerveWheel frontRightWheel,SwerveWheel frontLeftWheel, 
+            SwerveWheel rearRightWheel, SwerveWheel rearLeftWheel, double drivetrainWidth, 
+            double drivetrainLength) {
+        this(new DrivetrainProperties(drivetrainWidth, drivetrainLength), 
+            frontRightWheel, frontLeftWheel, rearRightWheel, rearLeftWheel);
+    }
+
+    public void fieldCentricDrive(double x, double y, double rotation, double gyroRadians) {
+        rotation = -y * Math.sin(gyroRadians) + rotation * Math.cos(gyroRadians);
+        y = y * Math.cos(gyroRadians) + rotation * Math.sin(gyroRadians);
+
+        drive(x, y, rotation);
+    }
+
+    public void drive(double x, double y, double rotation) {
+        holonomicCartesian(y, x, rotation);
+    }
+
+    @Override
+    public void move(Vector2 motionVector) {
+        holonomicCartesian(motionVector.y(), motionVector.x(), 0);
+    }
+
+    @Override
+    public void rotate(double speed) {
+        holonomicCartesian(0, 0, speed);
+    }
+
+    @Override
+    public void holonomicPolar(double magnitude, double direction, double rotation) {
+        Vector2 motionVector = Vector2.polar(magnitude, direction);
+        holonomicCartesian(motionVector.y(), motionVector.x(), rotation);
+    }
+
+    @Override
+    public void holonomicCartesian(double y, double x, double rotation) {
+        double a = x - rotation * (mDrivetrain.getLength() / mDrivetrain.getDiagonal());
+        double b = x + rotation * (mDrivetrain.getLength() / mDrivetrain.getDiagonal());
+        double c = y - rotation * (mDrivetrain.getWidth() / mDrivetrain.getDiagonal());
+        double d = y + rotation * (mDrivetrain.getWidth() / mDrivetrain.getDiagonal());
+
+        double frontRightSpeed = Math.sqrt((b * b) + (d * d));
+        double frontLeftSpeed = Math.sqrt((b * b) + (c * c));
+        double rearRightSpeed = Math.sqrt((a * a) + (d * d));
+        double rearLeftSpeed = Math.sqrt((a * a) + (c * c));
+
+        double frontRightAngle = Mathf.translateAngle(Math.toDegrees(Math.atan2(b, d)));
+        double frontLeftAngle = Mathf.translateAngle(Math.toDegrees(Math.atan2(b, c)));
+        double rearRightAngle = Mathf.translateAngle(Math.toDegrees(Math.atan2(a, d))); 
+        double rearLeftAngle = Mathf.translateAngle(Math.toDegrees(Math.atan2(a, c)));
+
+        mFrontRightWheel.move(Vector2.polar(frontRightSpeed, frontRightAngle));
+        mFrontLeftWheel.move(Vector2.polar(frontLeftSpeed, frontLeftAngle));
+        mRearRightWheel.move(Vector2.polar(rearRightSpeed, rearRightAngle));
+        mRearLeftWheel.move(Vector2.polar(rearLeftSpeed, rearLeftAngle));
+    }
+
+    @Override
+    public void stop() {
+        mFrontLeftWheel.stop();
+        mFrontRightWheel.stop();
+        mRearRightWheel.stop();
+        mRearLeftWheel.stop();
+    }
+}

flashlib.core.robot/src/main/java/com/flash3388/flashlib/robot/systems/drive/SwerveWheel.java

@@ -0,0 +1,7 @@
+package com.flash3388.flashlib.robot.systems.drive;
+
+import com.flash3388.flashlib.robot.motion.Movable2d;
+import com.flash3388.flashlib.robot.motion.Rotatable;
+
+public interface SwerveWheel extends Movable2d, Rotatable {
+}

flashlib.vision.cv/.settings/org.eclipse.buildship.core.prefs

@@ -0,0 +1,2 @@
+connection.project.dir=..
+eclipse.preferences.version=1

flashlib.vision.cv/.settings/org.eclipse.jdt.core.prefs

@@ -0,0 +1,4 @@
+eclipse.preferences.version=1
+org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.8
+org.eclipse.jdt.core.compiler.compliance=1.8
+org.eclipse.jdt.core.compiler.source=1.8