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

VIsion and Roller

Browse Source 
vision constants and implementation
roller percent output
operator controls update
This commit is contained in:
Michael Mikovsky
2026-02-17 18:53:26 -08:00
parent 7ec022bc7a
commit 3a5057fc41
11 changed files with 69 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+7 -7
View File
@@ -69,27 +69,27 @@ public class Intake extends SubsystemBase {
switch (mode) {
case Extended:
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get()));
io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get()));
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
break;
case Retracted:
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
io.setRollerVelocity(state, RotationsPerSecond.of(0));
io.setRollerOutput(state, 0);
break;
case Extending:
io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get()));
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
break;
case Retracting:
io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get());
io.setRollerVelocity(state, RotationsPerSecond.of(0));
io.setRollerOutput(state, 0);
break;
case Idle:
io.stopArm();
break;
}
if (state.retractedLimit){
this.mode = IntakeMode.Retracted;
}
// if (state.retractedLimit){
// this.mode = IntakeMode.Retracted;
// }
}
}
src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java
+5 -10
View File
@@ -33,11 +33,11 @@ public class IntakeConstants {
// public static final Angle ARM_LIMIT_LOWER = Degrees.of(90);
// public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", -0.3*0);
public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0);
public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0);
public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0*0.4);
public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.1);
public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.1);
public static final ConfigurableDouble ROLLER_ACTIVE = new ConfigurableDouble("Rolelr Active", 10);
public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .10);
// public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25);
// public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0);
@@ -49,18 +49,13 @@ public class IntakeConstants {
.withKI(0.0)
.withKD(0.0);
public static final Slot0Configs ROLLER_PID = new Slot0Configs()
.withKP(0.2)
.withKI(0.0)
.withKD(0.0);
public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.1);
public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0);
public static ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2);
public static ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0);
public static ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0);
// 0 is paralell to the ground, 90 is directly up
src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java
+3 -3
View File
@@ -22,8 +22,8 @@ public interface IntakeIO {
// Angle shooterTargetPitch = Rotations.of(0);
// Current pitchMotorCurrent = Amps.of(0);
AngularVelocity rollerVelocity = RotationsPerSecond.of(0);
AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0);
double rollerOutput = 0;
double rollerTargetOutput = 0;
Current rollerMotorCurrent = Amps.of(0);
// LinearVelocity feederVelocity = InchesPerSecond.of(0);
@@ -35,7 +35,7 @@ public interface IntakeIO {
// public default void setShooterPitch(ShooterState state, Angle angle) {}
public default void setArmAngle(IntakeState state, Angle angle) {}
public default void stopArm() {}
public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {}
public default void setRollerOutput(IntakeState state, double rollerOutput) {}
public default void armOutput(double percentOutput) {}
public default void updateInputs(IntakeState state) {}
public default void updateGains() {}
src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java
+14 -19
View File
@@ -19,7 +19,7 @@ public class IntakeReal implements IntakeIO {
DigitalInput m_armLimitSwitch;
PositionDutyCycle armPosition = new PositionDutyCycle(0);
VelocityDutyCycle rollerVelocity = new VelocityDutyCycle(0);
// VelocityDutyCycle rollerVelocity = new VelocityDutyCycle(0);
public IntakeReal(
DigitalInput armLimitSwitch,
@@ -35,11 +35,10 @@ public class IntakeReal implements IntakeIO {
// 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);
armPosition.Slot = 0;
rollerVelocity.Slot = 0;
// rollerVelocity.Slot = 0;
}
private Angle clampAng(Angle x, Angle min, Angle max){
@@ -55,20 +54,15 @@ public class IntakeReal implements IntakeIO {
@Override
public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {
state.rollerTargetVelocity = angularVelocity;
public void setRollerOutput(IntakeState state, double rollerOutput) {
state.rollerTargetOutput = rollerOutput;
if(angularVelocity.baseUnitMagnitude() == 0) {
if(rollerOutput == 0) {
m_rollerMotor.set(0);
return;
}
// (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC)
AngularVelocity motorSpeed = angularVelocity.times(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO);
// m_rollerMotor.set(motorSpeed);
// VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed);
m_rollerMotor.setControl(rollerVelocity.withVelocity(motorSpeed));
m_rollerMotor.set(rollerOutput);
}
@Override
@@ -109,9 +103,14 @@ public class IntakeReal implements IntakeIO {
public void updateInputs(IntakeState state) {
state.armAngle = m_armMotor.getPosition().getValue().div(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue();
state.retractedLimit = !m_armLimitSwitch.get();
state.rollerVelocity = m_rollerMotor.getVelocity().getValue().div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO);
state.rollerOutput = m_rollerMotor.get();
state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue();
state.retractedLimit = !m_armLimitSwitch.get();
if(state.retractedLimit) {
// Set the arm motor to be zero if the limit switch is pressed
m_armMotor.setPosition(0., 0);
}
}
@Override
@@ -122,9 +121,5 @@ public class IntakeReal implements IntakeIO {
IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get();
m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID);
IntakeConstants.ROLLER_PID.kP = IntakeConstants.roller_kP.get();
IntakeConstants.ROLLER_PID.kI = IntakeConstants.roller_kI.get();
IntakeConstants.ROLLER_PID.kD = IntakeConstants.roller_kD.get();
m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID);
}
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+4 -10
View File
@@ -25,8 +25,8 @@ public class ShooterConstants {
public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -35);
public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", -10);
public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD Velocity", -10);
public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse Velocity", 10);
public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.1);
public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.1);
// Tolerances
@@ -48,16 +48,10 @@ public class ShooterConstants {
.withKI(0.1)
.withKD(0.08);
public static Slot0Configs INDEXER_PID = new Slot0Configs()
.withKV(0.0)
.withKP(0.0)
.withKI(0.0)
.withKD(0.2);
public static ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2);
public static ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KI", 0);
public static ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KD", 0);
public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08);
public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.1);
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+2 -8
View File
@@ -13,7 +13,7 @@ public class ShooterReal implements ShooterIO {
VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0);
VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
// VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
public ShooterReal(
@@ -27,7 +27,6 @@ public class ShooterReal implements ShooterIO {
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG);
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG);
@@ -35,7 +34,7 @@ public class ShooterReal implements ShooterIO {
shooter1Velocity.Slot = 0;
shooter2Velocity.Slot = 0;
m_indexerVelocity.Slot = 0;
// m_indexerVelocity.Slot = 0;
}
@Override
@@ -95,11 +94,6 @@ public class ShooterReal implements ShooterIO {
ShooterConstants.SHOOTER_PID.kD = ShooterConstants.shooter_kD.get();
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
ShooterConstants.INDEXER_PID.kP = ShooterConstants.indexer_kP.get();
ShooterConstants.INDEXER_PID.kI = ShooterConstants.indexer_kI.get();
ShooterConstants.INDEXER_PID.kD = ShooterConstants.indexer_kD.get();
m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_PID);
}
}