Team_2648/2024_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

intake subsystem (Brad ignore the timestamp)

Browse Source
This commit is contained in:
Tyler-J42 2024-02-03 02:07:36 -05:00
parent 215f982932
commit a60b0fd424
2 changed files with 76 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/main/java/frc/robot/constants/IntakeConstants.java Normal file
View File

@ -0,0 +1,23 @@
package frc.robot.constants;
public class IntakeConstants {
public static final int kIntakePivotID = 9;
public static final int kIntakeRollerID = 10;
public static final double kIntakePivotConversionFactor = 0;
public static final int kPivotCurrentLimit = 40;
public static final double kPIntake = 0;
public static final double kIIntake = 0;
public static final double kDIntake = 0;
public static final double kSFeedForward = 0;
public static final double kGFeedForward = 0;
public static final double kVFeedForward = 0;
public static final double kStartingAngle = Math.toRadians(110.0);
public static final double kUpAngle = Math.toRadians(100.0);
public static final double kDownAngle = Math.toRadians(-40.0);
}

53
src/main/java/frc/robot/subsystems/Intake.java Normal file
View File

@ -0,0 +1,53 @@
package frc.robot.subsystems;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.IntakeConstants;
public class Intake extends SubsystemBase{
private PIDController intakeAnglePID;
private CANSparkMax intakeRoller;
private CANSparkMax intakePivot;
private RelativeEncoder intakeEncoder;
private ArmFeedforward intakeFeedForward;
public Intake(){
intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless);
intakePivot = new CANSparkMax(IntakeConstants.kIntakePivotID, MotorType.kBrushless);
intakePivot.setIdleMode(IdleMode.kBrake);
intakePivot.setSmartCurrentLimit(IntakeConstants.kPivotCurrentLimit);
intakePivot.burnFlash();
intakeFeedForward = new ArmFeedforward(IntakeConstants.kSFeedForward, IntakeConstants.kGFeedForward, IntakeConstants.kVFeedForward);
intakeEncoder = intakePivot.getEncoder();
intakeEncoder.setPosition(IntakeConstants.kStartingAngle);
intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor);
intakeAnglePID = new PIDController(IntakeConstants.kPIntake, IntakeConstants.kIIntake, IntakeConstants.kDIntake);
}
public void intakeDown(){
intakeRoller.set(1.0);
intakePivot.setVoltage(intakeAnglePID.calculate(intakeEncoder.getPosition(), IntakeConstants.kDownAngle) + intakeFeedForward.calculate(IntakeConstants.kDownAngle, 0.0));
}
public void climbingState(){
intakeRoller.set(0.0);
intakePivot.setVoltage(intakeAnglePID.calculate(intakeEncoder.getPosition(), IntakeConstants.kDownAngle) + intakeFeedForward.calculate(IntakeConstants.kDownAngle, 0.0));
}
public void intakeUp(){
intakeRoller.set(0.0);
intakePivot.setVoltage(intakeAnglePID.calculate(intakeEncoder.getPosition(), IntakeConstants.kUpAngle) + intakeFeedForward.calculate(IntakeConstants.kUpAngle, 0.0));
}
}