From 8dbb6cb1cb0674a848d967c9ae7538e9ed7efa20 Mon Sep 17 00:00:00 2001 From: PineappleGirl5 Date: Tue, 6 Feb 2024 21:11:25 -0600 Subject: [PATCH 01/12] spent way too long rewriting the upbeat --- .../java/frc/robot/subsystems/UpBeat.java | 58 ++++++++++++++----- 1 file changed, 42 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/UpBeat.java b/src/main/java/frc/robot/subsystems/UpBeat.java index efb6352..464247c 100644 --- a/src/main/java/frc/robot/subsystems/UpBeat.java +++ b/src/main/java/frc/robot/subsystems/UpBeat.java @@ -18,6 +18,7 @@ public class UpBeat extends SubsystemBase { private SparkPIDController topPid; private CANSparkMax bottomMotor; private SparkPIDController bottomPid; + private double setPoint; public UpBeat() { @@ -52,40 +53,63 @@ public UpBeat() { bottomPid.setIZone(100); bottomPid.setOutputRange(0.0, 1.0); bottomMotor.burnFlash(); + + setPoint = 0; + } + + private final static class Constants{ + private static final double stop = 0.00; + private static final double reverse = -1000.00; + private static final double shoot = 4000.00; + private static final double amp = 1000.00; + } + + private double getSpeed() { + return bottomMotor.getEncoder().getVelocity(); + } + + private double getError(){ + return setPoint-getSpeed(); + } + + private boolean atSpeed(){ + if(getError()<-100 && getError()>100){ + return true; + } + return false; + } + + private void setSpeed(){ + topPid.setReference((setPoint*1.2), ControlType.kVelocity); + bottomPid.setReference(setPoint, ControlType.kVelocity); + } + + private Command speedCommand(double speed){ + setPoint = speed; + return run (()-> setSpeed()).until(this::atSpeed); } public Command shootNote() { return startEnd( () -> { - topPid.setReference(4800, ControlType.kVelocity); - bottomPid.setReference(4000, ControlType.kVelocity); + speedCommand(Constants.shoot); }, () -> { - topPid.setReference(0, ControlType.kVelocity); - bottomPid.setReference(0, ControlType.kVelocity); + speedCommand(Constants.stop); } ); } public Command reverseShootNote() { - return runOnce(() -> { - topPid.setReference(-1000, ControlType.kVelocity); - bottomPid.setReference(-1200, ControlType.kVelocity); - }); + return speedCommand(Constants.reverse); } public Command pauseUpBeat() { - return runOnce(() -> { - topPid.setReference(0, ControlType.kVelocity); - bottomPid.setReference(0, ControlType.kVelocity); - }); + return speedCommand(Constants.stop); } public Command ampSpeed() { - return runOnce(() -> { - topPid.setReference(1000, ControlType.kVelocity); - bottomPid.setReference(1200, ControlType.kVelocity); - }); + return speedCommand(Constants.amp); } @Override @@ -94,5 +118,7 @@ public void periodic() { Logger.recordOutput("upBeat/ottomOutpt", bottomMotor.getAppliedOutput()); Logger.recordOutput("upBeat/topSpeed", topMotor.getEncoder().getVelocity()); Logger.recordOutput("upBeat/bottomSpeed", bottomMotor.getEncoder().getVelocity()); + Logger.recordOutput("upBeat/setpoint", setPoint); + Logger.recordOutput("upBeat/error", getError()); } } From 0978e40e54b42ce319bcce2ebab9ec1000587459 Mon Sep 17 00:00:00 2001 From: PineappleGirl5 Date: Tue, 6 Feb 2024 21:23:39 -0600 Subject: [PATCH 02/12] updated logger --- src/main/java/frc/robot/subsystems/UpBeat.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/UpBeat.java b/src/main/java/frc/robot/subsystems/UpBeat.java index 464247c..d782ec7 100644 --- a/src/main/java/frc/robot/subsystems/UpBeat.java +++ b/src/main/java/frc/robot/subsystems/UpBeat.java @@ -5,6 +5,7 @@ import java.util.ResourceBundle.Control; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import com.revrobotics.CANSparkMax; @@ -18,7 +19,8 @@ public class UpBeat extends SubsystemBase { private SparkPIDController topPid; private CANSparkMax bottomMotor; private SparkPIDController bottomPid; - private double setPoint; + @AutoLogOutput(key = "upBeat/setPoint") + private double setPoint = 0; public UpBeat() { @@ -53,8 +55,6 @@ public UpBeat() { bottomPid.setIZone(100); bottomPid.setOutputRange(0.0, 1.0); bottomMotor.burnFlash(); - - setPoint = 0; } private final static class Constants{ @@ -64,10 +64,12 @@ private final static class Constants{ private static final double amp = 1000.00; } + @AutoLogOutput(key = "upBeat/speed") private double getSpeed() { return bottomMotor.getEncoder().getVelocity(); } + @AutoLogOutput(key = "upBeat/error") private double getError(){ return setPoint-getSpeed(); } @@ -118,7 +120,5 @@ public void periodic() { Logger.recordOutput("upBeat/ottomOutpt", bottomMotor.getAppliedOutput()); Logger.recordOutput("upBeat/topSpeed", topMotor.getEncoder().getVelocity()); Logger.recordOutput("upBeat/bottomSpeed", bottomMotor.getEncoder().getVelocity()); - Logger.recordOutput("upBeat/setpoint", setPoint); - Logger.recordOutput("upBeat/error", getError()); } } From 47e009ac2bafa8723fb0249882fbe36b9af818d4 Mon Sep 17 00:00:00 2001 From: PineappleGirl5 Date: Thu, 8 Feb 2024 18:41:53 -0600 Subject: [PATCH 03/12] updated upbeat motor speeds for identical belts --- src/main/java/frc/robot/subsystems/UpBeat.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/UpBeat.java b/src/main/java/frc/robot/subsystems/UpBeat.java index d782ec7..6407376 100644 --- a/src/main/java/frc/robot/subsystems/UpBeat.java +++ b/src/main/java/frc/robot/subsystems/UpBeat.java @@ -82,7 +82,7 @@ private boolean atSpeed(){ } private void setSpeed(){ - topPid.setReference((setPoint*1.2), ControlType.kVelocity); + topPid.setReference(setPoint, ControlType.kVelocity); bottomPid.setReference(setPoint, ControlType.kVelocity); } From 4b61f6a18a0c6d5a15bd92569baaf2afdc66b40c Mon Sep 17 00:00:00 2001 From: PineappleGirl5 Date: Thu, 8 Feb 2024 19:24:16 -0600 Subject: [PATCH 04/12] added test thingies --- src/main/java/frc/robot/RobotContainer.java | 22 +++++++++++++++++++ .../java/frc/robot/subsystems/UpBeat.java | 5 +++++ 2 files changed, 27 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eb28aa0..30e1855 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ import java.io.IOException; import java.sql.DriverPropertyInfo; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import com.pathplanner.lib.auto.AutoBuilder; @@ -20,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.OIConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Bass; @@ -63,6 +65,26 @@ public RobotContainer() { } private void configureBindings() { + + // Create the SysId routine +var sysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config( + null, null, null, // Use default config + (state) -> Logger.recordOutput("SysIdTestState", state.toString()) + ), + new SysIdRoutine.Mechanism( + (voltage) -> m_shooter.runVolts(voltage.baseUnitMagnitude()), + null, // No log consumer, since data is recorded by AdvantageKit + m_shooter + ) +); + +one.button(5).onTrue(sysIdRoutine.quasistatic(SysIdRoutine.Direction.kForward)); +one.button(10).onTrue(sysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse)); +one.button(6).onTrue(sysIdRoutine.dynamic(SysIdRoutine.Direction.kForward)); +one.button(9).onTrue(sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse)); + + one.trigger().toggleOnTrue(new StartEndCommand(m_robotDrive::setX, () -> { }, m_robotDrive)); two.trigger().onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive)); diff --git a/src/main/java/frc/robot/subsystems/UpBeat.java b/src/main/java/frc/robot/subsystems/UpBeat.java index 6407376..f00c77c 100644 --- a/src/main/java/frc/robot/subsystems/UpBeat.java +++ b/src/main/java/frc/robot/subsystems/UpBeat.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import java.util.ResourceBundle.Control; @@ -121,4 +122,8 @@ public void periodic() { Logger.recordOutput("upBeat/topSpeed", topMotor.getEncoder().getVelocity()); Logger.recordOutput("upBeat/bottomSpeed", bottomMotor.getEncoder().getVelocity()); } + + public void runVolts(double baseUnitMagnitude) { + topMotor.setVoltage(baseUnitMagnitude); + } } From 9cbac4adb5b6a5628fe4e9200f6f0d90d77e3d6b Mon Sep 17 00:00:00 2001 From: PineappleGirl5 Date: Thu, 8 Feb 2024 20:37:23 -0600 Subject: [PATCH 05/12] updated pid --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- src/main/java/frc/robot/subsystems/UpBeat.java | 9 +++++---- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 30e1855..3b9245d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,10 +79,10 @@ private void configureBindings() { ) ); -one.button(5).onTrue(sysIdRoutine.quasistatic(SysIdRoutine.Direction.kForward)); -one.button(10).onTrue(sysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse)); -one.button(6).onTrue(sysIdRoutine.dynamic(SysIdRoutine.Direction.kForward)); -one.button(9).onTrue(sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse)); +two.button(5).onTrue(sysIdRoutine.quasistatic(SysIdRoutine.Direction.kForward)); +two.button(10).onTrue(sysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse)); +two.button(6).onTrue(sysIdRoutine.dynamic(SysIdRoutine.Direction.kForward)); +two.button(9).onTrue(sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse)); one.trigger().toggleOnTrue(new StartEndCommand(m_robotDrive::setX, () -> { diff --git a/src/main/java/frc/robot/subsystems/UpBeat.java b/src/main/java/frc/robot/subsystems/UpBeat.java index ea30ec7..f6aba54 100644 --- a/src/main/java/frc/robot/subsystems/UpBeat.java +++ b/src/main/java/frc/robot/subsystems/UpBeat.java @@ -41,16 +41,16 @@ public UpBeat() { bottomMotor.setIdleMode(IdleMode.kCoast); bottomPid = bottomMotor.getPIDController(); - topPid.setP(0.0003); - topPid.setI(0.0000001); + topPid.setP(3.0156E-07); + topPid.setI(0.0); topPid.setD(0.0); topPid.setFF(0.0001875); topPid.setIZone(100); topPid.setOutputRange(0.0, 1.0); topMotor.burnFlash(); - bottomPid.setP(0.0003); - bottomPid.setI(0.0000001); + bottomPid.setP(3.0156E-07); + bottomPid.setI(0.0); bottomPid.setD(0.0); bottomPid.setFF(0.0001875); bottomPid.setIZone(100); @@ -120,5 +120,6 @@ public void periodic() { public void runVolts(double baseUnitMagnitude) { topMotor.setVoltage(baseUnitMagnitude); + bottomMotor.setVoltage(baseUnitMagnitude); } } From 1966b08547f3dea56543ae8e5fac57a95734a000 Mon Sep 17 00:00:00 2001 From: PineappleGirl5 Date: Thu, 8 Feb 2024 20:38:16 -0600 Subject: [PATCH 06/12] removed test code --- src/main/java/frc/robot/RobotContainer.java | 20 ------------------- .../java/frc/robot/subsystems/UpBeat.java | 5 ----- 2 files changed, 25 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3b9245d..f7e486b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -65,26 +65,6 @@ public RobotContainer() { } private void configureBindings() { - - // Create the SysId routine -var sysIdRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - null, null, null, // Use default config - (state) -> Logger.recordOutput("SysIdTestState", state.toString()) - ), - new SysIdRoutine.Mechanism( - (voltage) -> m_shooter.runVolts(voltage.baseUnitMagnitude()), - null, // No log consumer, since data is recorded by AdvantageKit - m_shooter - ) -); - -two.button(5).onTrue(sysIdRoutine.quasistatic(SysIdRoutine.Direction.kForward)); -two.button(10).onTrue(sysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse)); -two.button(6).onTrue(sysIdRoutine.dynamic(SysIdRoutine.Direction.kForward)); -two.button(9).onTrue(sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse)); - - one.trigger().toggleOnTrue(new StartEndCommand(m_robotDrive::setX, () -> { }, m_robotDrive)); two.trigger().onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive)); diff --git a/src/main/java/frc/robot/subsystems/UpBeat.java b/src/main/java/frc/robot/subsystems/UpBeat.java index f6aba54..03355b2 100644 --- a/src/main/java/frc/robot/subsystems/UpBeat.java +++ b/src/main/java/frc/robot/subsystems/UpBeat.java @@ -117,9 +117,4 @@ public void periodic() { Logger.recordOutput("upBeat/topSpeed", topMotor.getEncoder().getVelocity()); Logger.recordOutput("upBeat/bottomSpeed", bottomMotor.getEncoder().getVelocity()); } - - public void runVolts(double baseUnitMagnitude) { - topMotor.setVoltage(baseUnitMagnitude); - bottomMotor.setVoltage(baseUnitMagnitude); - } } From 036ebfd6157814b7477d241ec3b58cc4da5d5358 Mon Sep 17 00:00:00 2001 From: PineappleGirl5 Date: Thu, 8 Feb 2024 20:47:24 -0600 Subject: [PATCH 07/12] added feedforward --- src/main/java/frc/robot/subsystems/UpBeat.java | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/UpBeat.java b/src/main/java/frc/robot/subsystems/UpBeat.java index 03355b2..6108d25 100644 --- a/src/main/java/frc/robot/subsystems/UpBeat.java +++ b/src/main/java/frc/robot/subsystems/UpBeat.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -7,6 +8,7 @@ import java.util.ResourceBundle.Control; import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; import com.revrobotics.CANSparkMax; @@ -22,6 +24,7 @@ public class UpBeat extends SubsystemBase { private SparkPIDController bottomPid; @AutoLogOutput(key = "upBeat/setPoint") private double setPoint = 0; + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.25044, 0.00039235); public UpBeat() { @@ -109,11 +112,13 @@ public Command ampSpeed() { @Override public void periodic() { - topPid.setReference(setPoint, ControlType.kVelocity); - bottomPid.setReference(setPoint, ControlType.kVelocity); + double feedforwardVolts = feedforward.calculate(getSpeed(), setPoint, LoggedRobot.defaultPeriodSecs); + topPid.setReference(setPoint, ControlType.kVelocity, 0, feedforwardVolts); + bottomPid.setReference(setPoint, ControlType.kVelocity, 0, feedforwardVolts); - Logger.recordOutput("upBeat/topOutPut", topMotor.getAppliedOutput()); - Logger.recordOutput("upBeat/ottomOutpt", bottomMotor.getAppliedOutput()); + Logger.recordOutput("upBeat/feedForward", feedforwardVolts); + Logger.recordOutput("upBeat/topOutput", topMotor.getAppliedOutput()); + Logger.recordOutput("upBeat/bottomOutput", bottomMotor.getAppliedOutput()); Logger.recordOutput("upBeat/topSpeed", topMotor.getEncoder().getVelocity()); Logger.recordOutput("upBeat/bottomSpeed", bottomMotor.getEncoder().getVelocity()); } From 450e378e36ff7133f8a50d9f6800d0e0a25e8ce2 Mon Sep 17 00:00:00 2001 From: PineappleGirl5 Date: Thu, 8 Feb 2024 20:54:22 -0600 Subject: [PATCH 08/12] updated feedforward --- src/main/java/frc/robot/subsystems/UpBeat.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/UpBeat.java b/src/main/java/frc/robot/subsystems/UpBeat.java index 6108d25..f4059f8 100644 --- a/src/main/java/frc/robot/subsystems/UpBeat.java +++ b/src/main/java/frc/robot/subsystems/UpBeat.java @@ -24,7 +24,7 @@ public class UpBeat extends SubsystemBase { private SparkPIDController bottomPid; @AutoLogOutput(key = "upBeat/setPoint") private double setPoint = 0; - private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.25044, 0.00039235); + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.25044, 0.0022092, 0.00039235); public UpBeat() { From b9ecbf0ccd854c04932c6cd8d381cfcd2f651fff Mon Sep 17 00:00:00 2001 From: PineappleGirl5 Date: Thu, 8 Feb 2024 21:08:18 -0600 Subject: [PATCH 09/12] updated feedforwards and PID --- src/main/java/frc/robot/subsystems/UpBeat.java | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/UpBeat.java b/src/main/java/frc/robot/subsystems/UpBeat.java index f4059f8..bf963d5 100644 --- a/src/main/java/frc/robot/subsystems/UpBeat.java +++ b/src/main/java/frc/robot/subsystems/UpBeat.java @@ -24,7 +24,7 @@ public class UpBeat extends SubsystemBase { private SparkPIDController bottomPid; @AutoLogOutput(key = "upBeat/setPoint") private double setPoint = 0; - private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.25044, 0.0022092, 0.00039235); + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.011072, 0.011947, 0.0017238); public UpBeat() { @@ -44,7 +44,7 @@ public UpBeat() { bottomMotor.setIdleMode(IdleMode.kCoast); bottomPid = bottomMotor.getPIDController(); - topPid.setP(3.0156E-07); + topPid.setP(0.8598E-07); topPid.setI(0.0); topPid.setD(0.0); topPid.setFF(0.0001875); @@ -52,7 +52,7 @@ public UpBeat() { topPid.setOutputRange(0.0, 1.0); topMotor.burnFlash(); - bottomPid.setP(3.0156E-07); + bottomPid.setP(0.8598E-07); bottomPid.setI(0.0); bottomPid.setD(0.0); bottomPid.setFF(0.0001875); @@ -89,13 +89,8 @@ private Command speedCommand(double speed){ public Command shootNote() { return startEnd( - () -> { - speedCommand(Constants.shoot); - }, - () -> { - speedCommand(Constants.stop); - } - ); + () -> setPoint = Constants.shoot, + () -> setPoint = Constants.stop); } public Command reverseShootNote() { From 5050a1b433b17bf737ce0fa164c700d5854e2863 Mon Sep 17 00:00:00 2001 From: PineappleGirl5 Date: Thu, 8 Feb 2024 21:32:38 -0600 Subject: [PATCH 10/12] changed the same 5 numbers a lot of times --- src/main/java/frc/robot/subsystems/UpBeat.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/UpBeat.java b/src/main/java/frc/robot/subsystems/UpBeat.java index bf963d5..0f5b07f 100644 --- a/src/main/java/frc/robot/subsystems/UpBeat.java +++ b/src/main/java/frc/robot/subsystems/UpBeat.java @@ -24,7 +24,7 @@ public class UpBeat extends SubsystemBase { private SparkPIDController bottomPid; @AutoLogOutput(key = "upBeat/setPoint") private double setPoint = 0; - private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.011072, 0.011947, 0.0017238); + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.0094122, 0.00226330450273642352463680255237, 0.0020114); public UpBeat() { @@ -44,18 +44,18 @@ public UpBeat() { bottomMotor.setIdleMode(IdleMode.kCoast); bottomPid = bottomMotor.getPIDController(); - topPid.setP(0.8598E-07); + topPid.setP(1.4395E-06); topPid.setI(0.0); topPid.setD(0.0); - topPid.setFF(0.0001875); + topPid.setFF(0.0); topPid.setIZone(100); topPid.setOutputRange(0.0, 1.0); topMotor.burnFlash(); - bottomPid.setP(0.8598E-07); + bottomPid.setP(1.4395E-06); bottomPid.setI(0.0); bottomPid.setD(0.0); - bottomPid.setFF(0.0001875); + bottomPid.setFF(0.0); bottomPid.setIZone(100); bottomPid.setOutputRange(0.0, 1.0); bottomMotor.burnFlash(); @@ -107,7 +107,7 @@ public Command ampSpeed() { @Override public void periodic() { - double feedforwardVolts = feedforward.calculate(getSpeed(), setPoint, LoggedRobot.defaultPeriodSecs); + double feedforwardVolts = feedforward.calculate(setPoint); topPid.setReference(setPoint, ControlType.kVelocity, 0, feedforwardVolts); bottomPid.setReference(setPoint, ControlType.kVelocity, 0, feedforwardVolts); From 0e66d1b3053f96d038d8fc1544aed3efcd5ab9e6 Mon Sep 17 00:00:00 2001 From: PineappleGirl5 Date: Fri, 9 Feb 2024 18:54:52 -0600 Subject: [PATCH 11/12] updated feedforwads/back --- src/main/java/frc/robot/subsystems/UpBeat.java | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/UpBeat.java b/src/main/java/frc/robot/subsystems/UpBeat.java index 0f5b07f..cf0aba0 100644 --- a/src/main/java/frc/robot/subsystems/UpBeat.java +++ b/src/main/java/frc/robot/subsystems/UpBeat.java @@ -3,12 +3,8 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - -import java.util.ResourceBundle.Control; import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; import com.revrobotics.CANSparkMax; @@ -24,7 +20,7 @@ public class UpBeat extends SubsystemBase { private SparkPIDController bottomPid; @AutoLogOutput(key = "upBeat/setPoint") private double setPoint = 0; - private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.0094122, 0.00226330450273642352463680255237, 0.0020114); + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.25011, 0.13256, 0.023543); public UpBeat() { @@ -44,7 +40,7 @@ public UpBeat() { bottomMotor.setIdleMode(IdleMode.kCoast); bottomPid = bottomMotor.getPIDController(); - topPid.setP(1.4395E-06); + topPid.setP(1.8097E-05); topPid.setI(0.0); topPid.setD(0.0); topPid.setFF(0.0); @@ -52,7 +48,7 @@ public UpBeat() { topPid.setOutputRange(0.0, 1.0); topMotor.burnFlash(); - bottomPid.setP(1.4395E-06); + bottomPid.setP(1.8097E-05); bottomPid.setI(0.0); bottomPid.setD(0.0); bottomPid.setFF(0.0); From 60354f1d6402924b7b0eb786f3e7ab517259c73d Mon Sep 17 00:00:00 2001 From: PineappleGirl5 Date: Fri, 9 Feb 2024 19:49:48 -0600 Subject: [PATCH 12/12] aaaaaa --- src/main/java/frc/robot/subsystems/UpBeat.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/UpBeat.java b/src/main/java/frc/robot/subsystems/UpBeat.java index cf0aba0..d15ef7e 100644 --- a/src/main/java/frc/robot/subsystems/UpBeat.java +++ b/src/main/java/frc/robot/subsystems/UpBeat.java @@ -20,7 +20,7 @@ public class UpBeat extends SubsystemBase { private SparkPIDController bottomPid; @AutoLogOutput(key = "upBeat/setPoint") private double setPoint = 0; - private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.25011, 0.13256, 0.023543); + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.25001, 0.1326, 0.02354); public UpBeat() { @@ -40,7 +40,7 @@ public UpBeat() { bottomMotor.setIdleMode(IdleMode.kCoast); bottomPid = bottomMotor.getPIDController(); - topPid.setP(1.8097E-05); + topPid.setP(1.2557E-08); topPid.setI(0.0); topPid.setD(0.0); topPid.setFF(0.0); @@ -48,7 +48,7 @@ public UpBeat() { topPid.setOutputRange(0.0, 1.0); topMotor.burnFlash(); - bottomPid.setP(1.8097E-05); + bottomPid.setP(1.2557E-08); bottomPid.setI(0.0); bottomPid.setD(0.0); bottomPid.setFF(0.0);