diff --git a/.gitignore b/.gitignore index ea4719288..ba03f9607 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,7 @@ /build/ /.idea/ bin/ + +*.project +/.settings/ +*.classpath \ No newline at end of file diff --git a/examples/.settings/org.eclipse.buildship.core.prefs b/examples/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..b1886adb4 --- /dev/null +++ b/examples/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=.. +eclipse.preferences.version=1 diff --git a/examples/robot/.settings/org.eclipse.buildship.core.prefs b/examples/robot/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..62e3e7e80 --- /dev/null +++ b/examples/robot/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../.. +eclipse.preferences.version=1 diff --git a/examples/robot/basic-robot/.settings/org.eclipse.buildship.core.prefs b/examples/robot/basic-robot/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/robot/basic-robot/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/examples/robot/basic-trigger/.settings/org.eclipse.buildship.core.prefs b/examples/robot/basic-trigger/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/robot/basic-trigger/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/examples/robot/pnuematics-system/.settings/org.eclipse.buildship.core.prefs b/examples/robot/pnuematics-system/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/robot/pnuematics-system/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/examples/robot/robot-manual-mode-control/.settings/org.eclipse.buildship.core.prefs b/examples/robot/robot-manual-mode-control/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/robot/robot-manual-mode-control/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/examples/robot/tankdrive-modes/.settings/org.eclipse.buildship.core.prefs b/examples/robot/tankdrive-modes/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/robot/tankdrive-modes/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/examples/robot/tankdrive-system-custom/.settings/org.eclipse.buildship.core.prefs b/examples/robot/tankdrive-system-custom/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/robot/tankdrive-system-custom/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/examples/robot/tankdrive-system-xbox/.settings/org.eclipse.buildship.core.prefs b/examples/robot/tankdrive-system-xbox/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/robot/tankdrive-system-xbox/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/examples/robot/tankdrive-system/.settings/org.eclipse.buildship.core.prefs b/examples/robot/tankdrive-system/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/robot/tankdrive-system/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/examples/robot/tankdrive/.settings/org.eclipse.buildship.core.prefs b/examples/robot/tankdrive/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/robot/tankdrive/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/examples/vision/.settings/org.eclipse.buildship.core.prefs b/examples/vision/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..62e3e7e80 --- /dev/null +++ b/examples/vision/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../.. +eclipse.preferences.version=1 diff --git a/examples/vision/basic-vision/.settings/org.eclipse.buildship.core.prefs b/examples/vision/basic-vision/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/vision/basic-vision/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/examples/vision/cv-2020-vision/.settings/org.eclipse.buildship.core.prefs b/examples/vision/cv-2020-vision/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/vision/cv-2020-vision/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/examples/vision/cv-colorfilter/.settings/org.eclipse.buildship.core.prefs b/examples/vision/cv-colorfilter/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/vision/cv-colorfilter/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/examples/vision/vision-control/.settings/org.eclipse.buildship.core.prefs b/examples/vision/vision-control/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..98da5219f --- /dev/null +++ b/examples/vision/vision-control/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=../../.. +eclipse.preferences.version=1 diff --git a/flashlib.core.robot/src/main/java/com/flash3388/flashlib/robot/systems/drive/DrivetrainProperties.java b/flashlib.core.robot/src/main/java/com/flash3388/flashlib/robot/systems/drive/DrivetrainProperties.java new file mode 100644 index 000000000..ba4320b65 --- /dev/null +++ b/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; + } +} diff --git a/flashlib.core.robot/src/main/java/com/flash3388/flashlib/robot/systems/drive/PidSwerveWheel.java b/flashlib.core.robot/src/main/java/com/flash3388/flashlib/robot/systems/drive/PidSwerveWheel.java new file mode 100644 index 000000000..2ec80f5b4 --- /dev/null +++ b/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; + } +} diff --git a/flashlib.core.robot/src/main/java/com/flash3388/flashlib/robot/systems/drive/SwerveDriveSystem.java b/flashlib.core.robot/src/main/java/com/flash3388/flashlib/robot/systems/drive/SwerveDriveSystem.java new file mode 100644 index 000000000..ce732c36c --- /dev/null +++ b/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(); + } +} diff --git a/flashlib.core.robot/src/main/java/com/flash3388/flashlib/robot/systems/drive/SwerveWheel.java b/flashlib.core.robot/src/main/java/com/flash3388/flashlib/robot/systems/drive/SwerveWheel.java new file mode 100644 index 000000000..bc18d5e11 --- /dev/null +++ b/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 { +} diff --git a/flashlib.vision.cv/.settings/org.eclipse.buildship.core.prefs b/flashlib.vision.cv/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000..b1886adb4 --- /dev/null +++ b/flashlib.vision.cv/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,2 @@ +connection.project.dir=.. +eclipse.preferences.version=1 diff --git a/flashlib.vision.cv/.settings/org.eclipse.jdt.core.prefs b/flashlib.vision.cv/.settings/org.eclipse.jdt.core.prefs new file mode 100644 index 000000000..35068d95f --- /dev/null +++ b/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