diff --git a/src/main/java/frc4388/robot/subsystems/TestRobot.java b/src/main/java/frc4388/robot/subsystems/TestRobot.java new file mode 100644 index 0000000..c435c43 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/TestRobot.java @@ -0,0 +1,245 @@ +package frc4388.robot.subsystems; + +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.configurable.ConfigurableDouble; + +public class TestRobot extends SubsystemBase { + + // TalonFX m_intakeMotor; + // TalonFX m_armMotor; + // TalonFX m_storageMotor; + TalonFX m_outerShooter; + TalonFX m_innerShooter; + + ConfigurableDouble intakeRate = new ConfigurableDouble("Intake Rate", 0); + + + ConfigurableDouble armUpPosition = new ConfigurableDouble("Arm Up Position", 0); + ConfigurableDouble armDownPosition = new ConfigurableDouble("Arm Down Position", 0); + + ConfigurableDouble storageRate = new ConfigurableDouble("Storage Rate", 0); + ConfigurableDouble outerRate = new ConfigurableDouble("Outer Rate", 0); + ConfigurableDouble innerRate = new ConfigurableDouble("Inner Rate", 0); + + VelocityDutyCycle outerVelocity = new VelocityDutyCycle(0); + VelocityDutyCycle innerVelocity = new VelocityDutyCycle(0); + + public static final double MAX_OUTER_VELOCITY = 1200; // Rots per second + + public enum RobotStare { + IntakeDown, + Idle, + Shoot + } + + public RobotStare robotStare = RobotStare.Idle; + + + public TestRobot( + // TalonFX intakeMotor, + // TalonFX armMotor, + // TalonFX storageMotor, + TalonFX outerShooter, + TalonFX innerShooter + ) { + // m_intakeMotor = intakeMotor; + // m_armMotor = armMotor; + // m_storageMotor = storageMotor; + m_outerShooter = outerShooter; + m_innerShooter = innerShooter; + + // m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG); + // m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG); + // m_storageMotor.getConfigurator().apply(STORAGE_MOTOR_CONFIG); + m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG); + m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG); + + m_outerShooter.getConfigurator().apply(SHOOTER_PID); + m_innerShooter.getConfigurator().apply(SHOOTER_PID); + + outerVelocity.Slot = 0; + innerVelocity.Slot = 0; + } + + // public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); + // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); + + // public static final TalonFXConfiguration STORAGE_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); + public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); + public static final TalonFXConfiguration INNER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + + ); + + public static Slot0Configs SHOOTER_PID; + + static { + SHOOTER_PID = new Slot0Configs(); + SHOOTER_PID.kV = 0.12; + // SHOOTER_PID.kP = 0.11; + // SHOOTER_PID.kI = 0.48; + // SHOOTER_PID.kD = 0.01; + SHOOTER_PID.kP = 0.3; + SHOOTER_PID.kI = 0.0; + SHOOTER_PID.kD = 0.0; + } + + + ConfigurableDouble kV = new ConfigurableDouble("kV", 0.12); + ConfigurableDouble kP = new ConfigurableDouble("kP", 0.11); + ConfigurableDouble kI = new ConfigurableDouble("kI", 0.48); + ConfigurableDouble kD = new ConfigurableDouble("kD", 0.01); + + @Override + public void periodic() { + + SHOOTER_PID = new Slot0Configs(); + SHOOTER_PID.kV = kV.get(); + SHOOTER_PID.kP = kP.get(); + SHOOTER_PID.kI = kI.get(); + SHOOTER_PID.kD = kD.get(); + + m_outerShooter.getConfigurator().apply(SHOOTER_PID); + m_innerShooter.getConfigurator().apply(SHOOTER_PID); + + + SmartDashboard.putNumber("Outer Velocity", m_outerShooter.getVelocity().getValue().in(RotationsPerSecond)); + SmartDashboard.putNumber("Inner Velocity", m_innerShooter.getVelocity().getValue().in(RotationsPerSecond)); + + + switch (robotStare) { + case Idle: + + // Move the arm to the up position + PositionDutyCycle armPosition = new PositionDutyCycle(armUpPosition.get()); + // m_armMotor.setControl(armPosition); + + + // // Stop the intake motor + // m_intakeMotor.set(0); + + // // Stop the storage motor + // m_storageMotor.set(0); + + + // Stop the outer shooter motor + m_outerShooter.set(0); + // Stop the inner shooter motor + m_innerShooter.set(0); + + break; + case IntakeDown: + // Move the arm to the down sposition + PositionDutyCycle armPosition1 = new PositionDutyCycle(armDownPosition.get()); + // m_armMotor.setControl(armPosition1); + + // Move balls into the robot because the arm is down + VelocityDutyCycle intakeVelocity = new VelocityDutyCycle(intakeRate.get()); + // m_intakeMotor.setControl(intakeVelocity); + + // Move balls into the main box + VelocityDutyCycle storageVelocity = new VelocityDutyCycle(storageRate.get()); + // m_storageMotor.setControl(storageVelocity); + + + // Stop the outer shooter motor + m_outerShooter.set(0); + // Stop the inner shooter motor + m_innerShooter.set(0); + + break; + case Shoot: + + // Move the arm to the up position + PositionDutyCycle armPosition2 = new PositionDutyCycle(armUpPosition.get()); + // m_armMotor.setControl(armPosition2); + + // // Stop the intake motor + // m_intakeMotor.set(0); + + // // Move the balls into the + VelocityDutyCycle storageVelocity1 = new VelocityDutyCycle(-storageRate.get()); + // m_storageMotor.setControl(storageVelocity1); + + // outerVelocity. + // m_outerShooter.setControl(outerVelocity); + double outerRPM = outerRate.get(); + m_outerShooter.setControl(outerVelocity.withVelocity(outerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY)); + + + + // m_innerShooter.setControl(innerVelocity); + // m_innerShooter.set(innerRate.get()); + + double innerRPM = innerRate.get(); + m_innerShooter.setControl(innerVelocity.withVelocity(innerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY)); + + + // SmartDashboard.putNumber("Test", outerRate.get()); + + break; + default: + break; + } + + + + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..29e2ba2 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -0,0 +1,58 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.Rotation; + +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.status.FaultReporter; + +public class Intake extends SubsystemBase { + IntakeIO io; + IntakeStateAutoLogged state = new IntakeStateAutoLogged(); + + Supplier m_swervePoseSupplier; + + public Intake( + IntakeIO io, + Supplier swervePoseSupplier + ) { + this.io = io; + this.m_swervePoseSupplier = swervePoseSupplier; + } + + public enum FieldZone { + // The robot should aim at the hub + InShootZone, + // The robot should aim towards the wall + AimAtWall, + + } + + // Calculate what should be done based off of the position of the robot + // TODO: Implement field zones + public FieldZone getTarget(Pose2d position) { + return FieldZone.InShootZone; + } + + @Override + public void periodic() { + + + + // FaultReporter.register(this); // TODO Implement fault reporter + + + Logger.processInputs("Intake", state); + + Pose2d pose = m_swervePoseSupplier.get(); + Angle robotRot = pose.getRotation().getMeasure(); + + io.updateInputs(state); + + } +} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..f21fe8f --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,77 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.Angle; +import frc4388.utility.status.CanDevice; + +public class IntakeConstants { + // Motor conversions + // public static final double ANGLE_MOTOR_GEAR_RATIO = 1.; + public static final double PITCH_MOTOR_GEAR_RATIO = 1.; + public static final double FLYWHEEL_GEAR_RATIO = 1.; + public static final double FEEDER_INCHES_PER_ROT = 1.; + + // Limits + + // 0 is the forward angle on the robot. + // negative is left, positive is right + // public static final Angle ANGLE_LIMIT_LEFT = Degrees.of(-180); + // 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); + + // Motor configs + // public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); + + public static final class IDs { + public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); + } + + public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); + public static final TalonFXConfiguration FLYWHEEL_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); + public static final TalonFXConfiguration FEEDER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); +} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..35d82e1 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,42 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.units.CurrentUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.LinearVelocity; + +public interface IntakeIO { + @AutoLog + public class IntakeState { + // Angle IntakeAngle = Rotations.of(0); + // Angle IntakeTargetAngle = Rotations.of(0); + // Current angleMotorCurrent = Amps.of(0); + + // Angle IntakePitch = Rotations.of(0); + // Angle IntakeTargetPitch = Rotations.of(0); + // Current pitchMotorCurrent = Amps.of(0); + + AngularVelocity flywheelVelocity = RotationsPerSecond.of(0); + AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0); + Current flywheelMotorCurrent = Amps.of(0); + + LinearVelocity feederVelocity = InchesPerSecond.of(0); + LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); + Current feederMotorCurrent = Amps.of(0); + } + + // public default void setIntakeAngle(IntakeState state, Angle angle) {} + // public default void setIntakePitch(IntakeState state, Angle angle) {} + public default void setFlywheelVelocity(IntakeState state, AngularVelocity angularVelocity) {} + public default void setFeederVelocity(IntakeState state, LinearVelocity linearVelocity) {} + + public default void updateInputs(IntakeState state) {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java new file mode 100644 index 0000000..febf17a --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -0,0 +1,111 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.*; + +public class IntakeReal implements IntakeIO { + + // TalonFX m_angleMotor; + // TalonFX m_pitchMotor; + TalonFX m_flywheelMotor; + TalonFX m_feederMotor; + + public IntakeReal( + // TalonFX angleMotor, + // TalonFX pitchMotor, + TalonFX flywheelMotor, + TalonFX feederMotor + ) { + // m_angleMotor = angleMotor; + // m_pitchMotor = pitchMotor; + m_flywheelMotor = flywheelMotor; + m_feederMotor = feederMotor; + + // Apply the configs + // m_angleMotor.getConfigurator().apply(IntakeConstants.ANGLE_MOTOR_CONFIG); + // m_pitchMotor.getConfigurator().apply(IntakeConstants.PITCH_MOTOR_CONFIG); + m_flywheelMotor.getConfigurator().apply(IntakeConstants.FLYWHEEL_MOTOR_CONFIG); + m_feederMotor.getConfigurator().apply(IntakeConstants.FEEDER_MOTOR_CONFIG); + } + + private Angle clampAng(Angle x, Angle min, Angle max){ + if(x.gt(max)) { + return max; + }else if(x.lt(min)) { + return min; + }else{ + return x; + } + } + + // // TODO: Test + // @Override + // public void setIntakeAngle(IntakeState state, Angle angle) { + // state.IntakeTargetAngle = angle; + // // Assume that the angle is always accurate, because I think we will use a shaft encoder + // // Assume that 0 degrees = forwards. Might need an offset here + + // Angle boundedAngle = clampAng(angle, IntakeConstants.ANGLE_LIMIT_LEFT, IntakeConstants.ANGLE_LIMIT_RIGHT); + // // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT + // double motorTargetAngle = boundedAngle.in(Rotations) / IntakeConstants.ANGLE_MOTOR_GEAR_RATIO; + // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); + // m_angleMotor.setControl(posRequest); + // } + + + // TODO: Test + // @Override + // public void setIntakePitch(IntakeState state, Angle angle) { + // state.IntakeTargetPitch = angle; + // // TODO: Test + // // This assumes that the 0 is paralell to the ground. Might need an offset here + + + // Angle boundedAngle = clampAng(angle, IntakeConstants.PITCH_LIMIT_UPPER, IntakeConstants.PITCH_LIMIT_LOWER); + // // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT + // double motorTargetAngle = boundedAngle.in(Rotations) / IntakeConstants.PITCH_MOTOR_GEAR_RATIO; + // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); + // m_pitchMotor.setControl(posRequest); + // } + + @Override + public void setFlywheelVelocity(IntakeState state, AngularVelocity angularVelocity) { + state.flywheelTargetVelocity = angularVelocity; + // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) + double motorSpeed = angularVelocity.in(RotationsPerSecond) / IntakeConstants.FLYWHEEL_GEAR_RATIO; + VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); + m_feederMotor.setControl(velocity); + } + + @Override + public void setFeederVelocity(IntakeState state, LinearVelocity linearVelocity) { + state.feederTargetVelocity = linearVelocity; + // (IN / SEC) * (ROT / IN) = (ROT / SEC) + double motorSpeed = linearVelocity.in(InchesPerSecond) / IntakeConstants.FEEDER_INCHES_PER_ROT; + VelocityDutyCycle velRequest = new VelocityDutyCycle(motorSpeed); + m_feederMotor.setControl(velRequest); + } + + @Override + public void updateInputs(IntakeState state) { + // state.IntakeAngle = m_angleMotor.getPosition().getValue().times(IntakeConstants.ANGLE_MOTOR_GEAR_RATIO); + // state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue(); + + // state.IntakePitch = m_pitchMotor.getPosition().getValue().times(IntakeConstants.PITCH_MOTOR_GEAR_RATIO); + // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); + + state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue(); + state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue(); + + state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * IntakeConstants.FEEDER_INCHES_PER_ROT); + state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue(); + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 59ce39b..e938be4 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -1,9 +1,10 @@ package frc4388.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Degrees; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -12,10 +13,17 @@ import frc4388.utility.status.CanDevice; public class ShooterConstants { // Motor conversions - // public static final double ANGLE_MOTOR_GEAR_RATIO = 1.; - public static final double PITCH_MOTOR_GEAR_RATIO = 1.; - public static final double FLYWHEEL_GEAR_RATIO = 1.; + public static final double FEEDER_INCHES_PER_ROT = 1.; + 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 Slot0Configs SHOOTER_PID = new Slot0Configs() + .withKV(0.0) + .withKP(0.0) + .withKI(0.0) + .withKD(0.0); // Limits @@ -44,7 +52,7 @@ public class ShooterConstants { public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); } - public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration() + public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? @@ -54,7 +62,7 @@ public class ShooterConstants { .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); - public static final TalonFXConfiguration FLYWHEEL_MOTOR_CONFIG = new TalonFXConfiguration() + public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? @@ -64,7 +72,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 FEEDER_MOTOR_CONFIG = new TalonFXConfiguration() + public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 999abc3..5f0a1b8 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -24,19 +24,26 @@ public interface ShooterIO { // Angle shooterTargetPitch = Rotations.of(0); // Current pitchMotorCurrent = Amps.of(0); - AngularVelocity flywheelVelocity = RotationsPerSecond.of(0); - AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0); - Current flywheelMotorCurrent = Amps.of(0); + AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0); + AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0); + AngularVelocity indexerTargetVelocity = RotationsPerSecond.of(0); - LinearVelocity feederVelocity = InchesPerSecond.of(0); - LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); - Current feederMotorCurrent = Amps.of(0); + AngularVelocity motor1Velocity = RotationsPerSecond.of(0); + AngularVelocity motor2Velocity = RotationsPerSecond.of(0); + AngularVelocity indexerVelocity = RotationsPerSecond.of(0); + + Current motor1Current = Amps.of(0); + Current motor2Current = Amps.of(0); + Current indexerCurrent = Amps.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 setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {} - public default void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {} + public default void setMotor1Velocity(ShooterState state, AngularVelocity angularVelocity) {} + public default void setMotor2Velocity(ShooterState state, AngularVelocity linearVelocity) {} + public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {} public default void updateInputs(ShooterState state) {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 9db0e8b..159c70b 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -1,38 +1,43 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.InchesPerSecond; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; -import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.measure.*; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.configurable.ConfigurableDouble; public class ShooterReal implements ShooterIO { - // TalonFX m_angleMotor; - // TalonFX m_pitchMotor; - TalonFX m_flywheelMotor; - TalonFX m_feederMotor; + 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); + public ShooterReal( - // TalonFX angleMotor, - // TalonFX pitchMotor, - TalonFX flywheelMotor, - TalonFX feederMotor + TalonFX shooter1Motor, + TalonFX shooter2Motor, + TalonFX indexerMotor ) { - // m_angleMotor = angleMotor; - // m_pitchMotor = pitchMotor; - m_flywheelMotor = flywheelMotor; - m_feederMotor = feederMotor; + 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); - // Apply the configs - // m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG); - // m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG); - m_flywheelMotor.getConfigurator().apply(ShooterConstants.FLYWHEEL_MOTOR_CONFIG); - m_feederMotor.getConfigurator().apply(ShooterConstants.FEEDER_MOTOR_CONFIG); + + m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG); + m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG); + m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG); } private Angle clampAng(Angle x, Angle min, Angle max){ @@ -45,6 +50,10 @@ public class ShooterReal implements ShooterIO { } } + + + + // // TODO: Test // @Override // public void setShooterAngle(ShooterState state, Angle angle) { @@ -75,37 +84,36 @@ public class ShooterReal implements ShooterIO { // m_pitchMotor.setControl(posRequest); // } + @Override - public void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) { - state.flywheelTargetVelocity = angularVelocity; - // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) - double motorSpeed = angularVelocity.in(RotationsPerSecond) / ShooterConstants.FLYWHEEL_GEAR_RATIO; - VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); - m_feederMotor.setControl(velocity); + public void setMotor1Velocity(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)); } @Override - public void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) { - state.feederTargetVelocity = linearVelocity; - // (IN / SEC) * (ROT / IN) = (ROT / SEC) - double motorSpeed = linearVelocity.in(InchesPerSecond) / ShooterConstants.FEEDER_INCHES_PER_ROT; - VelocityDutyCycle velRequest = new VelocityDutyCycle(motorSpeed); - m_feederMotor.setControl(velRequest); + 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)); } @Override public void updateInputs(ShooterState state) { - // state.shooterAngle = m_angleMotor.getPosition().getValue().times(ShooterConstants.ANGLE_MOTOR_GEAR_RATIO); - // state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue(); - // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); - // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); + state.motor1Velocity = m_shooter1Motor.getVelocity().getValue(); + state.motor2Velocity = m_shooter2Motor.getVelocity().getValue(); + state.indexerVelocity = m_indexerMotor.getVelocity().getValue(); - state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue(); - state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue(); - - state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); - state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue(); + 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(); } }