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

Intake code ready for robot

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-09 16:03:48 -08:00
parent 684e18ddcc
commit d90bddac0f
6 changed files with 61 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotMap.java
+9 -17
View File
@@ -9,17 +9,10 @@ package frc4388.robot;
import com.ctre.phoenix6.hardware.TalonFX;
import org.photonvision.PhotonCamera;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DigitalInput;
//import frc4388.robot.constants.Constants.ElevatorConstants;
import frc4388.robot.constants.Constants.SimConstants;
import frc4388.robot.constants.Constants.VisionConstants;
import frc4388.robot.subsystems.intake.IntakeConstants;
import frc4388.robot.subsystems.intake.IntakeIO;
import frc4388.robot.subsystems.intake.IntakeReal;
@@ -31,10 +24,7 @@ import frc4388.robot.subsystems.shooter.ShooterReal;
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
import frc4388.robot.subsystems.swerve.SwerveIO;
import frc4388.robot.subsystems.swerve.SwerveReal;
import frc4388.robot.subsystems.vision.VisionIO;
import frc4388.robot.subsystems.vision.VisionReal;
import frc4388.utility.status.FaultCANCoder;
import frc4388.utility.status.FaultPhotonCamera;
import frc4388.utility.status.FaultPidgeon2;
import frc4388.utility.status.FaultTalonFX;
@@ -93,9 +83,9 @@ public class RobotMap {
// Configure Shooter 22,23,24
TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id);
TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id);
TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id);
// TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id);
// TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id);
// TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id);
//Configure Intake 20,21
TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id);
@@ -105,7 +95,9 @@ public class RobotMap {
// DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
// DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
shooterIO = new ShooterReal(shooter1, shooter2, indexer);
shooterIO = new ShooterIO() {};
// shooterIO = new ShooterReal(shooter1, shooter2, indexer);
intakeIO = new IntakeReal(arm, roller);
@@ -113,9 +105,9 @@ public class RobotMap {
FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
FaultTalonFX.addDevice(shooter1, "Shooter1");
FaultTalonFX.addDevice(shooter2, "Shooter2");
FaultTalonFX.addDevice(indexer, "Indexer");
// FaultTalonFX.addDevice(shooter1, "Shooter1");
// FaultTalonFX.addDevice(shooter2, "Shooter2");
// FaultTalonFX.addDevice(indexer, "Indexer");
FaultTalonFX.addDevice(arm, "Arm");
FaultTalonFX.addDevice(roller, "Roller");
src/main/java/frc4388/robot/constants/BuildConstants.java
+7 -7
View File
@@ -7,13 +7,13 @@ 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 = 34;
public static final String GIT_SHA = "2d9ed527bebbf79d7643452fe4344ad32b3fab49";
public static final String GIT_DATE = "2026-02-07 14:51:05 MST";
public static final String GIT_BRANCH = "Subsystem-Boilerplate";
public static final String BUILD_DATE = "2026-02-07 14:51:24 MST";
public static final long BUILD_UNIX_TIME = 1770501084043L;
public static final int DIRTY = 0;
public static final int GIT_REVISION = 35;
public static final String GIT_SHA = "684e18ddcc5438f14f05c5902506d14cc4cbab19";
public static final String GIT_DATE = "2026-02-07 14:52:21 MST";
public static final String GIT_BRANCH = "arm-test";
public static final String BUILD_DATE = "2026-02-09 16:43:56 MST";
public static final long BUILD_UNIX_TIME = 1770680636562L;
public static final int DIRTY = 1;
private BuildConstants(){}
}
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+2 -8
View File
@@ -33,11 +33,11 @@ public class Intake extends SubsystemBase {
public void setMode(IntakeMode mode) {
switch (mode) {
case Up:
// io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP);
break;
case Down:
// io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY);
break;
}
@@ -60,17 +60,11 @@ public class Intake extends SubsystemBase {
@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);
}
src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java
+5 -18
View File
@@ -17,14 +17,14 @@ 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.;
public static final double ARM_MOTOR_GEAR_RATIO = 1/100;
public static final double ROLLER_MOTOR_GEAR_RATIO = 1/3;
//IDs
public static final CanDevice ARM_ID = new CanDevice("SHOOTER 1", 20);
public static final CanDevice ROLLER_ID = new CanDevice("SHOOTER 2", 21);
public static final CanDevice ARM_ID = new CanDevice("ARM", 20);
public static final CanDevice ROLLER_ID = new CanDevice("INTAKE_ROLLER", 21);
// Limits
@@ -44,7 +44,7 @@ public class IntakeConstants {
.withKI(0.0)
.withKD(10.0);
public static final Slot1Configs ROLLER_PID = new Slot1Configs()
public static final Slot0Configs ROLLER_PID = new Slot0Configs()
.withKP(2.0)
.withKI(0.0)
.withKD(10.0);
@@ -65,19 +65,6 @@ public class IntakeConstants {
.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()
src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java
+35 -14
View File
@@ -20,6 +20,9 @@ public class IntakeReal implements IntakeIO {
TalonFX m_armMotor;
TalonFX m_rollerMotor;
PositionDutyCycle armPosition = new PositionDutyCycle(0);
VelocityDutyCycle rollerVelocity = new VelocityDutyCycle(0);
public IntakeReal(
TalonFX armMotor,
@@ -35,6 +38,9 @@ public class IntakeReal implements IntakeIO {
m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG);
m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID);
m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG);
armPosition.Slot = 0;
rollerVelocity.Slot = 0;
}
private Angle clampAng(Angle x, Angle min, Angle max){
@@ -53,9 +59,11 @@ public class IntakeReal implements IntakeIO {
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);
AngularVelocity motorSpeed = angularVelocity.div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO);
// m_rollerMotor.set(motorSpeed);
// VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed);
m_rollerMotor.setControl(rollerVelocity.withVelocity(motorSpeed));
}
@Override
@@ -63,26 +71,39 @@ public class IntakeReal implements IntakeIO {
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);
Angle motorAngle = angle.div(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
// PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
m_armMotor.setControl(armPosition.withPosition(motorAngle));
}
ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2);
ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KP", 0);
ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KP", 0);
ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2);
ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0);
ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0);
@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();
IntakeConstants.ARM_PID.kP = arm_kP.get();
IntakeConstants.ARM_PID.kI = arm_kI.get();
IntakeConstants.ARM_PID.kD = arm_kD.get();
m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG);
IntakeConstants.ROLLER_PID.kP = roller_kP.get();
IntakeConstants.ROLLER_PID.kI = roller_kI.get();
IntakeConstants.ROLLER_PID.kD = roller_kD.get();
m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG);
}
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+3 -6
View File
@@ -3,14 +3,11 @@ package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.InchesPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX;
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 {
@@ -27,9 +24,9 @@ public class ShooterReal implements ShooterIO {
TalonFX shooter2Motor,
TalonFX indexerMotor
) {
m_shooter1Motor= shooter1Motor;
m_shooter2Motor= shooter2Motor;
m_indexerMotor = indexerMotor;
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);