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

Add bounce code

Browse Source
This commit is contained in:
Michael Mikovsky
2026-03-25 11:15:46 -06:00
parent d639b3076d
commit c6b0d29acd
4 changed files with 57 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+35 -12
View File
@@ -1,10 +1,15 @@
package frc4388.robot.subsystems.intake;
import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.Utils;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -25,16 +30,23 @@ public class Intake extends SubsystemBase {
public enum IntakeMode {
Extended,
Retracted,
Extending,
Retracting,
Idle,
RollerStop
Bouncing
}
private IntakeMode mode = IntakeMode.Idle;
public void setMode(IntakeMode mode) {
this.mode = mode;
switch (mode) {
case Bouncing:
// When bounce is enabled: set the bounce timer
this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.BOUNCE_HALF_PERIOD;
break;
default:
break;
}
}
public IntakeMode getMode() {
@@ -78,6 +90,8 @@ public class Intake extends SubsystemBase {
io.updateInputs(state);
// getCurrentTime
switch (mode) {
case Extended:
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get()));
@@ -87,18 +101,27 @@ public class Intake extends SubsystemBase {
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
io.setRollerOutput(state, 0);
break;
case Extending:
io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
break;
case Retracting:
io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get());
case Bouncing:
io.setRollerOutput(state, 0);
if(
state.armMotorCurrent.in(Amps) < IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get() &&
state.armMotorVelocity.in(RotationsPerSecond) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get()
) {
this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.BOUNCE_HALF_PERIOD;
}
// Get the time delta from the last bounce time update
double currentTime = Utils.getSystemTimeSeconds() - state.currentBounceTime;
// Get the percentage through the bounce period (0 output means one half period has passed)
double percentOutput = (currentTime / IntakeConstants.BOUNCE_HALF_PERIOD) * IntakeConstants.INTAKE_BOUNCE_OUTPUT;
// Clamp the output of the motor to some value
percentOutput = Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT);
io.armOutput(percentOutput);
break;
case Idle:
io.stopArm();
break;
case RollerStop:
// io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
io.setRollerOutput(state, 0);
break;
}