Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-08 16:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2 from Team4388/Intake-boilerplate

Browse Source 
Intake bp
This commit is contained in:
Shikhar Dey
2026-01-27 18:28:57 -08:00
committed by GitHub
parent 3a2f2dd99b 7fe101d285
commit e2797ab778
5 changed files with 127 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/constants/BuildConstants.java
+6 -6
View File
@@ -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 = 14;
public static final String GIT_SHA = "eb0d9de8d8342dc2b039ff2c80c2750776fa088f";
public static final String GIT_DATE = "2026-01-19 19:42:29 MST";
public static final String GIT_BRANCH = "Autorotate";
public static final String BUILD_DATE = "2026-01-20 19:19:56 MST";
public static final long BUILD_UNIX_TIME = 1768961996020L;
public static final int GIT_REVISION = 15;
public static final String GIT_SHA = "ba16f53d6d8155249e7ca767b54fbfa1f17ce68e";
public static final String GIT_DATE = "2026-01-20 19:23:14 MST";
public static final String GIT_BRANCH = "Intake-boilerplate";
public static final String BUILD_DATE = "2026-01-27 18:56:30 MST";
public static final long BUILD_UNIX_TIME = 1769565390510L;
public static final int DIRTY = 1;
private BuildConstants(){}
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+30 -10
View File
@@ -25,20 +25,38 @@ public class Intake extends SubsystemBase {
this.m_swervePoseSupplier = swervePoseSupplier;
}
public enum FieldZone {
// The robot should aim at the hub
InShootZone,
// The robot should aim towards the wall
AimAtWall,
public enum IntakeMode {
Up,
Down,
}
// 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;
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() {
@@ -56,3 +74,5 @@ public class Intake extends SubsystemBase {
}
}
src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java
+39 -36
View File
@@ -1,50 +1,49 @@
package frc4388.robot.subsystems.intake;
import static edu.wpi.first.units.Units.*;
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 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 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 ANGLE_LIMIT_LEFT = Degrees.of(-180);
// public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180);
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);
// 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()
public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
@@ -54,17 +53,21 @@ public class IntakeConstants {
.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()
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???
src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java
+15 -15
View File
@@ -16,27 +16,27 @@ 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 armAngle = Rotations.of(0);
Angle armTargetAngle = Rotations.of(0);
Current armMotorCurrent = Amps.of(0);
// Angle IntakePitch = Rotations.of(0);
// Angle IntakeTargetPitch = Rotations.of(0);
// Angle shooterPitch = Rotations.of(0);
// 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 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);
// 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 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) {}
}
src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java
+37 -60
View File
@@ -4,35 +4,37 @@ 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_angleMotor;
// TalonFX m_pitchMotor;
TalonFX m_flywheelMotor;
TalonFX m_feederMotor;
TalonFX m_armMotor;
TalonFX m_rollerMotor;
public IntakeReal(
// TalonFX angleMotor,
// TalonFX pitchMotor,
TalonFX flywheelMotor,
TalonFX feederMotor
TalonFX armMotor,
TalonFX rollerMotor
) {
// m_angleMotor = angleMotor;
// m_pitchMotor = pitchMotor;
m_flywheelMotor = flywheelMotor;
m_feederMotor = feederMotor;
m_armMotor = armMotor;
m_rollerMotor = rollerMotor;
// 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);
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){
@@ -45,67 +47,42 @@ public class IntakeReal implements IntakeIO {
}
}
// // 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;
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.FLYWHEEL_GEAR_RATIO;
double motorSpeed = angularVelocity.in(RotationsPerSecond) / IntakeConstants.ROLLER_MOTOR_GEAR_RATIO;
VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed);
m_feederMotor.setControl(velocity);
m_rollerMotor.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);
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.IntakeAngle = m_angleMotor.getPosition().getValue().times(IntakeConstants.ANGLE_MOTOR_GEAR_RATIO);
// state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue();
state.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
state.armMotorCurrent = m_armMotor.getStatorCurrent(false).getValue();
// state.IntakePitch = m_pitchMotor.getPosition().getValue().times(IntakeConstants.PITCH_MOTOR_GEAR_RATIO);
// state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
// state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue();
state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue();
// state.armAngle = m_armMotor.getPosition().getValue();
// state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue();
state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * IntakeConstants.FEEDER_INCHES_PER_ROT);
state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue();
state.rollerVelocity = m_rollerMotor.getVelocity().getValue();
state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue();
}
}