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..87faa80 --- /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.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_intakeMotor; + 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 intakeMotor, + TalonFX outerShooter, + TalonFX innerShooter + ) { + // m_intakeMotor = intakeMotor; + // m_armMotor = armMotor; + m_intakeMotor = intakeMotor; + m_outerShooter = outerShooter; + m_innerShooter = innerShooter; + + // m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG); + // m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG); + m_intakeMotor.getConfigurator().apply(INTAKE_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 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 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(); + } + +}