From 5e4932f68c716c3b90ce9fee080428d1bf882fca Mon Sep 17 00:00:00 2001 From: StabStabImAChicken <77760254+StabStabImAChicken@users.noreply.github.com> Date: Wed, 18 Jan 2023 17:57:54 -0600 Subject: [PATCH 1/8] Auto-Balance Update 0.1 - Added a driveType in SwerveDrive.java called AUTO_BALANCE - When the robot is in AUTO_BALANCE, robot will move at an xSpeed based upon it's pitch times 2 (for example, with a pitch of -15, the robot will move at an xSpeed of -30) --- .../year2023/subsystems/swerve/SwerveDrive.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java index a1e8c58f..f947b185 100644 --- a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java @@ -64,7 +64,7 @@ public class SwerveDrive extends SwerveDriveTemplate { private WSSwerveHelper swerveHelper = new WSSwerveHelper(); private AimHelper limelight; - public enum driveType {TELEOP, AUTO, CROSS, LL}; + public enum driveType {TELEOP, AUTO, CROSS, LL, AUTO_BALANCE}; public driveType driveState; @Override @@ -266,6 +266,13 @@ public void update() { this.swerveSignal = swerveHelper.setDrive(xSpeed, ySpeed, rotSpeed, getGyroAngle()); drive(); } + if (driveState == driveType.AUTO_BALANCE){ + if(gyro.getPitch() != 0){ + if(Math.abs(gyro.getPitch()) <= 5){ + xSpeed = gyro.getPitch() * 2; + } + } + } SmartDashboard.putNumber("Gyro Reading", getGyroAngle()); SmartDashboard.putNumber("X speed", xSpeed); SmartDashboard.putNumber("Y speed", ySpeed); From 47d1c6a216c821d9e1d96b768694d031b76e2ec3 Mon Sep 17 00:00:00 2001 From: StabStabImAChicken <77760254+StabStabImAChicken@users.noreply.github.com> Date: Wed, 18 Jan 2023 18:13:52 -0600 Subject: [PATCH 2/8] Update SwerveDrive.java - Changed AUTO_BALANCE to move bot when Math.abs(getPitch()) is greater than or equal to 5, instead of less than or equal to --- .../org/wildstang/year2023/subsystems/swerve/SwerveDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java index f947b185..c294217c 100644 --- a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java @@ -268,7 +268,7 @@ public void update() { } if (driveState == driveType.AUTO_BALANCE){ if(gyro.getPitch() != 0){ - if(Math.abs(gyro.getPitch()) <= 5){ + if(Math.abs(gyro.getPitch()) >= 5){ xSpeed = gyro.getPitch() * 2; } } From 3c65b7fce9081255e0798fef6f8fa1f9040bd439 Mon Sep 17 00:00:00 2001 From: StabStabImAChicken <77760254+StabStabImAChicken@users.noreply.github.com> Date: Thu, 19 Jan 2023 17:36:15 -0600 Subject: [PATCH 3/8] Roll Update - The previous version of AUTO_BALANCE only checked the pitch of the robot and adjusted the xSpeed. Now, the robot will check it's roll as well, as adjust it's ySpeed accordingly (in the same manner xSpeed was changed) --- .../wildstang/year2023/subsystems/swerve/SwerveDrive.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java index c294217c..35beef59 100644 --- a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java @@ -272,6 +272,11 @@ public void update() { xSpeed = gyro.getPitch() * 2; } } + if(gyro.getRoll() != 0){ + if(Math.abs(gyro.getRoll()) >= 5){ + ySpeed = gyro.getRoll() * 2; + } + } } SmartDashboard.putNumber("Gyro Reading", getGyroAngle()); SmartDashboard.putNumber("X speed", xSpeed); From 541be268c249d9d16bd18f925db9bbc47be7ddbf Mon Sep 17 00:00:00 2001 From: StabStabImAChicken <77760254+StabStabImAChicken@users.noreply.github.com> Date: Thu, 19 Jan 2023 18:21:04 -0600 Subject: [PATCH 4/8] Value changes - Swapped ySpeed and xSpeed. I mixed them up before --- .../org/wildstang/year2023/subsystems/swerve/SwerveDrive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java index 35beef59..19029e92 100644 --- a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java @@ -269,12 +269,12 @@ public void update() { if (driveState == driveType.AUTO_BALANCE){ if(gyro.getPitch() != 0){ if(Math.abs(gyro.getPitch()) >= 5){ - xSpeed = gyro.getPitch() * 2; + ySpeed = gyro.getPitch() * 2; } } if(gyro.getRoll() != 0){ if(Math.abs(gyro.getRoll()) >= 5){ - ySpeed = gyro.getRoll() * 2; + xSpeed = gyro.getRoll() * 2; } } } From ca3390a1ced4ad5bc4d966cf0e1a765d92553afa Mon Sep 17 00:00:00 2001 From: StabStabImAChicken <77760254+StabStabImAChicken@users.noreply.github.com> Date: Thu, 19 Jan 2023 18:34:21 -0600 Subject: [PATCH 5/8] Functional Update - Added else statements to both if(Math.abs(gyro.getPitch()) >= 5) and if(Math.abs(gyro.getRoll()) >= 5), so that the speeds are set to 0 while within 5 degrees of angle. - Added setDrive() and drive() at the end of AUTO_BALANCE --- .../wildstang/year2023/subsystems/swerve/SwerveDrive.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java index 19029e92..93229d0d 100644 --- a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java @@ -270,13 +270,19 @@ public void update() { if(gyro.getPitch() != 0){ if(Math.abs(gyro.getPitch()) >= 5){ ySpeed = gyro.getPitch() * 2; + }else{ + ySpeed = 0; } } if(gyro.getRoll() != 0){ if(Math.abs(gyro.getRoll()) >= 5){ xSpeed = gyro.getRoll() * 2; + }else{ + xSpeed = 0; } } + this.swerveSignal = swerveHelper.setDrive(xSpeed, ySpeed, 0, 0); + drive(); } SmartDashboard.putNumber("Gyro Reading", getGyroAngle()); SmartDashboard.putNumber("X speed", xSpeed); From 825ee4e28cc02da7a4545866c143afe8921e6ba4 Mon Sep 17 00:00:00 2001 From: StabStabImAChicken <77760254+StabStabImAChicken@users.noreply.github.com> Date: Fri, 20 Jan 2023 19:48:16 -0600 Subject: [PATCH 6/8] Issue Changes #1 - Removed if(gyro.getPitch() !=0) (and the same if statement for .getRoll()) as they were redundant and could cause the robot to move while pitch or roll is at 0 in some cases. - Slowed speed to a minimum of 2.5 and a max of 7.5 - Made code a little more readable, and added a comment above setDrive() explaining why roll and gyro are set to 0 --- .../subsystems/swerve/SwerveDrive.java | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java index 93229d0d..18b34749 100644 --- a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java @@ -267,20 +267,20 @@ public void update() { drive(); } if (driveState == driveType.AUTO_BALANCE){ - if(gyro.getPitch() != 0){ - if(Math.abs(gyro.getPitch()) >= 5){ - ySpeed = gyro.getPitch() * 2; - }else{ - ySpeed = 0; - } + if (Math.abs(gyro.getPitch()) >= 5){ + ySpeed = gyro.getPitch() * 0.5; } - if(gyro.getRoll() != 0){ - if(Math.abs(gyro.getRoll()) >= 5){ - xSpeed = gyro.getRoll() * 2; - }else{ - xSpeed = 0; - } + else { + ySpeed = 0; } + if (Math.abs(gyro.getRoll()) >= 5){ + xSpeed = gyro.getRoll() * 0.5; + } + else { + xSpeed = 0; + } + + //ignores robot's location the field, so translation can be robot centric this.swerveSignal = swerveHelper.setDrive(xSpeed, ySpeed, 0, 0); drive(); } From c629fbc13082f94b17de43f1a63c98165b542877 Mon Sep 17 00:00:00 2001 From: StabStabImAChicken <77760254+StabStabImAChicken@users.noreply.github.com> Date: Sun, 22 Jan 2023 13:45:52 -0600 Subject: [PATCH 7/8] Update to make merge of main into branch possible --- .../year2023/subsystems/swerve/SwerveDrive.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java index 18b34749..6457ee75 100644 --- a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java @@ -261,11 +261,11 @@ public void update() { this.swerveSignal = swerveHelper.setAuto(swerveHelper.getAutoPower(pathPos, pathVel, autoTravelled), pathHeading, rotSpeed, getGyroAngle()); drive(); } - if (driveState == driveType.LL) { - rotSpeed = -limelight.getRotPID(); - this.swerveSignal = swerveHelper.setDrive(xSpeed, ySpeed, rotSpeed, getGyroAngle()); - drive(); - } + //if (driveState == driveType.LL) { + // //rotSpeed = -limelight.getRotPID(); + // this.swerveSignal = swerveHelper.setDrive(xSpeed, ySpeed, rotSpeed, getGyroAngle()); + // drive(); + //} if (driveState == driveType.AUTO_BALANCE){ if (Math.abs(gyro.getPitch()) >= 5){ ySpeed = gyro.getPitch() * 0.5; From 59bbb649cd50542097f6b19a45b8e3469cbb4b3c Mon Sep 17 00:00:00 2001 From: StabStabImAChicken <77760254+StabStabImAChicken@users.noreply.github.com> Date: Sun, 22 Jan 2023 14:31:36 -0600 Subject: [PATCH 8/8] Should be a little faster - gyro.getPitch() and gyro.getRoll() are now stored in gyroPitch and gyroRoll, respectively, to not have to call from gyro multiple times. I plan to move these to a separate function in the future --- .../year2023/subsystems/swerve/SwerveDrive.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java index 8a4f2c99..270383e4 100644 --- a/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/org/wildstang/year2023/subsystems/swerve/SwerveDrive.java @@ -58,6 +58,8 @@ public class SwerveDrive extends SwerveDriveTemplate { private double[] lastY = {0,0,0,0}; private double autoTempX; private double autoTempY; + private double gyroPitch; + private double gyroRoll; //private final AHRS gyro = new AHRS(SerialPort.Port.kUSB); private final Pigeon2 gyro = new Pigeon2(CANConstants.GYRO); @@ -257,14 +259,18 @@ public void update() { // drive(); //} if (driveState == driveType.AUTO_BALANCE){ - if (Math.abs(gyro.getPitch()) >= 5){ - ySpeed = gyro.getPitch() * 0.5; + //storing variables + gyroPitch = gyro.getPitch(); + gyroRoll = gyro.getRoll(); + + if (Math.abs(gyroPitch) >= 5){ + ySpeed = gyroPitch * 0.5; } else { ySpeed = 0; } - if (Math.abs(gyro.getRoll()) >= 5){ - xSpeed = gyro.getRoll() * 0.5; + if (Math.abs(gyroRoll) >= 5){ + xSpeed = gyroRoll * 0.5; } else { xSpeed = 0;