mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 08:48:05 -06:00
intake start
This commit is contained in:
Shikhar
@@ -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<Pose2d> m_swervePoseSupplier;
|
||||
|
||||
public Intake(
|
||||
IntakeIO io,
|
||||
Supplier<Pose2d> 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);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
);
|
||||
}
|
||||
@@ -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) {}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user