From 92d640ab79aa1989a2dba6531dace96a2452ca35 Mon Sep 17 00:00:00 2001 From: Computer 2 <48131223+TheGamer1002@users.noreply.github.com> Date: Fri, 28 Apr 2023 09:36:16 -0400 Subject: [PATCH] armPositing;PIDRenaming;RemovedUsedStuff --- src/main/java/frc/robot/RobotContainer.java | 13 ++-- .../java/frc/robot/commands/AutoCommand.java | 31 +++++---- .../frc/robot/commands/BalanceCommand.java | 7 +- .../java/frc/robot/commands/TrackPiece.java | 6 +- .../java/frc/robot/commands/TrackPole.java | 2 +- src/main/java/frc/robot/subsystems/Arm.java | 55 ++++++++------- .../robot/subsystems/AutoArmPostioning.java | 69 +++++++++++++++++++ .../subsystems/{PID.java => AutoBalence.java} | 4 +- .../{Vision.java => AutoPiecePickUp.java} | 4 +- 9 files changed, 132 insertions(+), 59 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/AutoArmPostioning.java rename src/main/java/frc/robot/subsystems/{PID.java => AutoBalence.java} (96%) rename src/main/java/frc/robot/subsystems/{Vision.java => AutoPiecePickUp.java} (96%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 99372ec..d8d2ae8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -44,18 +44,17 @@ public class RobotContainer { private final Arm m_Arm = new Arm(); private final Claw m_Claw = new Claw(); private final Tank m_Tank = new Tank(); - private final PID m_PID = new PID(); + private final AutoBalence m_Balance = new AutoBalence(); private final PDP m_PDP = new PDP(); - private final Vision m_Vision = new Vision(m_Tank, m_Claw); + private final AutoPiecePickUp m_Vision = new AutoPiecePickUp(m_Tank, m_Claw); private final PoleTracking m_PoleTracking = new PoleTracking(m_Tank); - // INSTANTIATES ALL COMMANDS private final MoveClawCommand m_MoveClawCommand = new MoveClawCommand(m_Claw); private final MoveArmYCommand m_MoveArmYCommand = new MoveArmYCommand(m_Arm); private final DriveCommand m_DriveCommand = new DriveCommand(m_Tank); - private final BalanceCommand m_BalanceCommand = new BalanceCommand(m_PID, m_Tank); - private final AutoCommand m_AutoCommand = new AutoCommand(m_PID, m_Tank, m_Arm, m_Claw); + private final BalanceCommand m_BalanceCommand = new BalanceCommand(m_Balance, m_Tank); + private final AutoCommand m_AutoCommand = new AutoCommand(m_Balance, m_Tank, m_Arm, m_Claw); private final InvertDriveCommand m_InvertDriveCommand = new InvertDriveCommand(m_Tank, this); private final PlaceConeSecondLevelCommand m_PlaceConeSecondLevelCommand = new PlaceConeSecondLevelCommand(m_Tank, m_Arm, m_Claw); @@ -95,7 +94,7 @@ public RobotContainer() { subsystemsTab.add(m_PlaceCubeSecondLevelCommand); subsystemsTab.add(m_Vision); - subsystemsTab.add(m_PID); + subsystemsTab.add(m_Balance); subsystemsTab.add(m_PlaceConeSecondLevelCommand); subsystemsTab.add(m_Tank); subsystemsTab.add(m_Claw); @@ -247,7 +246,7 @@ public static double getControllerRightStickX() { } return axis; } - + public static double getControllerRightStickY() { double axis = m_armController2.getRawAxis(5); if (Math.abs(axis) < 0.4) { diff --git a/src/main/java/frc/robot/commands/AutoCommand.java b/src/main/java/frc/robot/commands/AutoCommand.java index 22a5e39..6811e70 100644 --- a/src/main/java/frc/robot/commands/AutoCommand.java +++ b/src/main/java/frc/robot/commands/AutoCommand.java @@ -8,33 +8,34 @@ import frc.robot.subsystems.*; /** - * This command will run the autonomous routine. In it, you can add commands to be run in sequence. + * This command will run the autonomous routine. In it, you can add commands to + * be run in sequence. */ public class AutoCommand extends SequentialCommandGroup { - public AutoCommand(PID m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) { + public AutoCommand(AutoBalence m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) { - //pick one - //sideAutoWCone(m_pid,m_driveBase,m_arm,m_claw); - //sideAutoWCube(m_pid,m_driveBase,m_arm,m_claw); - //middleAutoWCone(m_pid,m_driveBase,m_arm,m_claw); - //middleAutoWCube(m_pid,m_driveBase,m_arm,m_claw); + // pick one + // sideAutoWCone(m_pid,m_driveBase,m_arm,m_claw); + // sideAutoWCube(m_pid,m_driveBase,m_arm,m_claw); + // middleAutoWCone(m_pid,m_driveBase,m_arm,m_claw); + // middleAutoWCube(m_pid,m_driveBase,m_arm,m_claw); } + // TODO document this - private void sideAutoWCone(PID m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) - { + private void sideAutoWCone(AutoBalence m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) { addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm, m_claw)); } - private void sideAutoWCube(PID m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) - { + + private void sideAutoWCube(AutoBalence m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) { addCommands(new PlaceCubeSecondLevelCommand(m_driveBase, m_arm, m_claw)); } - private void middleAutoWCone(PID m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) - { + + private void middleAutoWCone(AutoBalence m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) { addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm, m_claw), new BalanceCommand(m_pid, m_driveBase)); } - private void middleAutoWCube(PID m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) - { + + private void middleAutoWCube(AutoBalence m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) { addCommands(new PlaceCubeSecondLevelCommand(m_driveBase, m_arm, m_claw), new BalanceCommand(m_pid, m_driveBase)); } } diff --git a/src/main/java/frc/robot/commands/BalanceCommand.java b/src/main/java/frc/robot/commands/BalanceCommand.java index 7a3f962..0bcd8be 100644 --- a/src/main/java/frc/robot/commands/BalanceCommand.java +++ b/src/main/java/frc/robot/commands/BalanceCommand.java @@ -8,7 +8,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants; -import frc.robot.subsystems.PID; +import frc.robot.subsystems.AutoBalence; +import frc.robot.subsystems.AutoPiecePickUp; import frc.robot.subsystems.Tank; /** @@ -16,14 +17,14 @@ */ public class BalanceCommand extends CommandBase { @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) - private final PID pidLoop; + private final AutoBalence pidLoop; private final Tank driveBase; private double initPitch, maxPitch; private boolean onRamp; /** Create a new {@link BalanceCommand}. */ - public BalanceCommand(PID m_pid, Tank m_driveBase) { + public BalanceCommand(AutoBalence m_pid, Tank m_driveBase) { pidLoop = m_pid; driveBase = m_driveBase; addRequirements(pidLoop); diff --git a/src/main/java/frc/robot/commands/TrackPiece.java b/src/main/java/frc/robot/commands/TrackPiece.java index b1b80ba..413147f 100644 --- a/src/main/java/frc/robot/commands/TrackPiece.java +++ b/src/main/java/frc/robot/commands/TrackPiece.java @@ -10,14 +10,14 @@ import frc.robot.RobotContainer; import frc.robot.subsystems.Claw; import frc.robot.subsystems.Tank; -import frc.robot.subsystems.Vision; +import frc.robot.subsystems.AutoPiecePickUp; public class TrackPiece extends CommandBase { /** Creates a new TrackPiece. */ - private Vision m_Vision; + private AutoPiecePickUp m_Vision; private Tank m_Tank; private Claw m_Claw; - public TrackPiece(Vision vision, Tank tank, Claw claw) { + public TrackPiece(AutoPiecePickUp vision, Tank tank, Claw claw) { // Use addRequirements() here to declare subsystem dependencies. m_Vision = vision; m_Tank = tank; diff --git a/src/main/java/frc/robot/commands/TrackPole.java b/src/main/java/frc/robot/commands/TrackPole.java index 1e34e31..c38f662 100644 --- a/src/main/java/frc/robot/commands/TrackPole.java +++ b/src/main/java/frc/robot/commands/TrackPole.java @@ -11,7 +11,7 @@ import frc.robot.subsystems.Claw; import frc.robot.subsystems.PoleTracking; import frc.robot.subsystems.Tank; -import frc.robot.subsystems.Vision; +import frc.robot.subsystems.AutoPiecePickUp; public class TrackPole extends CommandBase { /** Creates a new TrackPiece. */ diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index cd21f4d..37992d1 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -27,8 +27,8 @@ public class Arm extends SubsystemBase { public CANSparkMax armMotor; public DigitalInput allTheWayDownRear = new DigitalInput(1); - public double desiredArmAngle; - public double currentArmAngle; + //public double desiredArmAngle; + //public double currentArmAngle; public double gearRatio = 87; @@ -38,7 +38,7 @@ public class Arm extends SubsystemBase { public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, kSetpoint; - public SparkMaxPIDController armPID; + //public SparkMaxPIDController armPID; public AbsoluteEncoder encoder; @@ -47,15 +47,15 @@ public class Arm extends SubsystemBase { public Arm() { - kP = 0.0001; - kI = 0; - kD = 0; - kIz = 0; - kFF = 0; - kMaxOutput = 1; - kMinOutput = -1; + // kP = 0.0001; + // kI = 0; + // kD = 0; + // kIz = 0; + // kFF = 0; + // kMaxOutput = 1; + // kMinOutput = -1; - kSetpoint = 0; + // kSetpoint = 0; armMotor = new CANSparkMax(7, MotorType.kBrushless); armMotor.setIdleMode(IdleMode.kBrake); @@ -64,14 +64,14 @@ public Arm() { // set addChild("Arm Motor", armMotor); - armPID = armMotor.getPIDController(); + // armPID = armMotor.getPIDController(); - armPID.setP(kP); - armPID.setI(kI); - armPID.setD(kD); - armPID.setIZone(kIz); - armPID.setFF(kFF); - armPID.setOutputRange(kMinOutput, kMaxOutput); + // armPID.setP(kP); + // armPID.setI(kI); + // armPID.setD(kD); + // armPID.setIZone(kIz); + // armPID.setFF(kFF); + // armPID.setOutputRange(kMinOutput, kMaxOutput); // set the spark max to alternate encoder mode @@ -86,7 +86,7 @@ public Arm() { lastEncoderValue = armMotor.getEncoder().getPosition(); // configure the PID loop to use the alternate encoder - armPID.setFeedbackDevice(encoder); + // armPID.setFeedbackDevice(encoder); // add simulation data if (RobotBase.isSimulation()) { @@ -126,13 +126,16 @@ public void moveArm(double value) { public void simulationPeriodic() { // This method will be called once per scheduler run during simulation } - - public void setArmAngle(double angle) { - desiredArmAngle = angle; - armPID.setReference(angle, CANSparkMax.ControlType.kPosition); + public double getAngle(){ + return armMotor.getEncoder().getPosition() + rotations; } - public void putArmAtFront() { - setArmAngle(armToFrontAngle); - } + // public void setArmAngle(double angle) { + // //desiredArmAngle = angle; + // //armPID.setReference(angle, CANSparkMax.ControlType.kPosition); + // } + + // public void putArmAtFront() { + // setArmAngle(armToFrontAngle); + // } } diff --git a/src/main/java/frc/robot/subsystems/AutoArmPostioning.java b/src/main/java/frc/robot/subsystems/AutoArmPostioning.java new file mode 100644 index 0000000..ae8745e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AutoArmPostioning.java @@ -0,0 +1,69 @@ +package frc.robot.subsystems; + +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.PIDSubsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class AutoArmPostioning extends PIDSubsystem { + private static double kP = 0.18; + private static double kI = 0; + private static double kD = 0; + public PIDController m_controller; + public double target; + public Arm m_arm; + + public AutoArmPostioning(Arm arm) { + super( + // PIDController + new PIDController(kP, kI, kD)); + m_arm = arm; + m_controller.setTolerance(0.1); + m_controller.setSetpoint(0); + } + + public void setTarget(double target) { + this.target = target; + m_controller.setSetpoint(target); + } + + @Override + protected void useOutput(double output, double setpoint) { + m_arm.moveArm(output); + } + + @Override + protected double getMeasurement() { + return m_arm.getAngle(); + } + + public boolean atSetpoint() { + return m_controller.atSetpoint(); + } + + @Override + public void setSetpoint(double setpoint) { + m_controller.setSetpoint(setpoint); + } + + public void setPID(double p, double i, double d) { + m_controller.setPID(p, i, d); + } + + public void setTolerance(double tolerance) { + m_controller.setTolerance(tolerance); + } + + @Override + public void periodic() { + super.periodic(); + SmartDashboard.putNumber("caluclated power to arm form PID", + m_controller.calculate(getMeasurement(), m_controller.getSetpoint())); + } + +} diff --git a/src/main/java/frc/robot/subsystems/PID.java b/src/main/java/frc/robot/subsystems/AutoBalence.java similarity index 96% rename from src/main/java/frc/robot/subsystems/PID.java rename to src/main/java/frc/robot/subsystems/AutoBalence.java index 086327a..26df6d5 100644 --- a/src/main/java/frc/robot/subsystems/PID.java +++ b/src/main/java/frc/robot/subsystems/AutoBalence.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class PID extends SubsystemBase { +public class AutoBalence extends SubsystemBase { private final double kP = 0.18; private final double kI = 0.025; @@ -25,7 +25,7 @@ public class PID extends SubsystemBase { private float initPitch; - public PID() { + public AutoBalence() { toleranceDeg = 0.5; iLimit = 2.0; diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/AutoPiecePickUp.java similarity index 96% rename from src/main/java/frc/robot/subsystems/Vision.java rename to src/main/java/frc/robot/subsystems/AutoPiecePickUp.java index 5540f37..48c8c99 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/AutoPiecePickUp.java @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.PIDSubsystem; import frc.robot.RobotContainer; -public class Vision extends PIDSubsystem { +public class AutoPiecePickUp extends PIDSubsystem { /** Creates a new Vision. */ private static double kP = 0.1; private static double kI = 0; @@ -21,7 +21,7 @@ public class Vision extends PIDSubsystem { public Tank m_tank; public Claw m_claw; - public Vision(Tank tank, Claw claw) { + public AutoPiecePickUp(Tank tank, Claw claw) { super( // The PIDController used by the subsystem new PIDController(kP, kI, kD));