From 679568f3b2016693c586c354f29a8f8ee5183d62 Mon Sep 17 00:00:00 2001 From: Abhishrek05 Date: Tue, 14 Mar 2023 15:13:41 -0600 Subject: [PATCH] arm pids --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- .../frc4388/robot/commands/PelvicInflammatoryDisease.java | 2 ++ src/main/java/frc4388/robot/commands/PivotCommand.java | 6 ++++-- src/main/java/frc4388/robot/subsystems/Arm.java | 4 ++-- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 054a85b..1b6fb4c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -146,10 +146,10 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .whileTrue(new RunCommand(() -> new PivotCommand(m_robotArm, 135))); + .onTrue(new PivotCommand(m_robotArm, 135)); new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 135), m_robotArm)); + .onTrue(new PivotCommand(m_robotArm, 210)); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java index 7135326..24e8d9b 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -4,6 +4,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.utility.Gains; @@ -54,6 +55,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { + SmartDashboard.putBoolean("isFinished", Math.abs(getError()) < tolerance); return Math.abs(getError()) < tolerance; } } diff --git a/src/main/java/frc4388/robot/commands/PivotCommand.java b/src/main/java/frc4388/robot/commands/PivotCommand.java index 3f8235c..a84690d 100644 --- a/src/main/java/frc4388/robot/commands/PivotCommand.java +++ b/src/main/java/frc4388/robot/commands/PivotCommand.java @@ -4,6 +4,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.robot.subsystems.Arm; public class PivotCommand extends PelvicInflammatoryDisease { @@ -12,7 +13,7 @@ public class PivotCommand extends PelvicInflammatoryDisease { /** Creates a new ArmCommand. */ public PivotCommand(Arm arm, double target) { - super(0.6, 0, 0, 0, 0); + super(6, 1.5, 0, 0, 0.015); this.arm = arm; this.target = target; addRequirements(arm); @@ -20,11 +21,12 @@ public class PivotCommand extends PelvicInflammatoryDisease { @Override public double getError() { - return (arm.getArmRotation() - target) / 360; + return (target - arm.getArmRotation()) / 360; } @Override public void runWithOutput(double output) { + SmartDashboard.putNumber("pivot output", output); arm.setRotVel(output); } } diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index e20958f..e03897e 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -91,9 +91,9 @@ public class Arm extends SubsystemBase { if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) { m_pivot.set(ControlMode.PercentOutput, 0); } else if (degrees > 90 && vel > 0) { - m_pivot.set(ControlMode.PercentOutput, .2 * vel); + m_pivot.set(ControlMode.PercentOutput, .15 * vel); } else { - m_pivot.set(ControlMode.PercentOutput, .4 * vel); + m_pivot.set(ControlMode.PercentOutput, .3 * vel); } }