diff --git a/.factorypath b/.factorypath index 12b66cf..ceb01f7 100644 --- a/.factorypath +++ b/.factorypath @@ -1,4 +1,4 @@ - - + + diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 61273ec..478f3f5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -54,7 +54,6 @@ public static class IntakeConstants { public static class PivotConstants { public static final int pivotCANId = 13; - // 80 falcon rotations = 1 pivot rotation // 5:1 planetary + 5:1 planetary + 48:21 chain public static final double pivotGearRatio = 57.1429; @@ -76,4 +75,9 @@ public static class ShooterConstants { public static final double shootingPower = -0.75; public static final double conveyorPower = 0.6; } + + public static class HangConstants { + public static final int leaderCANId = 14; + public static final int followerCANId = 15; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0ce5129..01d1ee7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,7 @@ import frc.robot.commands.FeedForwardCharacterization; import frc.robot.commands.PivotCommands; import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.hang.*; import frc.robot.subsystems.intake.*; import frc.robot.subsystems.limelight.*; import frc.robot.subsystems.pivot.*; @@ -43,6 +44,7 @@ public class RobotContainer { private final Pivot pivot; private final Shooter shooter; private final Limelight limelight; + private final Hang hang; // Controller private final GenericHID driver = new GenericHID(0); // hotas @@ -67,6 +69,7 @@ public RobotContainer() { pivot = new Pivot(new PivotIOReal()); shooter = new Shooter(new ShooterIOReal()); limelight = new Limelight(new LimelightIOReal(), drive); + hang = new Hang(new HangIOReal()); break; @@ -85,6 +88,7 @@ public RobotContainer() { pivot = new Pivot(new PivotIOSim()); shooter = new Shooter(new ShooterIOSim()); limelight = new Limelight(new LimelightIO() {}, drive); + hang = new Hang(new HangIOSim()); break; @@ -102,6 +106,7 @@ public RobotContainer() { pivot = new Pivot(new PivotIO() {}); shooter = new Shooter(new ShooterIO() {}); limelight = new Limelight(new LimelightIO() {}, drive); + hang = new Hang(new HangIO() {}); break; } @@ -146,6 +151,9 @@ private void configureButtonBindings() { pivot.setDefaultCommand( PivotCommands.basicOperatorControl( pivot, () -> operator.getRightX() - operator.getLeftX())); + + hang.setDefaultCommand(hang.operatorControl(() -> + operator.getRightTriggerAxis() - operator.getLeftTriggerAxis())); } /** diff --git a/src/main/java/frc/robot/subsystems/hang/Hang.java b/src/main/java/frc/robot/subsystems/hang/Hang.java new file mode 100644 index 0000000..bc96043 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hang/Hang.java @@ -0,0 +1,35 @@ +package frc.robot.subsystems.hang; + +import java.util.function.DoubleSupplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Hang extends SubsystemBase { + HangIO io; + HangIOInputsAutoLogged inputs = new HangIOInputsAutoLogged(); + + public Hang(HangIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Hang", inputs); + } + + public void setSpeed(double speed) { + io.setSpeed(speed); + } + + public Command operatorControl(DoubleSupplier speedSupplier) { + return Commands.run(() -> { + double speed = speedSupplier.getAsDouble(); + setSpeed(speed * 0.1); + }, this); + } +} diff --git a/src/main/java/frc/robot/subsystems/hang/HangIO.java b/src/main/java/frc/robot/subsystems/hang/HangIO.java new file mode 100644 index 0000000..1caa8ef --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hang/HangIO.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems.hang; + +import org.littletonrobotics.junction.AutoLog; + +public interface HangIO { + @AutoLog + public static class HangIOInputs { + public double mainCurrent = 0.0; + public double followerCurrent = 0.0; + } + + default void updateInputs(HangIOInputs inputs) {} + + default void setSpeed(double power) {} +} diff --git a/src/main/java/frc/robot/subsystems/hang/HangIOReal.java b/src/main/java/frc/robot/subsystems/hang/HangIOReal.java new file mode 100644 index 0000000..9eb27f5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hang/HangIOReal.java @@ -0,0 +1,56 @@ +package frc.robot.subsystems.hang; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import frc.robot.Constants.HangConstants; + +public class HangIOReal implements HangIO { + + TalonFX leader; + TalonFX follower; + + StatusSignal leaderCurrent; + StatusSignal followerCurrent; + + public HangIOReal() { + + leader = new TalonFX(HangConstants.leaderCANId); + follower = new TalonFX(HangConstants.followerCANId); + + // TODO: should this boolean be false?? do they rotate in the same ways?? + follower.setControl(new Follower(HangConstants.leaderCANId, true)); + + var config = new TalonFXConfiguration(); + + config.CurrentLimits.SupplyCurrentLimit = 40; + config.CurrentLimits.SupplyCurrentLimitEnable = true; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + leader.getConfigurator().apply(config); + follower.getConfigurator().apply(config); + + leaderCurrent = leader.getTorqueCurrent(); + followerCurrent = follower.getTorqueCurrent(); + + // don't really need this to update that often b/c it's just for data visualization + BaseStatusSignal.setUpdateFrequencyForAll(100.0, leaderCurrent, followerCurrent); + leader.optimizeBusUtilization(1.0); + follower.optimizeBusUtilization(1.0); + + } + + public void updateInputs(HangIOInputs inputs) { + inputs.mainCurrent = leaderCurrent.getValueAsDouble(); + inputs.followerCurrent = followerCurrent.getValueAsDouble(); + } + + public void setSpeed(double power) { + leader.set(power); + } + + +} diff --git a/src/main/java/frc/robot/subsystems/hang/HangIOSim.java b/src/main/java/frc/robot/subsystems/hang/HangIOSim.java new file mode 100644 index 0000000..eb5f073 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hang/HangIOSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.hang; + +public class HangIOSim implements HangIO { + // todo +}