mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Intake bp
bp for intake with setmode method
This commit is contained in:
Michael Mikovsky
@@ -0,0 +1,78 @@
|
||||
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<Pose2d> m_swervePoseSupplier;
|
||||
|
||||
public Intake(
|
||||
IntakeIO io,
|
||||
Supplier<Pose2d> swervePoseSupplier
|
||||
) {
|
||||
this.io = io;
|
||||
this.m_swervePoseSupplier = swervePoseSupplier;
|
||||
}
|
||||
|
||||
public enum IntakeMode {
|
||||
Up,
|
||||
Down,
|
||||
}
|
||||
|
||||
public void setMode(IntakeMode mode) {
|
||||
switch (mode) {
|
||||
case Up:
|
||||
io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
|
||||
break;
|
||||
case Down:
|
||||
io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
|
||||
io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,80 @@
|
||||
package frc4388.robot.subsystems.intake;
|
||||
|
||||
import static edu.wpi.first.units.Units.Degrees;
|
||||
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.Slot1Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
|
||||
public class IntakeConstants {
|
||||
// Motor conversions
|
||||
|
||||
public static final double ARM_MOTOR_GEAR_RATIO = 1.;
|
||||
public static final double ROLLER_MOTOR_GEAR_RATIO = 1.;
|
||||
|
||||
// Limits
|
||||
|
||||
// 0 is the forward angle on the robot.
|
||||
// negative is left, positive is right
|
||||
public static final Angle ARM_LIMIT_LOWER = Degrees.of(-180);
|
||||
public static final Angle ARM_LIMIT_UPPER = Degrees.of(180);
|
||||
public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(0.0);
|
||||
|
||||
public static final Slot0Configs ARM_PID = new Slot0Configs()
|
||||
.withKP(2.0)
|
||||
.withKI(0.0)
|
||||
.withKD(10.0);
|
||||
|
||||
public static final Slot1Configs ROLLER_PID = new Slot1Configs()
|
||||
.withKP(2.0)
|
||||
.withKI(0.0)
|
||||
.withKD(10.0);
|
||||
|
||||
// 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 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 class IDs {
|
||||
public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22);
|
||||
}
|
||||
// public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
// .withCurrentLimits(
|
||||
// new CurrentLimitsConfigs()
|
||||
// .withStatorCurrentLimit(40) // TODO: tune???
|
||||
// .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
|
||||
// ).withMotorOutput(
|
||||
// new MotorOutputConfigs()
|
||||
// .withNeutralMode(NeutralModeValue.Brake) // Brake so it stop
|
||||
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
// );
|
||||
public static final TalonFXConfiguration ROLLER_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
|
||||
);
|
||||
}
|
||||
@@ -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 armAngle = Rotations.of(0);
|
||||
Angle armTargetAngle = Rotations.of(0);
|
||||
Current armMotorCurrent = Amps.of(0);
|
||||
|
||||
// Angle shooterPitch = Rotations.of(0);
|
||||
// Angle shooterTargetPitch = Rotations.of(0);
|
||||
// Current pitchMotorCurrent = Amps.of(0);
|
||||
|
||||
AngularVelocity rollerVelocity = RotationsPerSecond.of(0);
|
||||
AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0);
|
||||
Current rollerMotorCurrent = Amps.of(0);
|
||||
|
||||
// LinearVelocity feederVelocity = InchesPerSecond.of(0);
|
||||
// LinearVelocity feederTargetVelocity = InchesPerSecond.of(0);
|
||||
// Current feederMotorCurrent = Amps.of(0);
|
||||
}
|
||||
|
||||
// public default void setShooterAngle(ShooterState state, Angle angle) {}
|
||||
// public default void setShooterPitch(ShooterState state, Angle angle) {}
|
||||
public default void setArmAngle(IntakeState state, Angle angle) {}
|
||||
public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {}
|
||||
|
||||
public default void updateInputs(IntakeState state) {}
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
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.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.controls.PositionDutyCycle;
|
||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.units.measure.*;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
|
||||
public class IntakeReal implements IntakeIO {
|
||||
|
||||
|
||||
TalonFX m_armMotor;
|
||||
TalonFX m_rollerMotor;
|
||||
|
||||
public IntakeReal(
|
||||
|
||||
TalonFX armMotor,
|
||||
TalonFX rollerMotor
|
||||
) {
|
||||
// m_angleMotor = angleMotor;
|
||||
// m_pitchMotor = pitchMotor;
|
||||
m_armMotor = armMotor;
|
||||
m_rollerMotor = rollerMotor;
|
||||
|
||||
// Apply the configs
|
||||
m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID);
|
||||
m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG);
|
||||
m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID);
|
||||
m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {
|
||||
state.rollerTargetVelocity = angularVelocity;
|
||||
// (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC)
|
||||
double motorSpeed = angularVelocity.in(RotationsPerSecond) / IntakeConstants.ROLLER_MOTOR_GEAR_RATIO;
|
||||
VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed);
|
||||
m_rollerMotor.setControl(velocity);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setArmAngle(IntakeState state, Angle angle) {
|
||||
state.armTargetAngle = 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.ARM_LIMIT_LOWER, IntakeConstants.ARM_LIMIT_UPPER);
|
||||
// (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
|
||||
double motorTargetAngle = boundedAngle.in(Rotations) / IntakeConstants.ARM_MOTOR_GEAR_RATIO;
|
||||
PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
|
||||
m_armMotor.setControl(posRequest);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateInputs(IntakeState state) {
|
||||
state.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
|
||||
state.armMotorCurrent = m_armMotor.getStatorCurrent(false).getValue();
|
||||
|
||||
// state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
|
||||
// state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
|
||||
|
||||
// state.armAngle = m_armMotor.getPosition().getValue();
|
||||
// state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue();
|
||||
|
||||
state.rollerVelocity = m_rollerMotor.getVelocity().getValue();
|
||||
state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user