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

Adding trench autos and linear model for roller

Browse Source
This commit is contained in:
mimigamin
2026-04-04 10:27:44 -06:00
parent bf12e9cd7c
commit 0a2559bb4b
9 changed files with 443 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+9 -5
View File
@@ -2,7 +2,6 @@ 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;
@@ -11,19 +10,23 @@ import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.Utils;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.subsystems.swerve.SwerveDrive;
public class Intake extends SubsystemBase {
public IntakeIO io;
IntakeStateAutoLogged state = new IntakeStateAutoLogged();
SwerveDrive m_SwerveDrive;
Supplier<Pose2d> m_swervePoseSupplier;
public Intake(
IntakeIO io
IntakeIO io,
SwerveDrive m_SwerveDrive
// Supplier<Pose2d> swervePoseSupplier
) {
this.io = io;
this.m_SwerveDrive = m_SwerveDrive;
// this.m_swervePoseSupplier = swervePoseSupplier;
}
@@ -92,8 +95,9 @@ public class Intake extends SubsystemBase {
public void periodic() {
// FaultReporter.register(this); // TODO Implement fault reporter
// System.out.println(m_armLimitSwitch.get());
ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
double ChassisOverallSpeed = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond);
Logger.processInputs("Intake", state);
Logger.recordOutput("Intake/IntakeState", this.mode);
@@ -127,7 +131,7 @@ public class Intake extends SubsystemBase {
case ExtendingRolling:
io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
io.setRollerOutput(state, IntakeConstants. ROLLER_PERCENT_OUTPUT.get()); //getTargetRollerSpeed(ChassisOverallSpeed));
break;
case Retracting:
src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java
+8 -2
View File
@@ -6,6 +6,9 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.revrobotics.spark.FeedbackSensor;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.units.measure.AngularVelocity;
import com.revrobotics.spark.config.LimitSwitchConfig.Behavior;
import com.revrobotics.spark.config.LimitSwitchConfig.Type;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -59,8 +62,9 @@ public class IntakeConstants {
public static final ConfigurableDouble ARM_REVERSE_ROLLER_RANGE = new ConfigurableDouble("Arm reverse roller range", 1.17);
public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .80);
public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .50);
public static final ConfigurableDouble ROLLER_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Roller Retract Output", .40);
public static final ConfigurableDouble ROLLER_MULTIPLIER_CONST = new ConfigurableDouble("Roller Multiplier Constant", 0.4);
// public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25);
@@ -79,7 +83,9 @@ public class IntakeConstants {
// public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
// public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0.05);
public static double getTargetRollerSpeed(double chassisSpeed) {
return ROLLER_PERCENT_OUTPUT.get() * (1 + ROLLER_MULTIPLIER_CONST.get() * chassisSpeed);
}
// 0 is paralell to the ground, 90 is directly up