mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-08 16:28:05 -06:00
Robot should be ready
This commit is contained in:
Michael Mikovsky
@@ -34,7 +34,9 @@ import frc4388.robot.constants.FieldConstants;
|
||||
// Subsystems
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.intake.Intake;
|
||||
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
|
||||
import frc4388.robot.subsystems.shooter.Shooter;
|
||||
import frc4388.robot.subsystems.shooter.Shooter.ShooterMode;
|
||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.utility.DeferredBlock;
|
||||
@@ -98,13 +100,13 @@ public class RobotContainer {
|
||||
// new InstantCommand(() -> System.out.println(m_robotLED.getMode()))
|
||||
// );
|
||||
|
||||
private Command RobotShoot = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter)
|
||||
);
|
||||
// private Command RobotShoot = new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter)
|
||||
// );
|
||||
|
||||
private Command RobotIntake = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake)
|
||||
);
|
||||
// private Command RobotIntake = new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake)
|
||||
// );
|
||||
|
||||
public RobotContainer() {
|
||||
|
||||
@@ -147,23 +149,23 @@ public class RobotContainer {
|
||||
private void configureButtonBindings() {
|
||||
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
||||
|
||||
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
// .onTrue(new RotTo45(m_robotSwerveDrive));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
|
||||
.onTrue(RobotShoot)
|
||||
.onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter));
|
||||
// new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
|
||||
// .onTrue(RobotShoot)
|
||||
// .onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode)
|
||||
.onTrue(RobotIntake)
|
||||
.onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter));
|
||||
// new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode)
|
||||
// .onTrue(RobotIntake)
|
||||
// .onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive));
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
@@ -172,12 +174,27 @@ public class RobotContainer {
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter));
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Extended);}, m_robotIntake));
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake));
|
||||
|
||||
|
||||
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode()));
|
||||
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode()));
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
m_robotIntake.io.updateGains();
|
||||
m_robotShooter.io.updateGains();
|
||||
}));
|
||||
|
||||
// IF the driver is holding the aim button, aim the robot towards the hub
|
||||
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
|
||||
|
||||
@@ -11,6 +11,8 @@ import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
|
||||
import frc4388.robot.constants.Constants;
|
||||
//import frc4388.robot.constants.Constants.ElevatorConstants;
|
||||
import frc4388.robot.constants.Constants.SimConstants;
|
||||
import frc4388.robot.subsystems.intake.IntakeConstants;
|
||||
@@ -82,22 +84,21 @@ public class RobotMap {
|
||||
swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
|
||||
|
||||
// Configure Shooter 22,23,24
|
||||
|
||||
// TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id);
|
||||
// TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id);
|
||||
// TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id);
|
||||
TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.RIO_CANBUS);
|
||||
TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id, Constants.RIO_CANBUS);
|
||||
TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id, Constants.RIO_CANBUS);
|
||||
|
||||
//Configure Intake 20,21
|
||||
TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id);
|
||||
TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id);
|
||||
TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id, Constants.RIO_CANBUS);
|
||||
TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.RIO_CANBUS);
|
||||
|
||||
// DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
|
||||
// DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
|
||||
// DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
|
||||
|
||||
|
||||
shooterIO = new ShooterIO() {};
|
||||
// shooterIO = new ShooterReal(shooter1, shooter2, indexer);
|
||||
// shooterIO = new ShooterIO() {};
|
||||
shooterIO = new ShooterReal(shooter1, shooter2, indexer);
|
||||
|
||||
intakeIO = new IntakeReal(arm, roller);
|
||||
|
||||
@@ -105,9 +106,9 @@ public class RobotMap {
|
||||
FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
|
||||
|
||||
|
||||
// FaultTalonFX.addDevice(shooter1, "Shooter1");
|
||||
// FaultTalonFX.addDevice(shooter2, "Shooter2");
|
||||
// FaultTalonFX.addDevice(indexer, "Indexer");
|
||||
FaultTalonFX.addDevice(shooter1, "Shooter1");
|
||||
FaultTalonFX.addDevice(shooter2, "Shooter2");
|
||||
FaultTalonFX.addDevice(indexer, "Indexer");
|
||||
FaultTalonFX.addDevice(arm, "Arm");
|
||||
FaultTalonFX.addDevice(roller, "Roller");
|
||||
|
||||
|
||||
@@ -7,12 +7,12 @@ public final class BuildConstants {
|
||||
public static final String MAVEN_GROUP = "";
|
||||
public static final String MAVEN_NAME = "2026KPopRobotHunters";
|
||||
public static final String VERSION = "unspecified";
|
||||
public static final int GIT_REVISION = 35;
|
||||
public static final String GIT_SHA = "684e18ddcc5438f14f05c5902506d14cc4cbab19";
|
||||
public static final String GIT_DATE = "2026-02-07 14:52:21 MST";
|
||||
public static final int GIT_REVISION = 36;
|
||||
public static final String GIT_SHA = "d90bddac0fff1c8f1bb6dd9a6b4a862c822a005a";
|
||||
public static final String GIT_DATE = "2026-02-09 17:03:48 MST";
|
||||
public static final String GIT_BRANCH = "arm-test";
|
||||
public static final String BUILD_DATE = "2026-02-09 16:43:56 MST";
|
||||
public static final long BUILD_UNIX_TIME = 1770680636562L;
|
||||
public static final String BUILD_DATE = "2026-02-09 18:14:36 MST";
|
||||
public static final long BUILD_UNIX_TIME = 1770686076395L;
|
||||
public static final int DIRTY = 1;
|
||||
|
||||
private BuildConstants(){}
|
||||
|
||||
@@ -7,6 +7,8 @@
|
||||
|
||||
package frc4388.robot.constants;
|
||||
|
||||
import com.ctre.phoenix6.CANBus;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||
import edu.wpi.first.apriltag.AprilTagFields;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
@@ -31,7 +33,8 @@ import frc4388.utility.structs.LEDPatterns;
|
||||
* constants are needed, to reduce verbosity.
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final String CANBUS_NAME = "rio";
|
||||
public static final CANBus RIO_CANBUS = CANBus.roboRIO();
|
||||
public static final CANBus CANIVORE_CANBUS = new CANBus("drivetrain");
|
||||
|
||||
// public static final class LiDARConstants {
|
||||
// public static final int REEF_LIDAR_DIO_CHANNEL = 7;
|
||||
|
||||
@@ -1,6 +1,9 @@
|
||||
package frc4388.robot.subsystems.intake;
|
||||
|
||||
import static edu.wpi.first.units.Units.Rotation;
|
||||
import static edu.wpi.first.units.Units.Rotations;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
@@ -12,7 +15,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
|
||||
public class Intake extends SubsystemBase {
|
||||
IntakeIO io;
|
||||
public IntakeIO io;
|
||||
IntakeStateAutoLogged state = new IntakeStateAutoLogged();
|
||||
|
||||
Supplier<Pose2d> m_swervePoseSupplier;
|
||||
@@ -26,19 +29,19 @@ public class Intake extends SubsystemBase {
|
||||
}
|
||||
|
||||
public enum IntakeMode {
|
||||
Up,
|
||||
Down,
|
||||
Extended,
|
||||
Retracted,
|
||||
}
|
||||
|
||||
public void setMode(IntakeMode mode) {
|
||||
switch (mode) {
|
||||
case Up:
|
||||
io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
|
||||
io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP);
|
||||
case Extended:
|
||||
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get()));
|
||||
io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get()));
|
||||
break;
|
||||
case Down:
|
||||
io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
|
||||
io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY);
|
||||
case Retracted:
|
||||
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
|
||||
io.setRollerVelocity(state, RotationsPerSecond.of(0));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -12,6 +12,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
|
||||
public class IntakeConstants {
|
||||
@@ -33,21 +34,28 @@ public class IntakeConstants {
|
||||
|
||||
//when testing the negative output of 10% made the robot put the intake up
|
||||
|
||||
public static final Angle ARM_LIMIT_LOWER = Degrees.of(90);
|
||||
public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
|
||||
public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0);
|
||||
public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0);
|
||||
// public static final Angle ARM_LIMIT_LOWER = Degrees.of(90);
|
||||
// public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
|
||||
|
||||
public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0);
|
||||
public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.25);
|
||||
|
||||
public static final ConfigurableDouble ROLLER_ACTIVE = new ConfigurableDouble("Rolelr Active", 10);
|
||||
// public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25);
|
||||
|
||||
// public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0);
|
||||
// public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0);
|
||||
|
||||
|
||||
public static final Slot0Configs ARM_PID = new Slot0Configs()
|
||||
.withKP(2.0)
|
||||
.withKP(0.2)
|
||||
.withKI(0.0)
|
||||
.withKD(10.0);
|
||||
.withKD(0.0);
|
||||
|
||||
public static final Slot0Configs ROLLER_PID = new Slot0Configs()
|
||||
.withKP(2.0)
|
||||
.withKP(0.2)
|
||||
.withKI(0.0)
|
||||
.withKD(10.0);
|
||||
.withKD(0.0);
|
||||
|
||||
// 0 is paralell to the ground, 90 is directly up
|
||||
// public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
|
||||
|
||||
@@ -39,4 +39,5 @@ public interface IntakeIO {
|
||||
public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {}
|
||||
|
||||
public default void updateInputs(IntakeState state) {}
|
||||
public default void updateGains() {}
|
||||
}
|
||||
@@ -95,6 +95,10 @@ public class IntakeReal implements IntakeIO {
|
||||
|
||||
state.rollerVelocity = m_rollerMotor.getVelocity().getValue();
|
||||
state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateGains() {
|
||||
|
||||
IntakeConstants.ARM_PID.kP = arm_kP.get();
|
||||
IntakeConstants.ARM_PID.kI = arm_kI.get();
|
||||
|
||||
@@ -1,5 +1,9 @@
|
||||
package frc4388.robot.subsystems.shooter;
|
||||
|
||||
import static edu.wpi.first.units.Units.Rotation;
|
||||
import static edu.wpi.first.units.Units.Rotations;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
@@ -11,7 +15,7 @@ import frc4388.robot.subsystems.intake.IntakeConstants;
|
||||
import frc4388.robot.subsystems.shooter.ShooterIO.ShooterState;
|
||||
|
||||
public class Shooter extends SubsystemBase {
|
||||
ShooterIO io;
|
||||
public ShooterIO io;
|
||||
ShooterStateAutoLogged state = new ShooterStateAutoLogged();
|
||||
|
||||
// Supplier<Pose2d> m_swervePoseSupplier;
|
||||
@@ -46,19 +50,19 @@ public class Shooter extends SubsystemBase {
|
||||
public void setMode(ShooterMode mode) {
|
||||
switch (mode) {
|
||||
case Active:
|
||||
io.setMotor1Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY);
|
||||
io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY);
|
||||
io.setIndexerVelocity(state, ShooterConstants.INDEXER_ACTIVE_VELOCITY);
|
||||
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get()));
|
||||
// io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY);
|
||||
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_ACTIVE_VELOCITY.get()));
|
||||
break;
|
||||
case Resting:
|
||||
io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
|
||||
io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
|
||||
io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY);
|
||||
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
|
||||
// io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
|
||||
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get()));
|
||||
break;
|
||||
case Inactive:
|
||||
io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
|
||||
io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
|
||||
io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY);
|
||||
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
|
||||
// io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
|
||||
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get()));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -11,6 +11,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
|
||||
public class ShooterConstants {
|
||||
@@ -20,16 +21,20 @@ public class ShooterConstants {
|
||||
public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.;
|
||||
public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.;
|
||||
public static final double INDEXER_GEAR_RATIO = 1.;
|
||||
|
||||
public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(0.0);
|
||||
|
||||
public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(-15);
|
||||
public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
|
||||
public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(0);
|
||||
public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
|
||||
|
||||
// public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(30);
|
||||
// public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(15);
|
||||
// public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
|
||||
|
||||
// public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(10);
|
||||
// public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
|
||||
|
||||
public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30);
|
||||
public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15);
|
||||
public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
|
||||
|
||||
public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 10);
|
||||
public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
|
||||
|
||||
public static Slot0Configs SHOOTER_PID = new Slot0Configs()
|
||||
.withKV(0.0)
|
||||
@@ -37,6 +42,12 @@ public class ShooterConstants {
|
||||
.withKI(0.0)
|
||||
.withKD(0.0);
|
||||
|
||||
public static Slot0Configs INDEXER_PID = new Slot0Configs()
|
||||
.withKV(0.0)
|
||||
.withKP(0.0)
|
||||
.withKI(0.0)
|
||||
.withKD(0.0);
|
||||
|
||||
// Limits
|
||||
|
||||
// 0 is the forward angle on the robot.
|
||||
@@ -45,8 +56,8 @@ public class ShooterConstants {
|
||||
// public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180);
|
||||
|
||||
// 0 is paralell to the ground, 90 is directly up
|
||||
public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
|
||||
public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
|
||||
// public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
|
||||
// public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
|
||||
|
||||
// Motor configs
|
||||
// public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
@@ -61,10 +72,10 @@ public class ShooterConstants {
|
||||
// );
|
||||
|
||||
|
||||
public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22);
|
||||
public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23);
|
||||
public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24);
|
||||
|
||||
public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22);
|
||||
public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23);
|
||||
public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24);
|
||||
|
||||
|
||||
public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
@@ -73,7 +84,7 @@ public class ShooterConstants {
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
|
||||
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
|
||||
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
);
|
||||
public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
@@ -86,6 +97,7 @@ public class ShooterConstants {
|
||||
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
|
||||
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
);
|
||||
|
||||
public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
|
||||
@@ -36,14 +36,17 @@ public interface ShooterIO {
|
||||
Current motor2Current = Amps.of(0);
|
||||
Current indexerCurrent = Amps.of(0);
|
||||
|
||||
LinearVelocity motorLinearVelocity = InchesPerSecond.of(0);
|
||||
// LinearVelocity motorLinearVelocity = InchesPerSecond.of(0);
|
||||
}
|
||||
|
||||
// public default void setShooterAngle(ShooterState state, Angle angle) {}
|
||||
// public default void setShooterPitch(ShooterState state, Angle angle) {}
|
||||
public default void setMotor1Velocity(ShooterState state, AngularVelocity angularVelocity) {}
|
||||
public default void setMotor2Velocity(ShooterState state, AngularVelocity linearVelocity) {}
|
||||
public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {}
|
||||
// public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {}
|
||||
public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {}
|
||||
|
||||
public default void updateInputs(ShooterState state) {}
|
||||
|
||||
|
||||
public default void updateGains() {}
|
||||
}
|
||||
@@ -8,12 +8,15 @@ import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import frc4388.robot.subsystems.intake.IntakeConstants;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
|
||||
public class ShooterReal implements ShooterIO {
|
||||
|
||||
TalonFX m_shooter1Motor;
|
||||
TalonFX m_shooter2Motor;
|
||||
TalonFX m_indexerMotor;
|
||||
|
||||
VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0);
|
||||
VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
|
||||
VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
|
||||
@@ -27,14 +30,18 @@ public class ShooterReal implements ShooterIO {
|
||||
m_shooter1Motor = shooter1Motor;
|
||||
m_shooter2Motor = shooter2Motor;
|
||||
m_indexerMotor = indexerMotor;
|
||||
|
||||
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
|
||||
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
|
||||
m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
|
||||
|
||||
|
||||
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG);
|
||||
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG);
|
||||
m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG);
|
||||
|
||||
shooter1Velocity.Slot = 0;
|
||||
shooter2Velocity.Slot = 0;
|
||||
m_indexerVelocity.Slot = 0;
|
||||
}
|
||||
|
||||
private Angle clampAng(Angle x, Angle min, Angle max){
|
||||
@@ -83,34 +90,60 @@ public class ShooterReal implements ShooterIO {
|
||||
|
||||
|
||||
@Override
|
||||
public void setMotor1Velocity(ShooterState state, AngularVelocity target) {
|
||||
public void setShooterVelocity(ShooterState state, AngularVelocity target) {
|
||||
state.motor1TargetVelocity = target;
|
||||
state.motor2TargetVelocity = target;
|
||||
|
||||
double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
|
||||
m_shooter1Motor.setControl(new VelocityDutyCycle(motorRps));
|
||||
m_shooter2Motor.setControl(new VelocityDutyCycle(motorRps));
|
||||
AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO);
|
||||
|
||||
m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps));
|
||||
m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setIndexerVelocity(ShooterState state, AngularVelocity target) {
|
||||
state.indexerTargetVelocity = target;
|
||||
double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
|
||||
m_indexerMotor.setControl(new VelocityDutyCycle(motorRps));
|
||||
AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO);
|
||||
m_indexerMotor.setControl(m_indexerVelocity.withVelocity(motorRps));
|
||||
}
|
||||
|
||||
|
||||
ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2);
|
||||
ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KP", 0);
|
||||
ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KP", 0);
|
||||
|
||||
ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2);
|
||||
ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0);
|
||||
ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
|
||||
|
||||
@Override
|
||||
public void updateInputs(ShooterState state) {
|
||||
|
||||
state.motor1Velocity = m_shooter1Motor.getVelocity().getValue();
|
||||
state.motor2Velocity = m_shooter2Motor.getVelocity().getValue();
|
||||
state.indexerVelocity = m_indexerMotor.getVelocity().getValue();
|
||||
state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().times(ShooterConstants.SHOOTERMOTOR1_GEAR_RATIO);
|
||||
state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().times(ShooterConstants.SHOOTERMOTOR2_GEAR_RATIO);
|
||||
state.indexerVelocity = m_indexerMotor.getVelocity().getValue().times(ShooterConstants.INDEXER_GEAR_RATIO);
|
||||
|
||||
state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
|
||||
// state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
|
||||
|
||||
state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue();
|
||||
state.motor2Current = m_shooter2Motor.getStatorCurrent().getValue();
|
||||
state.indexerCurrent = m_indexerMotor.getStatorCurrent().getValue();
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateGains() {
|
||||
// TEMPORARY PIDs
|
||||
ShooterConstants.SHOOTER_PID.kP = shooter_kP.get();
|
||||
ShooterConstants.SHOOTER_PID.kI = shooter_kI.get();
|
||||
ShooterConstants.SHOOTER_PID.kD = shooter_kD.get();
|
||||
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
|
||||
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
|
||||
|
||||
ShooterConstants.INDEXER_PID.kP = indexer_kP.get();
|
||||
ShooterConstants.INDEXER_PID.kI = indexer_kI.get();
|
||||
ShooterConstants.INDEXER_PID.kD = indexer_kD.get();
|
||||
m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_PID);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -22,6 +22,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
|
||||
|
||||
import edu.wpi.first.units.measure.Distance;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import frc4388.robot.constants.Constants;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
//import edu.wpi.first.units.measure.measure.Distance;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
@@ -31,7 +32,6 @@ import frc4388.utility.structs.Gains;
|
||||
// No mans land
|
||||
// Beware, there be dragons.
|
||||
public final class SwerveDriveConstants {
|
||||
public static final String CANBUS_NAME = "drivetrain";
|
||||
public static final double MAX_ROT_SPEED = Math.PI * 2;
|
||||
public static final double AUTO_MAX_ROT_SPEED = 1.5;
|
||||
public static final double MIN_ROT_SPEED = 1.0;
|
||||
@@ -209,7 +209,7 @@ public final class SwerveDriveConstants {
|
||||
}
|
||||
|
||||
public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
|
||||
.withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(CANBUS_NAME);
|
||||
.withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(Constants.CANIVORE_CANBUS.getName());
|
||||
|
||||
private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
|
||||
new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() // holy verbosity batman.
|
||||
|
||||
@@ -85,13 +85,12 @@ public class FaultReporter {
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// CAN header
|
||||
System.out.println(CAN_HEADER);
|
||||
|
||||
CANBus canBus = new CANBus(Constants.CANBUS_NAME);
|
||||
// CANBus canBus = new CANBus();
|
||||
|
||||
CANBusStatus canInfo = canBus.getStatus();
|
||||
CANBusStatus canInfo = Constants.RIO_CANBUS.getStatus();
|
||||
|
||||
System.out.println("CANInfo BusOffCount - " + canInfo.BusOffCount);
|
||||
System.out.println("CANInfo BusUtilization - " + canInfo.BusUtilization);
|
||||
|
||||
Reference in New Issue
Block a user