From a51a29dd21122bdae906dadedfc3beb377c444a7 Mon Sep 17 00:00:00 2001 From: agheyi Date: Mon, 12 Sep 2022 16:41:58 -0700 Subject: [PATCH 1/3] replace math w/ calculate --- src/main/java/frc/team3128/commands/CmdAlign.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team3128/commands/CmdAlign.java b/src/main/java/frc/team3128/commands/CmdAlign.java index 698e99fc..48ec659c 100644 --- a/src/main/java/frc/team3128/commands/CmdAlign.java +++ b/src/main/java/frc/team3128/commands/CmdAlign.java @@ -1,6 +1,7 @@ package frc.team3128.commands; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -25,6 +26,7 @@ private enum VisionState { private boolean isAligned; private VisionState aimState = VisionState.SEARCHING; + private PIDController controller; /** * Aligns the robot to the hub using the limelight @@ -45,6 +47,7 @@ public void initialize() { // limelights.turnShooterLEDOn(); prevTime = RobotController.getFPGATime() / 1e6; plateauCount = 0; + controller = new PIDController(VISION_PID_kP, VISION_PID_kI, VISION_PID_kD); } @Override @@ -62,6 +65,7 @@ public void execute() { currHorizontalOffset = limelights.getShooterTX(); prevError = goalHorizontalOffset - currHorizontalOffset; aimState = VisionState.FEEDBACK; + controller.disableContinuousInput(); } break; @@ -70,16 +74,16 @@ public void execute() { if(!limelights.getShooterHasValidTarget()) { aimState = VisionState.SEARCHING; plateauCount = 0; + controller.enableContinuousInput(-180, 180); break; } // turn with PID loop using input as horizontal tx error to target currHorizontalOffset = limelights.getShooterTX(); currError = goalHorizontalOffset - currHorizontalOffset; // currError is positive if we are too far left - - double ff = Math.signum(currError) * VISION_PID_kF; - double feedbackPower = VISION_PID_kP * currError + VISION_PID_kD * (currError - prevError) / (currTime - prevTime) + ff; + double feedbackPower = controller.calculate(currHorizontalOffset); + feedbackPower = MathUtil.clamp(feedbackPower, -1, 1); drive.tankDrive(-feedbackPower, feedbackPower); From 8bb89c8e430a2f48f5df24d68b0e526a74cd5cb0 Mon Sep 17 00:00:00 2001 From: agheyi Date: Mon, 12 Sep 2022 17:19:36 -0700 Subject: [PATCH 2/3] fix problemos from adding calculate --- src/main/java/frc/team3128/commands/CmdAlign.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team3128/commands/CmdAlign.java b/src/main/java/frc/team3128/commands/CmdAlign.java index 48ec659c..9ab64a8b 100644 --- a/src/main/java/frc/team3128/commands/CmdAlign.java +++ b/src/main/java/frc/team3128/commands/CmdAlign.java @@ -65,7 +65,7 @@ public void execute() { currHorizontalOffset = limelights.getShooterTX(); prevError = goalHorizontalOffset - currHorizontalOffset; aimState = VisionState.FEEDBACK; - controller.disableContinuousInput(); + controller.enableContinuousInput(-180, 180); } break; @@ -74,7 +74,7 @@ public void execute() { if(!limelights.getShooterHasValidTarget()) { aimState = VisionState.SEARCHING; plateauCount = 0; - controller.enableContinuousInput(-180, 180); + controller.disableContinuousInput(); break; } @@ -82,7 +82,8 @@ public void execute() { currHorizontalOffset = limelights.getShooterTX(); currError = goalHorizontalOffset - currHorizontalOffset; // currError is positive if we are too far left - double feedbackPower = controller.calculate(currHorizontalOffset); + double ff = Math.signum(currError) * VISION_PID_kF; + double feedbackPower = controller.calculate(currHorizontalOffset) + ff; feedbackPower = MathUtil.clamp(feedbackPower, -1, 1); From 5bf4906bd7d116dbca21a113ec918a52adce840b Mon Sep 17 00:00:00 2001 From: agheyi Date: Mon, 12 Sep 2022 18:04:31 -0700 Subject: [PATCH 3/3] currHorizontalOffset --> currError --- src/main/java/frc/team3128/commands/CmdAlign.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team3128/commands/CmdAlign.java b/src/main/java/frc/team3128/commands/CmdAlign.java index 9ab64a8b..67efe5bc 100644 --- a/src/main/java/frc/team3128/commands/CmdAlign.java +++ b/src/main/java/frc/team3128/commands/CmdAlign.java @@ -83,7 +83,7 @@ public void execute() { currError = goalHorizontalOffset - currHorizontalOffset; // currError is positive if we are too far left double ff = Math.signum(currError) * VISION_PID_kF; - double feedbackPower = controller.calculate(currHorizontalOffset) + ff; + double feedbackPower = controller.calculate(currError) + ff; feedbackPower = MathUtil.clamp(feedbackPower, -1, 1);