Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -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/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);
}
}