mirror of
https://github.com/Team4388/2024AcrossTheRidgebotiverse.git
synced 2026-06-09 00:38:02 -06:00
AHHHHHHHHHHHHHHHHH (Im tweakin)
This commit is contained in:
Abhishrek05
@@ -51,6 +51,8 @@ public class Intake extends SubsystemBase {
|
||||
private TalonFX talonPivot;
|
||||
private CANcoder encoder;
|
||||
|
||||
private boolean r;
|
||||
|
||||
private HardwareLimitSwitchConfigs l;
|
||||
|
||||
TalonFXConfiguration doodooController = new TalonFXConfiguration();
|
||||
@@ -65,33 +67,33 @@ public class Intake extends SubsystemBase {
|
||||
|
||||
/** Creates a new Intake. */
|
||||
//For NEO
|
||||
public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) {
|
||||
this.intakeMotor = intakeMotor;
|
||||
this.pivot = pivot;
|
||||
// public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) {
|
||||
// this.intakeMotor = intakeMotor;
|
||||
// this.pivot = pivot;
|
||||
|
||||
pivot.restoreFactoryDefaults();
|
||||
//pivot.setInverted(true);
|
||||
// pivot.restoreFactoryDefaults();
|
||||
// //pivot.setInverted(true);
|
||||
|
||||
forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
|
||||
reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
|
||||
forwardLimit.enableLimitSwitch(true);
|
||||
reverseLimit.enableLimitSwitch(true);
|
||||
// forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
|
||||
// reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
|
||||
// forwardLimit.enableLimitSwitch(true);
|
||||
// reverseLimit.enableLimitSwitch(true);
|
||||
|
||||
intakeMotor.restoreFactoryDefaults();
|
||||
// intakeMotor.restoreFactoryDefaults();
|
||||
|
||||
intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
|
||||
intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
|
||||
intakeforwardLimit.enableLimitSwitch(true);
|
||||
intakereverseLimit.enableLimitSwitch(false);
|
||||
// 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);
|
||||
// //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);
|
||||
}
|
||||
// SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
|
||||
// }
|
||||
|
||||
//For Talon
|
||||
public Intake(TalonFX talonIntake, TalonFX talonPivot) {
|
||||
@@ -115,9 +117,9 @@ public class Intake extends SubsystemBase {
|
||||
|
||||
// in init function, set slot 0 gains
|
||||
var slot0Configs = new Slot0Configs();
|
||||
slot0Configs.kP = 0.05; // An error of 0.5 rotations results in 12 V output
|
||||
slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output
|
||||
slot0Configs.kI = 0.0; // no output for integrated error
|
||||
slot0Configs.kD = 0.0; // A velocity of 1 rps results in 0.1 V output
|
||||
slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output
|
||||
|
||||
talonPivot.getConfigurator().apply(slot0Configs);
|
||||
|
||||
@@ -126,13 +128,13 @@ public class Intake extends SubsystemBase {
|
||||
|
||||
// ! Talon Methods
|
||||
public void talonPIDIn() {
|
||||
PositionVoltage request = new PositionVoltage(0).withSlot(0);
|
||||
talonPivot.setControl(request.withPosition(53)); //TODO: Find actual value
|
||||
PositionVoltage request = new PositionVoltage(-59);
|
||||
talonPivot.setControl(request.withPosition(0)); //TODO: Find actual value
|
||||
}
|
||||
|
||||
public void talonPIDOut() {
|
||||
PositionVoltage request = new PositionVoltage(53).withSlot(53);
|
||||
talonPivot.setControl(request.withPosition(0)); //TODO: Find actual value
|
||||
PositionVoltage request = new PositionVoltage(0);
|
||||
talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value
|
||||
}
|
||||
|
||||
public void talonHandoff() {
|
||||
@@ -144,8 +146,10 @@ public class Intake extends SubsystemBase {
|
||||
}
|
||||
|
||||
public boolean getTalonIntakeLimitSwitchState() {
|
||||
var r = talonIntake.getForwardLimit().getValue().value == 1;
|
||||
return r;
|
||||
if(r = talonIntake.getForwardLimit().getValue().value == 0) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
public void talonSetPivotEncoderPosition(int val) {
|
||||
@@ -160,144 +164,158 @@ public class Intake extends SubsystemBase {
|
||||
talonPivot.set(0);
|
||||
}
|
||||
|
||||
public double getArmPos() {
|
||||
return talonPivot.getPosition().getValue();
|
||||
}
|
||||
|
||||
public void resetArmPosition() {
|
||||
if(getTalonIntakeLimitSwitchState()){
|
||||
// talonPivot.setPosition(0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// ! NEO Methods
|
||||
//hanoff
|
||||
public void spinIntakeMotor() {
|
||||
intakeMotor.set(IntakeConstants.INTAKE_SPEED);
|
||||
}
|
||||
// public void spinIntakeMotor() {
|
||||
// intakeMotor.set(IntakeConstants.INTAKE_SPEED);
|
||||
// }
|
||||
|
||||
//Rotate robot in for handoff
|
||||
public void rotateArmIn() {
|
||||
pivot.set(IntakeConstants.PIVOT_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);
|
||||
// //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 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 pidOut() {
|
||||
// m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition);
|
||||
// }
|
||||
|
||||
public void limitNote() {
|
||||
if (intakeforwardLimit.isPressed()) {
|
||||
rotateArmIn2();
|
||||
} else {
|
||||
spinIntakeMotor();
|
||||
}
|
||||
}
|
||||
// public void limitNote() {
|
||||
// if (intakeforwardLimit.isPressed()) {
|
||||
// rotateArmIn2();
|
||||
// } else {
|
||||
// spinIntakeMotor();
|
||||
// }
|
||||
// }
|
||||
|
||||
public void rotateArmOut2() {
|
||||
if(reverseLimit.isPressed()){
|
||||
stopArmMotor();
|
||||
} else {
|
||||
pidOut();
|
||||
}
|
||||
}
|
||||
// public void rotateArmOut2() {
|
||||
// if(reverseLimit.isPressed()){
|
||||
// stopArmMotor();
|
||||
// } else {
|
||||
// pidOut();
|
||||
// }
|
||||
// }
|
||||
|
||||
public void rotateArmIn2() {
|
||||
if(forwardLimit.isPressed()){
|
||||
stopArmMotor();
|
||||
} else {
|
||||
pidIn();
|
||||
}
|
||||
}
|
||||
// public void rotateArmIn2() {
|
||||
// if(forwardLimit.isPressed()){
|
||||
// stopArmMotor();
|
||||
// } else {
|
||||
// pidIn();
|
||||
// }
|
||||
// }
|
||||
|
||||
public void handoff() {
|
||||
intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
|
||||
}
|
||||
// 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 handoff2() {
|
||||
// if(intakeforwardLimit.isPressed()) {
|
||||
// intakeMotor.set(-smartDashboardOuttakeValue);
|
||||
// } else {
|
||||
// intakeMotor.set(-smartDashboardOuttakeValue);
|
||||
// }
|
||||
// }
|
||||
|
||||
public void stopIntakeMotors() {
|
||||
intakeMotor.set(0);
|
||||
}
|
||||
// public void stopIntakeMotors() {
|
||||
// intakeMotor.set(0);
|
||||
// }
|
||||
|
||||
public void stopArmMotor() {
|
||||
pivot.set(0);
|
||||
}
|
||||
// public void stopArmMotor() {
|
||||
// pivot.set(0);
|
||||
// }
|
||||
|
||||
public RelativeEncoder getEncoder() {
|
||||
return pivot.getEncoder();
|
||||
}
|
||||
// public RelativeEncoder getEncoder() {
|
||||
// return pivot.getEncoder();
|
||||
// }
|
||||
|
||||
public boolean getForwardLimitSwitchState() {
|
||||
return forwardLimit.isPressed();
|
||||
}
|
||||
// public boolean getForwardLimitSwitchState() {
|
||||
// return forwardLimit.isPressed();
|
||||
// }
|
||||
|
||||
public boolean getReverseLimitSwitchState() {
|
||||
return reverseLimit.isPressed();
|
||||
}
|
||||
// public boolean getReverseLimitSwitchState() {
|
||||
// return reverseLimit.isPressed();
|
||||
// }
|
||||
|
||||
public boolean getIntakeLimitSwtichState() {
|
||||
return intakeforwardLimit.isPressed();
|
||||
}
|
||||
// public boolean getIntakeLimitSwtichState() {
|
||||
// return intakeforwardLimit.isPressed();
|
||||
// }
|
||||
|
||||
public void setVoltage(double voltage) {
|
||||
pivot.setVoltage(voltage);
|
||||
}
|
||||
// public void setVoltage(double voltage) {
|
||||
// pivot.setVoltage(voltage);
|
||||
// }
|
||||
|
||||
public double getVelocity() {
|
||||
return pivot.getEncoder().getVelocity();
|
||||
}
|
||||
// public double getVelocity() {
|
||||
// return pivot.getEncoder().getVelocity();
|
||||
// }
|
||||
|
||||
public void setPivotEncoderPosition(int val) {
|
||||
pivot.getEncoder().setPosition(val);
|
||||
}
|
||||
// public void setPivotEncoderPosition(int val) {
|
||||
// pivot.getEncoder().setPosition(val);
|
||||
// }
|
||||
|
||||
public void resetPosition() {
|
||||
if(forwardLimit.isPressed()) {
|
||||
setPivotEncoderPosition(0);
|
||||
}
|
||||
}
|
||||
// public void resetPosition() {
|
||||
// if(forwardLimit.isPressed()) {
|
||||
// setPivotEncoderPosition(0);
|
||||
// }
|
||||
// }
|
||||
|
||||
public double getPos() {
|
||||
return pivot.getEncoder().getPosition();
|
||||
}
|
||||
// public double getPos() {
|
||||
// return pivot.getEncoder().getPosition();
|
||||
// }
|
||||
|
||||
public double getIntakeVelocity() {
|
||||
return intakeMotor.getEncoder().getVelocity();
|
||||
}
|
||||
// public double getIntakeVelocity() {
|
||||
// return intakeMotor.getEncoder().getVelocity();
|
||||
// }
|
||||
|
||||
public void rotateArm() {
|
||||
// public void rotateArm() {
|
||||
|
||||
}
|
||||
// }
|
||||
|
||||
public BooleanSupplier getArmFowardLimitState() {
|
||||
if(forwardLimit.isPressed()) {
|
||||
return sup;
|
||||
} else {
|
||||
return dup;
|
||||
}
|
||||
}
|
||||
// public BooleanSupplier getArmFowardLimitState() {
|
||||
// if(forwardLimit.isPressed()) {
|
||||
// return sup;
|
||||
// } else {
|
||||
// return dup;
|
||||
// }
|
||||
// }
|
||||
|
||||
public void changeIntakeNeutralState() {
|
||||
if(forwardLimit.isPressed()) {
|
||||
intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast);
|
||||
}
|
||||
}
|
||||
// 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();
|
||||
// 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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user