From 2eaa1efc689169ccae6855ad3def6659cbfcb323 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 15 Dec 2023 10:37:06 -0800 Subject: [PATCH] feat: what's sleep?????? --- .../team1540/bunnybotTank2023/Constants.java | 15 +++-- .../bunnybotTank2023/RobotContainer.java | 34 ++++++---- .../commands/indexer/IndexerCommand.java | 1 + .../commands/indexer/IndexerIdleCommand.java | 19 ++++++ .../commands/shooter/Shooter.java | 1 - .../commands/turret/Turret.java | 29 +++++++++ .../commands/turret/TurretManualCommand.java | 27 ++++++++ .../turret/TurretSetpointCommand.java | 25 ++++++++ .../turret/TurretZeroSequenceCommand.java | 20 ++++++ .../bunnybotTank2023/io/turret/TurretIO.java | 10 ++- .../io/turret/TurretIOReal.java | 63 +++++++++++++------ 11 files changed, 205 insertions(+), 39 deletions(-) create mode 100644 src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerIdleCommand.java create mode 100644 src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretManualCommand.java create mode 100644 src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretSetpointCommand.java create mode 100644 src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretZeroSequenceCommand.java diff --git a/src/main/java/org/team1540/bunnybotTank2023/Constants.java b/src/main/java/org/team1540/bunnybotTank2023/Constants.java index 7932a60..ccc5eae 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/Constants.java +++ b/src/main/java/org/team1540/bunnybotTank2023/Constants.java @@ -1,10 +1,11 @@ package org.team1540.bunnybotTank2023; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; public final class Constants { // Allow PID constants to be tuned from a dashboard input or not - public static final boolean TUNING_MODE = true; + public static final boolean TUNING_MODE = false; // Simulation mode, irrelevant for code running on physical robot public static final SimulationMode simulationMode = SimulationMode.SIM; @@ -76,10 +77,16 @@ public static class ShooterConstants { } public static class TurretConstants{ - public static final int MOTOR_ID = 9; //TODO: MAKE SURE THIS CAN ID WORKS!!! - public static final double kP = 3.2; + public static final int MOTOR_ID = 10; //TODO: MAKE SURE THIS CAN ID WORKS!!! + public static final double kP = 3.636; public static final double kI = 0; - public static final double kD = 0.1; + public static final double kD = 0; public static final double gearRatio = 166.66666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666667; + + public static final double CRUISE_VELOCITY_RPS = 100; + public static final double MAX_ACCEL_RPS2 = 600; + + public static final Rotation2d FORWARD_LIMIT_POSITION = Rotation2d.fromDegrees(66.064); + public static final Rotation2d REVERSE_LIMIT_POSITION = Rotation2d.fromDegrees(150); } } diff --git a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java index 6e881d2..57209fb 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java +++ b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java @@ -5,28 +5,31 @@ package org.team1540.bunnybotTank2023; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.team1540.bunnybotTank2023.commands.auto.AutoShoot5RamTotes; -import org.team1540.bunnybotTank2023.commands.drivetrain.ArcadeDriveCommand; import org.team1540.bunnybotTank2023.commands.drivetrain.Drivetrain; import org.team1540.bunnybotTank2023.commands.indexer.Indexer; import org.team1540.bunnybotTank2023.commands.indexer.IndexerCommand; +import org.team1540.bunnybotTank2023.commands.indexer.IndexerIdleCommand; import org.team1540.bunnybotTank2023.commands.intake.Intake; import org.team1540.bunnybotTank2023.commands.intake.IntakeCommand; import org.team1540.bunnybotTank2023.commands.shooter.Shooter; +import org.team1540.bunnybotTank2023.commands.turret.Turret; +import org.team1540.bunnybotTank2023.commands.turret.TurretManualCommand; +import org.team1540.bunnybotTank2023.commands.turret.TurretSetpointCommand; +import org.team1540.bunnybotTank2023.commands.turret.TurretZeroSequenceCommand; import org.team1540.bunnybotTank2023.io.drivetrain.DrivetrainIOSim; import org.team1540.bunnybotTank2023.io.drivetrain.DrivetrainIOReal; import org.team1540.bunnybotTank2023.io.indexer.IndexerIOReal; import org.team1540.bunnybotTank2023.io.intake.IntakeIOReal; import org.team1540.bunnybotTank2023.io.shooter.ShooterIOReal; import org.team1540.bunnybotTank2023.io.shooter.ShooterIOSim; - -import static org.team1540.bunnybotTank2023.Constants.*; +import org.team1540.bunnybotTank2023.io.turret.TurretIOReal; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -40,12 +43,13 @@ public class RobotContainer { Shooter shooter; Indexer indexer; Intake intake; + Turret turret; // Controllers CommandXboxController driver = new CommandXboxController(0); CommandXboxController copilot = new CommandXboxController(1); - LoggedDashboardNumber shooterSpeed = new LoggedDashboardNumber("Shooter/setpoint"); + LoggedDashboardNumber turretSetpoint = new LoggedDashboardNumber("Turret/setpoint"); public RobotContainer() { if (Robot.isReal()) { @@ -54,12 +58,14 @@ public RobotContainer() { shooter = new Shooter(new ShooterIOReal()); indexer = new Indexer(new IndexerIOReal()); intake = new Intake(new IntakeIOReal()); + turret = new Turret(new TurretIOReal()); } else { // Initialize subsystems with simulation IO drivetrain = new Drivetrain(new DrivetrainIOSim()); shooter = new Shooter(new ShooterIOSim()); indexer = null; intake = null; + turret = null; } setDefaultCommands(); configureButtonBindings(); @@ -69,7 +75,7 @@ public RobotContainer() { /** Use this method to define your trigger->command mappings. */ private void configureButtonBindings() { copilot.a() - .whileTrue(new InstantCommand(() -> shooter.setVelocity(shooterSpeed.get())) + .whileTrue(new InstantCommand(() -> shooter.setVelocity(turretSetpoint.get())) .andThen(new IndexerCommand(indexer))) .onFalse(new InstantCommand(() -> { shooter.stop(); @@ -78,17 +84,21 @@ private void configureButtonBindings() { // copilot.b().whileTrue(new InstantCommand(() -> intake.setFold(false)).andThen(new IndexerCommand(indexer))).whileFalse(new InstantCommand(() -> intake.setFold(true))); copilot.b().whileTrue(Commands.parallel(new IntakeCommand(intake), new IndexerCommand(indexer))); + copilot.x().whileTrue(new TurretSetpointCommand(turret, Rotation2d.fromDegrees(turretSetpoint.get()))); + copilot.y().whileTrue(new TurretZeroSequenceCommand(turret)); // copilot.x().onTrue(new InstantCommand(() -> intake.setFold(false))).onFalse(new InstantCommand(() -> intake.setFold(true))); } private void setDefaultCommands() { - drivetrain.setDefaultCommand(new ArcadeDriveCommand(drivetrain, driver)); - shooter.setDefaultCommand(new StartEndCommand( - () -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM), - () -> {}, - shooter - )); +// drivetrain.setDefaultCommand(new ArcadeDriveCommand(drivetrain, driver)); +// shooter.setDefaultCommand(new StartEndCommand( +// () -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM), +// () -> {}, +// shooter +// )); + turret.setDefaultCommand(new TurretManualCommand(turret, copilot)); +// indexer.setDefaultCommand(new IndexerIdleCommand(indexer)); } diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerCommand.java index f246b4c..27d2aef 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerCommand.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerCommand.java @@ -7,6 +7,7 @@ public class IndexerCommand extends CommandBase { public IndexerCommand(Indexer indexer) { this.indexer = indexer; + addRequirements(indexer); } @Override diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerIdleCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerIdleCommand.java new file mode 100644 index 0000000..00c1519 --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerIdleCommand.java @@ -0,0 +1,19 @@ +package org.team1540.bunnybotTank2023.commands.indexer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class IndexerIdleCommand extends CommandBase { + private final Indexer indexer; + + public IndexerIdleCommand(Indexer indexer) { + this.indexer = indexer; + addRequirements(indexer); + } + + @Override + public void initialize() { + indexer.setBottomSpeed(0.4); + indexer.setTopSpeed(-0.2); + } +} + diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java index aae6d02..90642de 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java @@ -41,7 +41,6 @@ public void periodic() { } public void setVelocity(double speedRPM) { - System.out.println("test"); setpoint = speedRPM; averageFilter.clear(); io.setVelocity(speedRPM); diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java index f360090..690e5db 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java @@ -3,14 +3,25 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; +import org.team1540.bunnybotTank2023.Constants; import org.team1540.bunnybotTank2023.io.turret.TurretInputsAutoLogged; import org.team1540.bunnybotTank2023.io.turret.TurretIO; +import org.team1540.bunnybotTank2023.utils.AverageFilter; +import org.team1540.bunnybotTank2023.utils.LoggedTunableNumber; + +import static org.team1540.bunnybotTank2023.Constants.*; public class Turret extends SubsystemBase { //fields :) private final TurretInputsAutoLogged inputs = new TurretInputsAutoLogged(); private final TurretIO io; + private final AverageFilter averageFilter = new AverageFilter(20); + + private final LoggedTunableNumber kP = new LoggedTunableNumber("Turret/kP", TurretConstants.kP); + private final LoggedTunableNumber kI = new LoggedTunableNumber("Turret/kI", TurretConstants.kI); + private final LoggedTunableNumber kD = new LoggedTunableNumber("Turret/kD", TurretConstants.kD); + //constructor public Turret(TurretIO io){ this.io = io; @@ -20,6 +31,11 @@ public Turret(TurretIO io){ public void periodic(){ io.updateInputs(inputs); Logger.getInstance().processInputs("Turret", inputs); + averageFilter.add(inputs.turretCurrentPositionDegrees); + + if (TUNING_MODE && (kP.hasChanged() || kI.hasChanged() || kD.hasChanged())) { + io.configurePID(kP.get(), kI.get(), kD.get()); + } } public boolean getForwardLimitSwitch(){ @@ -30,7 +46,16 @@ public boolean getReverseLimitSwitch(){ return inputs.reverseLimitSwitch; } + public boolean isAtSetpoint() { + return Math.abs(inputs.turretSetPointDegrees - averageFilter.getAverage()) < 0.5; + } + + public void resetToEncoder(Rotation2d position) { + io.resetEncoder(position); + } + public void autoSetPosition(Rotation2d position){ + averageFilter.clear(); io.setTurretPosition(position); } @@ -38,6 +63,10 @@ public void setVoltage(double volts){ io.setVoltage(volts); } + public void stop() { + setVoltage(0); + } + } diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretManualCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretManualCommand.java new file mode 100644 index 0000000..a95e60b --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretManualCommand.java @@ -0,0 +1,27 @@ +package org.team1540.bunnybotTank2023.commands.turret; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.team1540.bunnybotTank2023.Constants; + +public class TurretManualCommand extends CommandBase { + private final Turret turret; + private final CommandXboxController controller; + + public TurretManualCommand(Turret turret, CommandXboxController controller) { + this.turret = turret; + this.controller = controller; + addRequirements(turret); + } + + @Override + public void execute() { + turret.setVoltage(MathUtil.applyDeadband(controller.getRightX(), Constants.DEADZONE_RADIUS) * 12.0); + } + + @Override + public void end(boolean interrupted) { + turret.stop(); + } +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretSetpointCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretSetpointCommand.java new file mode 100644 index 0000000..f1a3f96 --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretSetpointCommand.java @@ -0,0 +1,25 @@ +package org.team1540.bunnybotTank2023.commands.turret; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurretSetpointCommand extends CommandBase { + private final Turret turret; + private final Rotation2d setpoint; + + public TurretSetpointCommand(Turret turret, Rotation2d setpoint) { + this.turret = turret; + this.setpoint = setpoint; + addRequirements(turret); + } + + @Override + public void initialize() { + turret.autoSetPosition(setpoint); + } + + @Override + public boolean isFinished() { + return turret.isAtSetpoint(); + } +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretZeroSequenceCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretZeroSequenceCommand.java new file mode 100644 index 0000000..992c007 --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretZeroSequenceCommand.java @@ -0,0 +1,20 @@ +package org.team1540.bunnybotTank2023.commands.turret; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import static org.team1540.bunnybotTank2023.Constants.*; + +public class TurretZeroSequenceCommand extends SequentialCommandGroup { + public TurretZeroSequenceCommand(Turret turret) { + addCommands( + new InstantCommand(() -> turret.setVoltage(2)), + new WaitUntilCommand(turret::getForwardLimitSwitch), + new InstantCommand(() -> turret.resetToEncoder(TurretConstants.FORWARD_LIMIT_POSITION)), + new TurretSetpointCommand(turret, Rotation2d.fromDegrees(0)).asProxy() + ); + addRequirements(turret); + } +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/turret/TurretIO.java b/src/main/java/org/team1540/bunnybotTank2023/io/turret/TurretIO.java index ac84dc1..813f7b3 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/io/turret/TurretIO.java +++ b/src/main/java/org/team1540/bunnybotTank2023/io/turret/TurretIO.java @@ -8,11 +8,11 @@ public interface TurretIO { @AutoLog class TurretInputs{ // fields: - public double motorCurrentPositionDegrees = 0; - public double SetPointDegrees = 0; + public double turretCurrentPositionDegrees = 0; + public double turretSetPointDegrees = 0; public double motorVoltage = 0; public double motorCurrentAmps = 0; - public double motorVelocityRPM = 0; + public double motorVelocityRPS = 0; public boolean forwardLimitSwitch = false; public boolean reverseLimitSwitch = false; } @@ -21,6 +21,10 @@ class TurretInputs{ void setTurretPosition(Rotation2d position); + void resetEncoder(Rotation2d position); + + void configurePID(double kP, double kI, double kD); + void updateInputs(TurretInputs inputs); } diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/turret/TurretIOReal.java b/src/main/java/org/team1540/bunnybotTank2023/io/turret/TurretIOReal.java index 33c5f5e..ee0c911 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/io/turret/TurretIOReal.java +++ b/src/main/java/org/team1540/bunnybotTank2023/io/turret/TurretIOReal.java @@ -1,35 +1,33 @@ package org.team1540.bunnybotTank2023.io.turret; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.ForwardLimitValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.ReverseLimitValue; import edu.wpi.first.math.geometry.Rotation2d; -import org.team1540.bunnybotTank2023.Constants; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.signals.InvertedValue; +import org.team1540.bunnybotTank2023.utils.Conversions; + +import static org.team1540.bunnybotTank2023.Constants.*; public class TurretIOReal implements TurretIO { // fields ^-^ - private final TalonFX motor = new TalonFX(Constants.TurretConstants.MOTOR_ID); + private final TalonFX motor = new TalonFX(TurretConstants.MOTOR_ID); private final PositionVoltage positionVoltage = new PositionVoltage(0).withSlot(0); + private final MotionMagicVoltage MMVoltage = new MotionMagicVoltage(0).withSlot(0); private Rotation2d setPosition = new Rotation2d(); // constructor - TurretIOReal() { + public TurretIOReal() { // robot init, set slot 0 gains - var slot0Configs = new Slot0Configs(); - slot0Configs.kP = Constants.TurretConstants.kP; - slot0Configs.kI = Constants.TurretConstants.kI; - slot0Configs.kD = Constants.TurretConstants.kD; - - motor.getConfigurator().apply(slot0Configs, 0.050); - - positionVoltage.Slot = 0; + Slot0Configs slot0Configs = new Slot0Configs(); + slot0Configs.kP = TurretConstants.kP; + slot0Configs.kI = TurretConstants.kI; + slot0Configs.kD = TurretConstants.kD; // Current Limits CurrentLimitsConfigs turretCurrentLimit = new CurrentLimitsConfigs(); @@ -40,10 +38,22 @@ public class TurretIOReal implements TurretIO { // Motor Inversion MotorOutputConfigs turretOutput = new MotorOutputConfigs(); - turretOutput.Inverted = InvertedValue.Clockwise_Positive; + turretOutput.Inverted = InvertedValue.CounterClockwise_Positive; // Brake Mode turretOutput.NeutralMode = NeutralModeValue.Brake; + + MotionMagicConfigs turretMotionMagic = new MotionMagicConfigs(); + turretMotionMagic.MotionMagicAcceleration = TurretConstants.MAX_ACCEL_RPS2; + turretMotionMagic.MotionMagicCruiseVelocity = TurretConstants.CRUISE_VELOCITY_RPS; + + TalonFXConfiguration motorConfig = new TalonFXConfiguration(); + motorConfig.Slot0 = slot0Configs; + motorConfig.CurrentLimits = turretCurrentLimit; + motorConfig.MotorOutput = turretOutput; + motorConfig.MotionMagic = turretMotionMagic; + + motor.getConfigurator().apply(motorConfig); } // sets the voltage @@ -56,15 +66,30 @@ public void setVoltage(double volts) { @Override public void setTurretPosition(Rotation2d position) { // using the gear ratio to convert from motor rotations to mechanism rotations - motor.setControl(positionVoltage.withPosition(position.getRotations()/Constants.TurretConstants.gearRatio)); + motor.setControl(MMVoltage.withPosition(Conversions.Rotation2dToMotorRots(position, TurretConstants.gearRatio))); setPosition = position; } + @Override + public void resetEncoder(Rotation2d position) { + motor.setRotorPosition(Conversions.Rotation2dToMotorRots(position, TurretConstants.gearRatio)); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + Slot0Configs pidConfigs = new Slot0Configs(); + motor.getConfigurator().refresh(pidConfigs); + pidConfigs.kP = kP; + pidConfigs.kI = kI; + pidConfigs.kD = kD; + motor.getConfigurator().apply(pidConfigs); + } + @Override public void updateInputs(TurretInputs inputs) { - inputs.motorVelocityRPM = motor.getVelocity().getValue(); - inputs.motorCurrentPositionDegrees = motor.getPosition().getValue() * 360; - inputs.SetPointDegrees = setPosition.getRotations() * 360; + inputs.motorVelocityRPS = motor.getVelocity().getValue(); + inputs.turretCurrentPositionDegrees = Conversions.motorRotsToRotation2d(motor.getPosition().getValue(), TurretConstants.gearRatio).getDegrees(); + inputs.turretSetPointDegrees = setPosition.getDegrees(); inputs.motorCurrentAmps = motor.getSupplyCurrent().getValue(); inputs.motorVoltage = motor.getSupplyVoltage().getValue(); inputs.forwardLimitSwitch = (motor.getForwardLimit().getValue() == ForwardLimitValue.ClosedToGround);