From e63278a8287daa0832cc8dd14f492da12c1e592e Mon Sep 17 00:00:00 2001 From: turtle4831 Date: Thu, 29 Aug 2024 21:38:15 -0500 Subject: [PATCH 1/3] redid DifferentialSwerveModule.java --- .../quail/DifferentialSwerveDrive.java | 31 ++++++++ .../quail/DifferentialSwerveModuleBase.java | 79 +++++++++++++++++-- 2 files changed, 103 insertions(+), 7 deletions(-) create mode 100644 quail/src/main/java/com/mineinjava/quail/DifferentialSwerveDrive.java diff --git a/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveDrive.java b/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveDrive.java new file mode 100644 index 0000000..03e8579 --- /dev/null +++ b/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveDrive.java @@ -0,0 +1,31 @@ +package com.mineinjava.quail; + +import java.util.List; + +import com.mineinjava.quail.util.geometry.Vec2d; + +public class DifferentialSwerveDrive extends SwerveDrive { + List swerveModules; + double maxSpeed; + public DifferentialSwerveDrive(List swerveModules) { + super(swerveModules); + this.swerveModules = swerveModules; + } + public DifferentialSwerveDrive(List swerveModules, double maxSpeed){ + super(swerveModules,maxSpeed); + this.swerveModules = swerveModules; + this.maxSpeed = maxSpeed; + } + + @Override + public void move(RobotMovement movement, double gyroOffset) { + Vec2d[] moduleVectors = + calculateMoveAngles(movement.translation, movement.rotation, gyroOffset); + moduleVectors = normalizeModuleVectors(moduleVectors, this.maxSpeed); + for (int i = 0; i < this.swerveModules.size(); i++) { + swerveModules.get(i).set(moduleVectors[i]); + } + } + + +} diff --git a/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveModuleBase.java b/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveModuleBase.java index ebcca81..5608e12 100644 --- a/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveModuleBase.java +++ b/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveModuleBase.java @@ -20,18 +20,39 @@ package com.mineinjava.quail; +import com.mineinjava.quail.util.MiniPID; import com.mineinjava.quail.util.geometry.Vec2d; /** A base class for differential swerve modules. */ public class DifferentialSwerveModuleBase extends SwerveModuleBase { - - public DifferentialSwerveModuleBase(Vec2d position, double steeringRatio, double driveRatio) { + Vec2d position; + double steeringRatio; + double driveRatio; + double ticksPerRev; + boolean optimized; + MiniPID pid; + + private double degrees = ticksPerRev / 360; + + public DifferentialSwerveModuleBase(Vec2d position, double steeringRatio, double driveRatio, double ticksPerRev, MiniPID pid) { super(position, steeringRatio, driveRatio); + this.position = position; + this.steeringRatio = steeringRatio; + this.driveRatio = driveRatio; + this.ticksPerRev = ticksPerRev; + this.pid = pid; + } public DifferentialSwerveModuleBase( - Vec2d position, double steeringRatio, double driveRatio, boolean optimized) { + Vec2d position, double steeringRatio, double driveRatio,double ticksPerRev,MiniPID pid, boolean optimized) { super(position, steeringRatio, driveRatio, optimized); + this.position = position; + this.steeringRatio = steeringRatio; + this.driveRatio = driveRatio; + this.ticksPerRev = ticksPerRev; + this.pid = pid; + this.optimized = optimized; } /** @@ -53,10 +74,54 @@ public double[] calculateMotorSpeeds(double rotationSpeed, double wheelSpeed) { return motorSpeeds; } + /** + * Calculates the motor speeds for a differential swerve module. + * + * @param rotationSpeed the current rotation speed of the pod + * @param wheelSpeed the current speed of the wheel + * @return motor speeds (array of length 2) + */ + public double[] calculateeMotorSpeeds(double speed, double angle, double motor1pos, double motor2pos){ + double[] motorSpeeds = new double[2]; + double currentAngle = calculateModuleAngle(motor1pos,motor2pos); + double oppAngle = angleWrap(currentAngle + 180); + + double angleFromTarget = angleWrap(angle - currentAngle); + double oppAngleFromTarget = angleWrap(angle - oppAngle); + + if(Math.abs(angleFromTarget) > Math.abs(oppAngleFromTarget)){ + angle = oppAngleFromTarget; + }else{ + angle = angleFromTarget; + } + + if (Math.abs(oppAngleFromTarget) < 0.3){//margin of allowed error + speed = -speed; + }else if (Math.abs(angleFromTarget) > 0.3){ + speed = 0; + } + motorSpeeds[0] = (-speed * 1) + pid.getOutput(currentAngle, angle); + motorSpeeds[1] = (speed * 1) + pid.getOutput(currentAngle, angle); + return motorSpeeds; + } - /** Calculates the module angle based on the positions of the two motors. */ - public double calculateModuleAngle(double motor1pos, double motor2pos) { - double averageRotation = (motor1pos + motor2pos) / 2; - return averageRotation / steeringRatio; + public double calculateModuleAngle(double motor1pos, double motor2pos){ + double angleTicks = (motor1pos + motor2pos) / 2; + double angle = ticksToDegrees(angleTicks); + angle = angleWrap(currentAngle); + return angle; } + + + + private static double angleWrap(double angle) { + if (angle > 0) + return ((angle + Math.PI) % (Math.PI * 2)) - Math.PI; + else + return ((angle - Math.PI) % (Math.PI * 2)) + Math.PI; +} + private double ticksToDegrees(double ticks){ + return ticks * degrees; + } + } From 88d8538f40f78e2c5ac38cf44984dd04830dfdba Mon Sep 17 00:00:00 2001 From: turtle4831 Date: Fri, 30 Aug 2024 10:53:27 -0500 Subject: [PATCH 2/3] finished diffy swerve module --- .../quail/DifferentialSwerveDrive.java | 31 ----------- .../quail/DifferentialSwerveModuleBase.java | 52 +++++++++++++++++-- 2 files changed, 49 insertions(+), 34 deletions(-) delete mode 100644 quail/src/main/java/com/mineinjava/quail/DifferentialSwerveDrive.java diff --git a/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveDrive.java b/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveDrive.java deleted file mode 100644 index 03e8579..0000000 --- a/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveDrive.java +++ /dev/null @@ -1,31 +0,0 @@ -package com.mineinjava.quail; - -import java.util.List; - -import com.mineinjava.quail.util.geometry.Vec2d; - -public class DifferentialSwerveDrive extends SwerveDrive { - List swerveModules; - double maxSpeed; - public DifferentialSwerveDrive(List swerveModules) { - super(swerveModules); - this.swerveModules = swerveModules; - } - public DifferentialSwerveDrive(List swerveModules, double maxSpeed){ - super(swerveModules,maxSpeed); - this.swerveModules = swerveModules; - this.maxSpeed = maxSpeed; - } - - @Override - public void move(RobotMovement movement, double gyroOffset) { - Vec2d[] moduleVectors = - calculateMoveAngles(movement.translation, movement.rotation, gyroOffset); - moduleVectors = normalizeModuleVectors(moduleVectors, this.maxSpeed); - for (int i = 0; i < this.swerveModules.size(); i++) { - swerveModules.get(i).set(moduleVectors[i]); - } - } - - -} diff --git a/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveModuleBase.java b/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveModuleBase.java index 5608e12..ef0f242 100644 --- a/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveModuleBase.java +++ b/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveModuleBase.java @@ -20,6 +20,7 @@ package com.mineinjava.quail; + import com.mineinjava.quail.util.MiniPID; import com.mineinjava.quail.util.geometry.Vec2d; @@ -33,6 +34,8 @@ public class DifferentialSwerveModuleBase extends SwerveModuleBase { MiniPID pid; private double degrees = ticksPerRev / 360; + private double motor1Pos; + private double motor2Pos; public DifferentialSwerveModuleBase(Vec2d position, double steeringRatio, double driveRatio, double ticksPerRev, MiniPID pid) { super(position, steeringRatio, driveRatio); @@ -81,7 +84,7 @@ public double[] calculateMotorSpeeds(double rotationSpeed, double wheelSpeed) { * @param wheelSpeed the current speed of the wheel * @return motor speeds (array of length 2) */ - public double[] calculateeMotorSpeeds(double speed, double angle, double motor1pos, double motor2pos){ + public double[] calculateMotorSpeeds(double speed, double angle, double motor1pos, double motor2pos){ double[] motorSpeeds = new double[2]; double currentAngle = calculateModuleAngle(motor1pos,motor2pos); double oppAngle = angleWrap(currentAngle + 180); @@ -100,8 +103,10 @@ public double[] calculateeMotorSpeeds(double speed, double angle, double motor1 }else if (Math.abs(angleFromTarget) > 0.3){ speed = 0; } - motorSpeeds[0] = (-speed * 1) + pid.getOutput(currentAngle, angle); - motorSpeeds[1] = (speed * 1) + pid.getOutput(currentAngle, angle); + //motorSpeeds[0] = (-speed * 1) + pid.getOutput(currentAngle, angle); + //motorSpeeds[1] = (speed * 1) + pid.getOutput(currentAngle, angle); + motorSpeeds[0] = speed; + motorSpeeds[1] = angle; return motorSpeeds; } @@ -112,6 +117,47 @@ public double calculateModuleAngle(double motor1pos, double motor2pos){ return angle; } +/** + * Sets the raw speed of the module. + * + *

OVERRIDE ME!!! This is where you call your motor / pid controllers + * + * @param speed the speed to set the module to + */ + private void setMotor1Power(double power){ + return; + } + /** + * Sets the raw speed of the module. + * + *

OVERRIDE ME!!! This is where you call your motor / pid controllers + * + * @param speed the speed to set the module to + */ + private void setMotor2Power(double power){ + return; + } + + + + + public void setMotorPoses(double motor1Pos, double motor2Pos){ + this.motor1Pos = motor1Pos; + this.motor2Pos = motor2Pos; + } + /** + * Sets the module's motion to the specified vector; + * + * @param vec the vector to set the module to + */ + @Override + public void set(Vec2d vec){ + double[] powers = calculateMotorSpeeds(vec.getLength(),vec.getAngle(),motor1Pos,motor2Pos); + setMotor1Power(powers[0]); + setMotor2Power(powers[1]); + + } + private static double angleWrap(double angle) { From 798970892f64e60db9b8762bb2e36bd2daf7eedb Mon Sep 17 00:00:00 2001 From: turtle4831 Date: Fri, 30 Aug 2024 11:06:46 -0500 Subject: [PATCH 3/3] fixed funnies --- .../quail/DifferentialSwerveModuleBase.java | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveModuleBase.java b/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveModuleBase.java index ef0f242..4286a8f 100644 --- a/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveModuleBase.java +++ b/quail/src/main/java/com/mineinjava/quail/DifferentialSwerveModuleBase.java @@ -57,7 +57,7 @@ public DifferentialSwerveModuleBase( this.pid = pid; this.optimized = optimized; } - + @Deprecated /** * Calculates the motor speeds for a differential swerve module. * @@ -80,8 +80,10 @@ public double[] calculateMotorSpeeds(double rotationSpeed, double wheelSpeed) { /** * Calculates the motor speeds for a differential swerve module. * - * @param rotationSpeed the current rotation speed of the pod - * @param wheelSpeed the current speed of the wheel + * @param speed the speed for the pod + * @param angle the angle the pod needs to point to + * @param motor1pos the encoder pose for motor1 + * @param motor2pos the encoder pose for motor2 * @return motor speeds (array of length 2) */ public double[] calculateMotorSpeeds(double speed, double angle, double motor1pos, double motor2pos){ @@ -124,7 +126,7 @@ public double calculateModuleAngle(double motor1pos, double motor2pos){ * * @param speed the speed to set the module to */ - private void setMotor1Power(double power){ + public void setMotor1Power(double power){ return; } /** @@ -134,7 +136,7 @@ private void setMotor1Power(double power){ * * @param speed the speed to set the module to */ - private void setMotor2Power(double power){ + public void setMotor2Power(double power){ return; }