From 162dfe63a0bbbad0ed124bf4890c66a3b71c0c72 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Wed, 24 May 2023 17:14:39 -0400 Subject: [PATCH 1/2] Add tuning values --- .../com/stuypulse/robot/constants/Settings.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 85f8ebee..e0ff768e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -121,7 +121,7 @@ public interface Drive { public interface FrontRight { String ID = "Front Right"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(357.213206) // recalibrated 4/21 + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(2.419310) // recalibrated 3/24 .plus(Rotation2d.fromDegrees(0)); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * -0.5); } @@ -142,7 +142,7 @@ public interface BackLeft { public interface BackRight { String ID = "Back Right"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(6.342437) // recalibrated 4/21 + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(7.748473) // recalibrated 5/24 .plus(Rotation2d.fromDegrees(90)); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * -0.5); } @@ -237,7 +237,7 @@ public interface Wrist { MOI, RADIUS); - Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.662482 + 60.0/360.0).plus(Rotation2d.fromDegrees(180)); + Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.662482).plus(Rotation2d.fromDegrees(120)); SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Wrist/Teleop Max Velocity (deg)", 480.0); SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Wrist/Teleop Max Acceleration (deg)", 480.0); @@ -255,16 +255,16 @@ public interface Wrist { SmartNumber INTAKE_VOLTAGE = new SmartNumber("Arm/Wrist/Intake Voltage", 0); public interface PID { - SmartNumber kP = new SmartNumber("Arm/Wrist/kP", 6.0); + SmartNumber kP = new SmartNumber("Arm/Wrist/kP", 3.0); SmartNumber kI = new SmartNumber("Arm/Wrist/kI", 0); - SmartNumber kD = new SmartNumber("Arm/Wrist/kD", 1); + SmartNumber kD = new SmartNumber("Arm/Wrist/kD", 0.1); } public interface Feedforward { SmartNumber kS = new SmartNumber("Arm/Wrist/kS", 0.0); - SmartNumber kA = new SmartNumber("Arm/Wrist/kA", 0.01); + SmartNumber kA = new SmartNumber("Arm/Wrist/kA", 0.04); SmartNumber kG = new SmartNumber("Arm/Wrist/kG", 0.0); - SmartNumber kV = new SmartNumber("Arm/Wrist/kV", 1.0); + SmartNumber kV = new SmartNumber("Arm/Wrist/kV", 1.7); } } } From d620ae68fdda15138a13beba7b123fd2909b9c03 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Wed, 24 May 2023 17:44:26 -0400 Subject: [PATCH 2/2] Move arm motor back to tower --- .../java/com/stuypulse/robot/constants/Settings.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index e0ff768e..9ccae1bd 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -121,7 +121,7 @@ public interface Drive { public interface FrontRight { String ID = "Front Right"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(2.419310) // recalibrated 3/24 + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(357.213206) // recalibrated 4/21 .plus(Rotation2d.fromDegrees(0)); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * -0.5); } @@ -255,16 +255,16 @@ public interface Wrist { SmartNumber INTAKE_VOLTAGE = new SmartNumber("Arm/Wrist/Intake Voltage", 0); public interface PID { - SmartNumber kP = new SmartNumber("Arm/Wrist/kP", 3.0); + SmartNumber kP = new SmartNumber("Arm/Wrist/kP", 6.0); SmartNumber kI = new SmartNumber("Arm/Wrist/kI", 0); - SmartNumber kD = new SmartNumber("Arm/Wrist/kD", 0.1); + SmartNumber kD = new SmartNumber("Arm/Wrist/kD", 1); } public interface Feedforward { SmartNumber kS = new SmartNumber("Arm/Wrist/kS", 0.0); - SmartNumber kA = new SmartNumber("Arm/Wrist/kA", 0.04); + SmartNumber kA = new SmartNumber("Arm/Wrist/kA", 0.01); SmartNumber kG = new SmartNumber("Arm/Wrist/kG", 0.0); - SmartNumber kV = new SmartNumber("Arm/Wrist/kV", 1.7); + SmartNumber kV = new SmartNumber("Arm/Wrist/kV", 1.0); } } }