From 14a5fb74b13b8e71d141b0c28280b653fc51b6e7 Mon Sep 17 00:00:00 2001 From: Richie Xue Date: Wed, 30 Aug 2023 12:39:26 -0400 Subject: [PATCH 01/10] Add pose for back limelight --- src/main/java/com/stuypulse/robot/constants/Settings.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index d4e83c68..b3089c1a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -75,12 +75,14 @@ public interface Vision { public interface Limelight { String [] LIMELIGHTS = { "limelight-left", - "limelight-right" + "limelight-right", + "limelight-back" }; int[] PORTS = {5800, 5801, 5802, 5803, 5804, 5805}; Pose3d [] POSITIONS = new Pose3d[] { new Pose3d(new Translation3d(0.0635, -0.3429, 0.4699), new Rotation3d(Math.toRadians(90), 0, Math.toRadians(-30))), - new Pose3d(new Translation3d(0.0635, 0.3429, 0.4699), new Rotation3d(Math.toRadians(-90), 0, Math.toRadians(30))) + new Pose3d(new Translation3d(0.0635, 0.3429, 0.4699), new Rotation3d(Math.toRadians(-90), 0, Math.toRadians(30))), + new Pose3d(new Translation3d(0.1, 0, 1.29032), new Rotation3d(0, Math.toRadians(-30), Math.PI)) }; } } From cc324d79d136426f3f869a844cf33534f826f362 Mon Sep 17 00:00:00 2001 From: Richie Xue Date: Wed, 30 Aug 2023 14:39:21 -0400 Subject: [PATCH 02/10] Changed: -driver filters settings -CONE_TIP_OUT XPose, -stopped swerve when not moving while scoring --- .Glass/glass-window.json | 118 ++++++----- .Glass/glass.json | 188 +++++++++++++++--- .../robot/commands/RobotAlignThenScore.java | 4 + .../commands/RobotAlignThenScoreCubes.java | 4 + .../com/stuypulse/robot/constants/Field.java | 2 +- .../stuypulse/robot/constants/Settings.java | 12 +- 6 files changed, 242 insertions(+), 86 deletions(-) diff --git a/.Glass/glass-window.json b/.Glass/glass-window.json index 36235e3a..8b6bbb44 100644 --- a/.Glass/glass-window.json +++ b/.Glass/glass-window.json @@ -1,8 +1,8 @@ { "Docking": { "Data": [ - "DockSpace ID=0x8B93E3BD Window=0xA787BDB4 Pos=0,22 Size=32,32 Split=X Selected=0xC98BB475", - "DockNode ID=0x00000002 Parent=0x8B93E3BD SizeRef=1281,979 Split=X", + "DockSpace ID=0x8B93E3BD Window=0xA787BDB4 Pos=0,31 Size=2560,1524 Split=X Selected=0xC98BB475", + "DockNode ID=0x00000002 Parent=0x8B93E3BD SizeRef=1565,979 Split=X", "DockNode ID=0x00000001 Parent=0x00000002 SizeRef=622,213 Split=Y", "DockNode ID=0x00000004 Parent=0x00000001 SizeRef=742,213 Split=X Selected=0x0EC36FA3", "DockNode ID=0x00000008 Parent=0x00000004 SizeRef=495,190 Selected=0xE290EDCE", @@ -10,10 +10,10 @@ "DockNode ID=0x00000005 Parent=0x00000001 SizeRef=742,211 Split=X Selected=0xC59FBC06", "DockNode ID=0x0000000A Parent=0x00000005 SizeRef=501,188 Selected=0x3F2B753E", "DockNode ID=0x0000000B Parent=0x00000005 SizeRef=119,188 Selected=0xD2C348FC", - "DockNode ID=0x00000007 Parent=0x00000002 SizeRef=657,213 Split=Y Selected=0xF5CC1934", - "DockNode ID=0x00000011 Parent=0x00000007 SizeRef=718,484 CentralNode=1 Selected=0xF5CC1934", + "DockNode ID=0x00000007 Parent=0x00000002 SizeRef=941,213 Split=Y Selected=0xF5CC1934", + "DockNode ID=0x00000011 Parent=0x00000007 SizeRef=718,484 CentralNode=1 Selected=0xB5CBE9F0", "DockNode ID=0x00000012 Parent=0x00000007 SizeRef=718,493 Selected=0xC98BB475", - "DockNode ID=0x00000003 Parent=0x8B93E3BD SizeRef=637,979 Split=Y Selected=0x2D9B9756", + "DockNode ID=0x00000003 Parent=0x8B93E3BD SizeRef=993,979 Split=Y Selected=0x2D9B9756", "DockNode ID=0x00000006 Parent=0x00000003 SizeRef=501,757 Split=Y Selected=0x2D9B9756", "DockNode ID=0x0000000F Parent=0x00000006 SizeRef=501,642 Selected=0x2D9B9756", "DockNode ID=0x00000010 Parent=0x00000006 SizeRef=501,113 Selected=0x410DE7DE", @@ -22,53 +22,52 @@ }, "MainWindow": { "GLOBAL": { - "fps": "120", - "height": "1001", + "height": "1555", "maximized": "1", "style": "0", "userScale": "2", - "width": "1920", + "width": "2560", "xpos": "0", - "ypos": "34" + "ypos": "45" } }, "Table": { "0x542B5671,2": { - "Column 0 Width": "391", - "Column 1 Width": "268", - "RefScale": "16" + "Column 0 Width": "493", + "Column 1 Width": "418", + "RefScale": "25" } }, "Window": { "###/SmartDashboard/Arm Mech2d": { "Collapsed": "0", "DockId": "0x00000012,1", - "Pos": "9,39", - "Size": "6,15" + "Pos": "624,1062", + "Size": "941,493" }, "###/SmartDashboard/Autonomous": { "Collapsed": "0", "DockId": "0x00000010,0", - "Pos": "17,42", - "Size": "15,3" + "Pos": "1567,1034", + "Size": "993,176" }, "###/SmartDashboard/Field": { "Collapsed": "0", "DockId": "0x00000011,0", - "Pos": "9,22", - "Size": "6,15" + "Pos": "624,31", + "Size": "941,1029" }, "###NetworkTables": { "Collapsed": "0", "DockId": "0x0000000F,0", - "Pos": "17,22", - "Size": "15,18" + "Pos": "1567,31", + "Size": "993,1001" }, "###NetworkTables Settings": { "Collapsed": "0", "DockId": "0x0000000C,0", - "Pos": "17,47", - "Size": "15,7" + "Pos": "1567,1212", + "Size": "993,343" }, "###Plot <0>": { "Collapsed": "0", @@ -78,32 +77,32 @@ "###Plot <10>": { "Collapsed": "0", "DockId": "0x00000008,0", - "Pos": "0,22", - "Size": "4,15" + "Pos": "0,31", + "Size": "495,765" }, "###Plot <11>": { "Collapsed": "0", "DockId": "0x0000000A,0", - "Pos": "0,39", - "Size": "4,15" + "Pos": "0,798", + "Size": "501,757" }, "###Plot <12>": { "Collapsed": "0", "DockId": "0x00000009,1", - "Pos": "6,22", - "Size": "1,15" + "Pos": "497,31", + "Size": "125,765" }, "###Plot <13>": { "Collapsed": "0", "DockId": "0x00000008,1", - "Pos": "0,22", - "Size": "4,15" + "Pos": "0,31", + "Size": "495,765" }, "###Plot <14>": { "Collapsed": "0", "DockId": "0x0000000A,1", - "Pos": "0,39", - "Size": "4,15" + "Pos": "0,798", + "Size": "501,757" }, "###Plot <15>": { "Collapsed": "1", @@ -118,38 +117,53 @@ "###Plot <17>": { "Collapsed": "0", "DockId": "0x00000012,0", - "Pos": "9,39", - "Size": "6,15" + "Pos": "624,1062", + "Size": "941,493" }, "###Plot <18>": { "Collapsed": "0", "DockId": "0x00000008,2", - "Pos": "0,22", - "Size": "4,15" + "Pos": "0,31", + "Size": "495,765" }, "###Plot <19>": { "Collapsed": "0", "DockId": "0x00000009,2", - "Pos": "6,22", - "Size": "1,15" + "Pos": "497,31", + "Size": "125,765" }, "###Plot <1>": { "Collapsed": "0", "DockId": "0x00000011,1", - "Pos": "9,22", - "Size": "6,15" + "Pos": "624,31", + "Size": "941,1029" }, "###Plot <20>": { "Collapsed": "0", "DockId": "0x0000000A,2", - "Pos": "0,39", - "Size": "4,15" + "Pos": "0,798", + "Size": "501,757" }, "###Plot <21>": { "Collapsed": "0", "DockId": "0x0000000B,2", - "Pos": "6,39", - "Size": "1,15" + "Pos": "503,798", + "Size": "119,757" + }, + "###Plot <22>": { + "Collapsed": "0", + "Pos": "60,60", + "Size": "770,830" + }, + "###Plot <23>": { + "Collapsed": "0", + "Pos": "840,66", + "Size": "718,822" + }, + "###Plot <24>": { + "Collapsed": "0", + "Pos": "112,926", + "Size": "1494,619" }, "###Plot <2>": { "Collapsed": "0", @@ -164,8 +178,8 @@ "###Plot <4>": { "Collapsed": "0", "DockId": "0x0000000B,1", - "Pos": "6,39", - "Size": "1,15" + "Pos": "503,798", + "Size": "119,757" }, "###Plot <5>": { "Collapsed": "0", @@ -180,14 +194,14 @@ "###Plot <7>": { "Collapsed": "0", "DockId": "0x00000009,0", - "Pos": "6,22", - "Size": "1,15" + "Pos": "497,31", + "Size": "125,765" }, "###Plot <8>": { "Collapsed": "0", "DockId": "0x0000000B,0", - "Pos": "6,39", - "Size": "1,15" + "Pos": "503,798", + "Size": "119,757" }, "###Plot <9>": { "Collapsed": "0", @@ -201,8 +215,8 @@ }, "DockSpaceViewport_11111111": { "Collapsed": "0", - "Pos": "0,22", - "Size": "32,32" + "Pos": "0,31", + "Size": "2560,1524" } } } diff --git a/.Glass/glass.json b/.Glass/glass.json index 2ac6743c..bce9dc32 100644 --- a/.Glass/glass.json +++ b/.Glass/glass.json @@ -1,8 +1,5 @@ { "NetworkTables": { - "Persistent Values": { - "open": false - }, "Retained Values": { "open": false }, @@ -19,17 +16,19 @@ }, "transitory": { "SmartDashboard": { + "Alignment": { + "Controller": { + "open": true + }, + "open": true + }, "Arm": { "Shoulder": { "open": true }, "Wrist": { "open": true - }, - "open": true - }, - "Arm Trajectories": { - "open": true + } }, "Auto Balance": { "Translation": { @@ -42,9 +41,6 @@ "open": true } }, - "BOOM": { - "open": true - }, "Back Left": { "open": true }, @@ -54,6 +50,20 @@ "Driver Setings": { "open": true }, + "Driver Settings": { + "Drive": { + "open": true + }, + "Gyro Feedback": { + "open": true + }, + "Turn": { + "open": true + } + }, + "Odometry": { + "open": true + }, "Settings": { "Intake": { "open": true @@ -85,6 +95,12 @@ }, "limelight-back": { "open": true + }, + "limelight-left": { + "open": true + }, + "limelight-right": { + "open": true } }, "Wrist": { @@ -135,14 +151,14 @@ } }, "NetworkTables Settings": { - "mode": "Disabled", + "mode": "Client (NT3)", "serverTeam": "10.6.94.2" }, "Plots": { "Plot <0>": { "plots": [ { - "height": 416, + "height": 398, "series": [ { "color": [ @@ -186,7 +202,7 @@ "autoFit": true } ], - "height": -30, + "height": -39, "series": [ { "color": [ @@ -221,7 +237,7 @@ "autoFit": true } ], - "height": -30, + "height": -39, "series": [ { "color": [ @@ -251,7 +267,7 @@ "Plot <12>": { "plots": [ { - "height": -30, + "height": -39, "series": [ { "color": [ @@ -286,7 +302,7 @@ "autoFit": true } ], - "height": -30, + "height": -39, "series": [ { "color": [ @@ -316,7 +332,7 @@ "Plot <14>": { "plots": [ { - "height": -30, + "height": -39, "series": [ { "color": [ @@ -422,7 +438,7 @@ "autoFit": true } ], - "height": 425, + "height": 407, "series": [ { "color": [ @@ -470,7 +486,7 @@ "Plot <18>": { "plots": [ { - "height": 423, + "height": 679, "series": [ { "color": [ @@ -500,7 +516,7 @@ "Plot <19>": { "plots": [ { - "height": -30, + "height": -39, "series": [ { "color": [ @@ -530,7 +546,7 @@ "Plot <1>": { "plots": [ { - "height": 416, + "height": 1469, "series": [ { "color": [ @@ -596,7 +612,7 @@ "Plot <20>": { "plots": [ { - "height": 418, + "height": 671, "series": [ { "color": [ @@ -626,7 +642,7 @@ "Plot <21>": { "plots": [ { - "height": -30, + "height": -39, "series": [ { "color": [ @@ -653,6 +669,124 @@ "name": "Back Right Current" } }, + "Plot <22>": { + "plots": [ + { + "axis": [ + { + "autoFit": true + } + ], + "height": 744, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/Alignment/Controller/X Measurement (meters)" + }, + { + "color": [ + 0.8666667342185974, + 0.5176470875740051, + 0.32156863808631897, + 1.0 + ], + "id": "NT:/SmartDashboard/Alignment/Controller/X Setpoint (meters)" + }, + { + "color": [ + 0.3333333432674408, + 0.658823549747467, + 0.4078431725502014, + 1.0 + ], + "id": "NT:/SmartDashboard/Alignment/Controller/Is Aligned X" + } + ] + } + ] + }, + "Plot <23>": { + "plots": [ + { + "axis": [ + { + "autoFit": true + } + ], + "height": 736, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/Alignment/Controller/Y Measurement (meters)" + }, + { + "color": [ + 0.8666667342185974, + 0.5176470875740051, + 0.32156863808631897, + 1.0 + ], + "id": "NT:/SmartDashboard/Alignment/Controller/Y Setpoint (meters)" + }, + { + "color": [ + 0.3333333432674408, + 0.658823549747467, + 0.4078431725502014, + 1.0 + ], + "id": "NT:/SmartDashboard/Alignment/Controller/Is Aligned Y" + } + ] + } + ] + }, + "Plot <24>": { + "plots": [ + { + "height": 533, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/Alignment/Controller/Angle Measurement (degrees)" + }, + { + "color": [ + 0.8666667342185974, + 0.5176470875740051, + 0.32156863808631897, + 1.0 + ], + "id": "NT:/SmartDashboard/Alignment/Controller/Angle Setpoint (degrees)" + }, + { + "color": [ + 0.3333333432674408, + 0.658823549747467, + 0.4078431725502014, + 1.0 + ], + "id": "NT:/SmartDashboard/Alignment/Controller/Is Aligned Angle" + } + ] + } + ] + }, "Plot <2>": { "plots": [ { @@ -719,7 +853,7 @@ "Plot <4>": { "plots": [ { - "height": -30, + "height": -39, "series": [ { "color": [ @@ -830,7 +964,7 @@ "autoFit": true } ], - "height": 423, + "height": 679, "series": [ { "color": [ @@ -865,7 +999,7 @@ "autoFit": true } ], - "height": 418, + "height": 671, "series": [ { "color": [ diff --git a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java index 9041ffbf..a8417a04 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java @@ -126,6 +126,10 @@ public void execute() { controller.update(targetPose, currentPose); if (aligned.get() || movingWhileScoring) { + if (!movingWhileScoring) { + swerve.stop(); + } + // simply outtake when low if (manager.getNodeLevel() == NodeLevel.LOW) { intake.deacquire(); diff --git a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java index a27d6950..99bc4c7c 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java @@ -100,6 +100,10 @@ public void execute() { controller.update(targetPose, currentPose); if (aligned.get() || movingWhileScoring) { + if (!movingWhileScoring) { + swerve.stop(); + } + // simply outtake when low if (manager.getNodeLevel() == NodeLevel.LOW) { intake.deacquire(); diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 07bbebe2..ac4b8d03 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -94,7 +94,7 @@ public interface Mid { Number CUBE_BACK = 1.881; Number CUBE_FRONT = 2.106; Number CONE_TIP_IN = 2.231; - Number CONE_TIP_OUT = 2.052; + Number CONE_TIP_OUT = 2.18; } public interface Low { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b3089c1a..c561925c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -326,17 +326,17 @@ public interface Driver { public interface Drive { SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.10); - SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.125); + SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.2); //was 0.125 SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); SmartNumber MAX_TELEOP_SPEED = new SmartNumber("Driver Settings/Drive/Max Speed", Swerve.MAX_MODULE_SPEED.get()); - SmartNumber MAX_TELEOP_ACCEL = new SmartNumber("Driver Settings/Drive/Max Accleration", 20); + SmartNumber MAX_TELEOP_ACCEL = new SmartNumber("Driver Settings/Drive/Max Accleration", 18); //was 20 } public interface Turn { SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.05); - SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.05); + SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.15); //was 0.05 SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); SmartNumber MAX_TELEOP_TURNING = new SmartNumber("Driver Settings/Turn/Max Turning", 7.0); @@ -363,15 +363,15 @@ public interface Alignment { SmartNumber ALIGNED_CUBE_THRESHOLD_X = new SmartNumber("Alignment/X Cube Threshold", 0.08); SmartNumber ALIGNED_CUBE_THRESHOLD_Y = new SmartNumber("Alignment/Y Cube Threshold", 0.1); - SmartNumber ALIGNED_CUBE_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Cube Threshold", 5); + SmartNumber ALIGNED_CUBE_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Cube Threshold", 90); SmartNumber ALIGNED_CONE_THRESHOLD_X = new SmartNumber("Alignment/X Cone Threshold", 0.08); SmartNumber ALIGNED_CONE_THRESHOLD_Y = new SmartNumber("Alignment/Y Cone Threshold", 0.05); - SmartNumber ALIGNED_CONE_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Cone Threshold", 1); + SmartNumber ALIGNED_CONE_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Cone Threshold", 90); SmartNumber ALIGNED_LOW_THRESHOLD_X = new SmartNumber("Alignment/X Low Threshold", 0.16); SmartNumber ALIGNED_LOW_THRESHOLD_Y = new SmartNumber("Alignment/Y Low Threshold", 0.2); - SmartNumber ALIGNED_LOW_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Low Threshold", 10); + SmartNumber ALIGNED_LOW_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Low Threshold", 90); SmartNumber AGAINST_GRID_VEL_X = new SmartNumber("Alignment/Against Grid X Velocity (m per s)", 0.02); From 6945aa85f1a979dfc2dd636140175bda7684d624 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Wed, 30 Aug 2023 16:20:55 -0400 Subject: [PATCH 03/10] Clean settings --- .../java/com/stuypulse/robot/constants/Settings.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index c561925c..9307d628 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -326,17 +326,17 @@ public interface Driver { public interface Drive { SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.10); - SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.2); //was 0.125 + SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.2); SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); SmartNumber MAX_TELEOP_SPEED = new SmartNumber("Driver Settings/Drive/Max Speed", Swerve.MAX_MODULE_SPEED.get()); - SmartNumber MAX_TELEOP_ACCEL = new SmartNumber("Driver Settings/Drive/Max Accleration", 18); //was 20 + SmartNumber MAX_TELEOP_ACCEL = new SmartNumber("Driver Settings/Drive/Max Accleration", 18); } public interface Turn { SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.05); - SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.15); //was 0.05 + SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.15); SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); SmartNumber MAX_TELEOP_TURNING = new SmartNumber("Driver Settings/Turn/Max Turning", 7.0); @@ -363,15 +363,15 @@ public interface Alignment { SmartNumber ALIGNED_CUBE_THRESHOLD_X = new SmartNumber("Alignment/X Cube Threshold", 0.08); SmartNumber ALIGNED_CUBE_THRESHOLD_Y = new SmartNumber("Alignment/Y Cube Threshold", 0.1); - SmartNumber ALIGNED_CUBE_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Cube Threshold", 90); + SmartNumber ALIGNED_CUBE_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Cube Threshold", 8); SmartNumber ALIGNED_CONE_THRESHOLD_X = new SmartNumber("Alignment/X Cone Threshold", 0.08); SmartNumber ALIGNED_CONE_THRESHOLD_Y = new SmartNumber("Alignment/Y Cone Threshold", 0.05); - SmartNumber ALIGNED_CONE_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Cone Threshold", 90); + SmartNumber ALIGNED_CONE_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Cone Threshold", 3); SmartNumber ALIGNED_LOW_THRESHOLD_X = new SmartNumber("Alignment/X Low Threshold", 0.16); SmartNumber ALIGNED_LOW_THRESHOLD_Y = new SmartNumber("Alignment/Y Low Threshold", 0.2); - SmartNumber ALIGNED_LOW_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Low Threshold", 90); + SmartNumber ALIGNED_LOW_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Low Threshold", 8); SmartNumber AGAINST_GRID_VEL_X = new SmartNumber("Alignment/Against Grid X Velocity (m per s)", 0.02); From 087dbb0b5500ef693254c0322dab92dd125c8331 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 31 Aug 2023 11:36:32 -0400 Subject: [PATCH 04/10] More alignment logging --- .../stuypulse/robot/commands/RobotAlignThenScoreCubes.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java index 99bc4c7c..202cd84f 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java @@ -64,17 +64,20 @@ public RobotAlignThenScoreCubes(){ private boolean isAligned() { if(Manager.getInstance().getNodeLevel() == Manager.NodeLevel.LOW) { + SmartDashboard.putString("Alignment/Mode", "LOW"); return controller.isDone( Alignment.ALIGNED_LOW_THRESHOLD_X.get(), Alignment.ALIGNED_LOW_THRESHOLD_Y.get(), Alignment.ALIGNED_LOW_THRESHOLD_ANGLE.get()); } else if (Manager.getInstance().getGamePiece().isCone()) { + SmartDashboard.putString("Alignment/Mode", "CONE"); return controller.isDone( Alignment.ALIGNED_CONE_THRESHOLD_X.get(), Alignment.ALIGNED_CONE_THRESHOLD_Y.get(), Alignment.ALIGNED_CONE_THRESHOLD_ANGLE.get()); } else { + SmartDashboard.putString("Alignment/Mode", "CUBE"); return controller.isDone( Alignment.ALIGNED_CUBE_THRESHOLD_X.get(), Alignment.ALIGNED_CUBE_THRESHOLD_Y.get(), @@ -122,6 +125,9 @@ public void execute() { } else { swerve.setChassisSpeeds(controller.getOutput()); } + + SmartDashboard.putBoolean("Alignment/Aligned", isAligned()); + SmartDashboard.putBoolean("Alignment/Aligned Debounced", aligned.get()); } @Override From 366474460f000eff93b15ca21174a226e76856e8 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 31 Aug 2023 11:36:50 -0400 Subject: [PATCH 05/10] Cone and cube back deaquire arm trajectories --- .../com/stuypulse/robot/constants/ArmTrajectories.java | 10 +++++++--- .../java/com/stuypulse/robot/subsystems/Manager.java | 7 ++++++- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java index 34da509f..504bf13d 100644 --- a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java +++ b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java @@ -46,9 +46,13 @@ public interface Deacquire { new SmartNumber("Arm Trajectories/Deacquire Front Shoulder", -77), new SmartNumber("Arm Trajectories/Deacquire Front Wrist", 90)); - ArmState kBackTrajectory = new ArmState( - new SmartNumber("Arm Trajectories/Deacquire Back Shoulder", -134), - new SmartNumber("Arm Trajectories/Deacquire Back Wrist", -22.9)); + ArmState kBackConeTrajectory = new ArmState( + new SmartNumber("Arm Trajectories/Deacquire Cone Back Shoulder", -110), + new SmartNumber("Arm Trajectories/Deacquire Cone Back Wrist", 177)); + + ArmState kBackCubeTrajectory = new ArmState( + new SmartNumber("Arm Trajectories/Deacquire Cube Back Shoulder", -134), + new SmartNumber("Arm Trajectories/Deacquire Cube Back Wrist", -22.9)); } public interface Stow { diff --git a/src/main/java/com/stuypulse/robot/subsystems/Manager.java b/src/main/java/com/stuypulse/robot/subsystems/Manager.java index aa89f86b..7719459f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/Manager.java +++ b/src/main/java/com/stuypulse/robot/subsystems/Manager.java @@ -122,7 +122,12 @@ public ArmState getReadyTrajectory() { } private ArmState getLowReadyTrajectory() { - return scoreSide == ScoreSide.FRONT ? Deacquire.kFrontTrajectory : Deacquire.kBackTrajectory; + if (scoreSide == ScoreSide.FRONT) + return Deacquire.kFrontTrajectory; + else if (gamePiece.isCone()) + return Deacquire.kBackConeTrajectory; + else + return Deacquire.kBackCubeTrajectory; } private ArmState getMidReadyTrajectory() { From 8ff1383eb33d31a91c55afa4c7fa0c90652c6057 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 31 Aug 2023 11:37:02 -0400 Subject: [PATCH 06/10] Tune alignment poses --- src/main/java/com/stuypulse/robot/constants/Field.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index ac4b8d03..af71fccf 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -86,19 +86,19 @@ public interface ScoreXPoses { public interface High { Number CUBE_BACK = 1.846; Number CUBE_FRONT = 1.825; - Number CONE_TIP_IN = new RedBlueNumber(1.881, 1.8556); + Number CONE_TIP_IN = 1.75; Number CONE_TIP_OUT = 1.772; } public interface Mid { Number CUBE_BACK = 1.881; Number CUBE_FRONT = 2.106; - Number CONE_TIP_IN = 2.231; + Number CONE_TIP_IN = 2.21; Number CONE_TIP_OUT = 2.18; } public interface Low { - Number BACK = 1.825; + Number BACK = 1.9; Number FRONT = 1.825; } } From 55c7e5ce3aff6614207a1718270fc6c24fd4b530 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 31 Aug 2023 11:37:13 -0400 Subject: [PATCH 07/10] Tune alignment constants --- src/main/java/com/stuypulse/robot/constants/Settings.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 9307d628..19b745b8 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -367,7 +367,7 @@ public interface Alignment { SmartNumber ALIGNED_CONE_THRESHOLD_X = new SmartNumber("Alignment/X Cone Threshold", 0.08); SmartNumber ALIGNED_CONE_THRESHOLD_Y = new SmartNumber("Alignment/Y Cone Threshold", 0.05); - SmartNumber ALIGNED_CONE_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Cone Threshold", 3); + SmartNumber ALIGNED_CONE_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Cone Threshold", 1.5); SmartNumber ALIGNED_LOW_THRESHOLD_X = new SmartNumber("Alignment/X Low Threshold", 0.16); SmartNumber ALIGNED_LOW_THRESHOLD_Y = new SmartNumber("Alignment/Y Low Threshold", 0.2); @@ -383,7 +383,7 @@ public interface Translation { SmartNumber D = new SmartNumber("Alignment/Translation/kD", 0.0); } public interface Rotation { - SmartNumber P = new SmartNumber("Alignment/Rotation/kP", 2); + SmartNumber P = new SmartNumber("Alignment/Rotation/kP", 3); SmartNumber I = new SmartNumber("Alignment/Rotation/kI", 0); SmartNumber D = new SmartNumber("Alignment/Rotation/kD", 0); } From d4f935050f7e55f3d00ac2ecd339af10ccac5be7 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 31 Aug 2023 14:10:27 -0400 Subject: [PATCH 08/10] Tune driver filters --- src/main/java/com/stuypulse/robot/constants/Settings.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 19b745b8..722aba70 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -324,13 +324,13 @@ public interface Operator { public interface Driver { public interface Drive { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.10); + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.03); SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.2); - SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); + SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 1.5); SmartNumber MAX_TELEOP_SPEED = new SmartNumber("Driver Settings/Drive/Max Speed", Swerve.MAX_MODULE_SPEED.get()); - SmartNumber MAX_TELEOP_ACCEL = new SmartNumber("Driver Settings/Drive/Max Accleration", 18); + SmartNumber MAX_TELEOP_ACCEL = new SmartNumber("Driver Settings/Drive/Max Accleration", 15); } public interface Turn { @@ -339,7 +339,7 @@ public interface Turn { SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.15); SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); - SmartNumber MAX_TELEOP_TURNING = new SmartNumber("Driver Settings/Turn/Max Turning", 7.0); + SmartNumber MAX_TELEOP_TURNING = new SmartNumber("Driver Settings/Turn/Max Turning", 6.0); public interface GyroFeedback { SmartBoolean GYRO_FEEDBACK_ENABLED = new SmartBoolean("Driver Settings/Gyro Feedback/Enabled", true); From ee431733010731c37c1cc3f505aacbaa88d992cb Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 31 Aug 2023 14:59:40 -0400 Subject: [PATCH 09/10] LED green when outtaking align --- .../com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java index 202cd84f..a60fdbfb 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java @@ -109,6 +109,7 @@ public void execute() { // simply outtake when low if (manager.getNodeLevel() == NodeLevel.LOW) { + LEDController.getInstance().setColor(LEDColor.GREEN, 694000000); intake.deacquire(); } @@ -118,6 +119,7 @@ public void execute() { // only score for cubes if (manager.getGamePiece().isCube()) { + LEDController.getInstance().setColor(LEDColor.GREEN, 694000000); intake.deacquire(); } } From 7f83890a1a7884c10ea24aa0e4908e3f9739b925 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 31 Aug 2023 14:59:50 -0400 Subject: [PATCH 10/10] Adjust peg Y poses --- .../com/stuypulse/robot/constants/Field.java | 37 ++++++++++--------- 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index af71fccf..66e6286d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -55,28 +55,29 @@ public static Pose2d getAprilTagFromId(int id) { } public interface Pegs { + // LAB VALUES double RED_Y[] = { - 7.494778, - 6.935978, - 6.377178, - 5.843778, - 5.259578, - 4.700778, - 4.141978, - 3.583178, - 3.024378 + 7.494778 - 0.075, + 6.935978 - 0.075, + 6.377178 - 0.075, + 5.843778 - 0.075, + 5.259578 - 0.075, + 4.700778 - 0.075, + 4.141978 - 0.075, + 3.583178 - 0.075, + 3.024378 - 0.075 }; double BLUE_Y[] = { - 4.9784, - 4.4196, - 3.8608, - 3.3020, - 2.7432, - 2.1844, - 1.6256, - 1.0668, - 0.5080 + 4.9784 - 0.06, + 4.4196 - 0.06, + 3.8608 - 0.06, + 3.3020 - 0.06, + 2.7432 - 0.06, + 2.1844 - 0.06, + 1.6256 - 0.06, + 1.0668 - 0.06, + 0.5080 - 0.06 }; }