mirror of
https://github.com/Team4388/2024AcrossTheRidgebotiverse.git
synced 2026-06-09 08:48:03 -06:00
Code Cleanup, Remove old autos in container. Refactor Intake to be talon only.
This commit is contained in:
C4llSiqn
@@ -4,116 +4,30 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonSRXPIDSetConfiguration;
|
||||
import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.controls.PositionVoltage;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
|
||||
import com.ctre.phoenix6.signals.ForwardLimitValue;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
|
||||
import com.ctre.phoenix6.signals.ReverseLimitValue;
|
||||
import com.revrobotics.CANSparkBase;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.SparkLimitSwitch;
|
||||
import com.revrobotics.SparkPIDController;
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import edu.wpi.first.wpilibj.CAN;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Talon;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants;
|
||||
import frc4388.robot.Constants.IntakeConstants;
|
||||
import frc4388.robot.commands.PID;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
|
||||
public class Intake extends SubsystemBase {
|
||||
|
||||
//NEO
|
||||
private CANSparkMax intakeMotor;
|
||||
private CANSparkMax pivot;
|
||||
private SparkPIDController m_spedController;
|
||||
private SparkLimitSwitch forwardLimit;
|
||||
private SparkLimitSwitch reverseLimit;
|
||||
private SparkLimitSwitch intakeforwardLimit;
|
||||
private SparkLimitSwitch intakereverseLimit;
|
||||
|
||||
//Talon
|
||||
private TalonFX talonIntake;
|
||||
private TalonFX talonPivot;
|
||||
private CANcoder encoder;
|
||||
|
||||
private boolean r;
|
||||
|
||||
private HardwareLimitSwitchConfigs l;
|
||||
|
||||
TalonFXConfiguration doodooController = new TalonFXConfiguration();
|
||||
|
||||
private TalonFX intakeMotor;
|
||||
private TalonFX pivotMotor;
|
||||
public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS;
|
||||
private ConfigurableDouble outtakeSpeed = new ConfigurableDouble("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
|
||||
private BooleanSupplier sup = () -> true;
|
||||
private BooleanSupplier dup = () -> false;
|
||||
|
||||
private double smartDashboardOuttakeValue;
|
||||
|
||||
/** Creates a new Intake. */
|
||||
//For NEO
|
||||
// public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) {
|
||||
// this.intakeMotor = intakeMotor;
|
||||
// this.pivot = pivot;
|
||||
public Intake(TalonFX intakeMotor, TalonFX pivotMotor) {
|
||||
this.intakeMotor = intakeMotor;
|
||||
this.pivotMotor = pivotMotor;
|
||||
|
||||
// pivot.restoreFactoryDefaults();
|
||||
// //pivot.setInverted(true);
|
||||
intakeMotor.getConfigurator().apply(new TalonFXConfiguration());
|
||||
pivotMotor.getConfigurator().apply(new TalonFXConfiguration());
|
||||
|
||||
// forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
|
||||
// reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
|
||||
// forwardLimit.enableLimitSwitch(true);
|
||||
// reverseLimit.enableLimitSwitch(true);
|
||||
|
||||
// intakeMotor.restoreFactoryDefaults();
|
||||
|
||||
// intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
|
||||
// intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
|
||||
// intakeforwardLimit.enableLimitSwitch(true);
|
||||
// intakereverseLimit.enableLimitSwitch(false);
|
||||
|
||||
// //Arm PID
|
||||
// m_spedController = pivot.getPIDController();
|
||||
// m_spedController.setP(armGains.kP);
|
||||
// m_spedController.setI(armGains.kI);
|
||||
// m_spedController.setD(armGains.kD);
|
||||
|
||||
// SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
|
||||
// }
|
||||
|
||||
//For Talon
|
||||
public Intake(TalonFX talonIntake, TalonFX talonPivot) {
|
||||
this.talonIntake = talonIntake;
|
||||
this.talonPivot = talonPivot;
|
||||
|
||||
talonIntake.getConfigurator().apply(new TalonFXConfiguration());
|
||||
talonPivot.getConfigurator().apply(new TalonFXConfiguration());
|
||||
|
||||
talonIntake.setNeutralMode(NeutralModeValue.Brake);
|
||||
talonPivot.setNeutralMode(NeutralModeValue.Brake);
|
||||
|
||||
// talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs());
|
||||
// talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs());
|
||||
|
||||
|
||||
|
||||
// doodooController.Slot0.kP = armGains.kP;
|
||||
// doodooController.Slot1.kI = armGains.kI;
|
||||
// doodooController.Slot2.kD = armGains.kD;
|
||||
intakeMotor.setNeutralMode(NeutralModeValue.Brake);
|
||||
pivotMotor.setNeutralMode(NeutralModeValue.Brake);
|
||||
|
||||
// in init function, set slot 0 gains
|
||||
var slot0Configs = new Slot0Configs();
|
||||
@@ -121,222 +35,76 @@ public class Intake extends SubsystemBase {
|
||||
slot0Configs.kI = 0.0; // no output for integrated error
|
||||
slot0Configs.kD = 0.21; // A velocity of 1 rps results in 0.1 V output
|
||||
|
||||
talonPivot.getConfigurator().apply(slot0Configs);
|
||||
|
||||
|
||||
pivotMotor.getConfigurator().apply(slot0Configs);
|
||||
}
|
||||
|
||||
// ! Talon Methods
|
||||
public void talonPIDIn() {
|
||||
PositionVoltage request = new PositionVoltage(-53);
|
||||
talonPivot.setControl(request.withPosition(0));
|
||||
public void PIDIn() {
|
||||
PIDPosition(0);
|
||||
}
|
||||
|
||||
public void talonPIDOut() {
|
||||
PositionVoltage request = new PositionVoltage(0);
|
||||
talonPivot.setControl(request.withPosition(-53));
|
||||
public void PIDOut() {
|
||||
PIDPosition(-53);
|
||||
}
|
||||
|
||||
public void talonPIDPosition(double out2) {
|
||||
PositionVoltage request = new PositionVoltage(out2);
|
||||
talonPivot.setControl(request);
|
||||
public void PIDPosition(double pos) {
|
||||
PositionVoltage request = new PositionVoltage(pos);
|
||||
pivotMotor.setControl(request);
|
||||
}
|
||||
|
||||
public void talonHandoff() {
|
||||
talonIntake.set(-outtakeSpeed.get());
|
||||
public void handoff() {
|
||||
intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
|
||||
}
|
||||
|
||||
public void talonSpinIntakeMotor() {
|
||||
talonIntake.set(IntakeConstants.INTAKE_SPEED);
|
||||
public void spinIntakeMotor() {
|
||||
intakeMotor.set(IntakeConstants.INTAKE_SPEED);
|
||||
}
|
||||
|
||||
public void talonSpinIntakeMotor(double speed) {
|
||||
talonIntake.set(speed);
|
||||
public void spinIntakeMotor(double speed) {
|
||||
intakeMotor.set(speed);
|
||||
}
|
||||
|
||||
public boolean getTalonIntakeLimitSwitchState() {
|
||||
if(r = talonIntake.getForwardLimit().getValue().value == 0) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
public boolean getIntakeLimitSwitchState() {
|
||||
return intakeMotor.getForwardLimit().getValue().value == 0;
|
||||
}
|
||||
|
||||
public void talonSetPivotEncoderPosition(double val) {
|
||||
talonPivot.setPosition(val);
|
||||
public void setPivotEncoderPosition(double val) {
|
||||
pivotMotor.setPosition(val);
|
||||
}
|
||||
|
||||
public void talonStopIntakeMotors() {
|
||||
talonIntake.set(0);
|
||||
public void stopIntakeMotors() {
|
||||
intakeMotor.set(0);
|
||||
}
|
||||
|
||||
public void talonStopArmMotor() {
|
||||
talonPivot.set(0);
|
||||
public void stopArmMotor() {
|
||||
pivotMotor.set(0);
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
intakeMotor.set(0);
|
||||
pivotMotor.set(0);
|
||||
}
|
||||
|
||||
public double getArmPos() {
|
||||
return talonPivot.getPosition().getValue();
|
||||
return pivotMotor.getPosition().getValue();
|
||||
}
|
||||
|
||||
public void resetArmPosition() {
|
||||
if(getTalonIntakeLimitSwitchState()){
|
||||
if (getIntakeLimitSwitchState()) {
|
||||
// talonPivot.setPosition(0);
|
||||
}
|
||||
}
|
||||
|
||||
public void ampPosition() {
|
||||
PositionVoltage request = new PositionVoltage(-0);
|
||||
talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value
|
||||
PIDPosition(-59); //TODO: Find actual value
|
||||
}
|
||||
|
||||
public void ampOuttake(double speed) {
|
||||
talonSpinIntakeMotor(speed);
|
||||
spinIntakeMotor(speed);
|
||||
}
|
||||
|
||||
|
||||
// ! NEO Methods
|
||||
//hanoff
|
||||
// public void spinIntakeMotor() {
|
||||
// intakeMotor.set(IntakeConstants.INTAKE_SPEED);
|
||||
// }
|
||||
|
||||
// //Rotate robot in for handoff
|
||||
// public void rotateArmIn() {
|
||||
// pivot.set(IntakeConstants.PIVOT_SPEED);
|
||||
// }
|
||||
|
||||
// //Rotates robot out for intake
|
||||
// public void rotateArmOut() {
|
||||
// pivot.set(-IntakeConstants.PIVOT_SPEED);
|
||||
|
||||
// }
|
||||
|
||||
// public void pidIn() {
|
||||
// m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition);
|
||||
// //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity());
|
||||
// }
|
||||
|
||||
// public void pidOut() {
|
||||
// m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition);
|
||||
// }
|
||||
|
||||
// public void limitNote() {
|
||||
// if (intakeforwardLimit.isPressed()) {
|
||||
// rotateArmIn2();
|
||||
// } else {
|
||||
// spinIntakeMotor();
|
||||
// }
|
||||
// }
|
||||
|
||||
// public void rotateArmOut2() {
|
||||
// if(reverseLimit.isPressed()){
|
||||
// stopArmMotor();
|
||||
// } else {
|
||||
// pidOut();
|
||||
// }
|
||||
// }
|
||||
|
||||
// public void rotateArmIn2() {
|
||||
// if(forwardLimit.isPressed()){
|
||||
// stopArmMotor();
|
||||
// } else {
|
||||
// pidIn();
|
||||
// }
|
||||
// }
|
||||
|
||||
// public void handoff() {
|
||||
// intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
|
||||
// }
|
||||
|
||||
// public void handoff2() {
|
||||
// if(intakeforwardLimit.isPressed()) {
|
||||
// intakeMotor.set(-smartDashboardOuttakeValue);
|
||||
// } else {
|
||||
// intakeMotor.set(-smartDashboardOuttakeValue);
|
||||
// }
|
||||
// }
|
||||
|
||||
// public void stopIntakeMotors() {
|
||||
// intakeMotor.set(0);
|
||||
// }
|
||||
|
||||
// public void stopArmMotor() {
|
||||
// pivot.set(0);
|
||||
// }
|
||||
|
||||
// public RelativeEncoder getEncoder() {
|
||||
// return pivot.getEncoder();
|
||||
// }
|
||||
|
||||
// public boolean getForwardLimitSwitchState() {
|
||||
// return forwardLimit.isPressed();
|
||||
// }
|
||||
|
||||
// public boolean getReverseLimitSwitchState() {
|
||||
// return reverseLimit.isPressed();
|
||||
// }
|
||||
|
||||
// public boolean getIntakeLimitSwtichState() {
|
||||
// return intakeforwardLimit.isPressed();
|
||||
// }
|
||||
|
||||
// public void setVoltage(double voltage) {
|
||||
// pivot.setVoltage(voltage);
|
||||
// }
|
||||
|
||||
// public double getVelocity() {
|
||||
// return pivot.getEncoder().getVelocity();
|
||||
// }
|
||||
|
||||
// public void setPivotEncoderPosition(int val) {
|
||||
// pivot.getEncoder().setPosition(val);
|
||||
// }
|
||||
|
||||
// public void resetPosition() {
|
||||
// if(forwardLimit.isPressed()) {
|
||||
// setPivotEncoderPosition(0);
|
||||
// }
|
||||
// }
|
||||
|
||||
// public double getPos() {
|
||||
// return pivot.getEncoder().getPosition();
|
||||
// }
|
||||
|
||||
// public double getIntakeVelocity() {
|
||||
// return intakeMotor.getEncoder().getVelocity();
|
||||
// }
|
||||
|
||||
// public void rotateArm() {
|
||||
|
||||
// }
|
||||
|
||||
// public BooleanSupplier getArmFowardLimitState() {
|
||||
// if(forwardLimit.isPressed()) {
|
||||
// return sup;
|
||||
// } else {
|
||||
// return dup;
|
||||
// }
|
||||
// }
|
||||
|
||||
// public void changeIntakeNeutralState() {
|
||||
// if(forwardLimit.isPressed()) {
|
||||
// intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast);
|
||||
// }
|
||||
// }
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
// SmartDashboard.putNumber("Vel Output", getVelocity());
|
||||
// SmartDashboard.putNumber("Position", getPos());
|
||||
// resetPosition();
|
||||
// changeIntakeNeutralState();
|
||||
|
||||
resetArmPosition();
|
||||
|
||||
// SmartDashboard.putNumber("Pivot Position", getArmPos());
|
||||
|
||||
//smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
|
||||
|
||||
//SmartDashboard.putBoolean("Limit Switch State", getTalonIntakeLimitSwitchState());
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user