extender and intake changes

This commit is contained in:
aarav18
2022-03-14 20:10:12 -06:00
parent ed10b1e83f
commit 8a902e635f
11 changed files with 77 additions and 75 deletions
@@ -15,13 +15,11 @@ import frc4388.robot.Constants.ExtenderConstants;
public class Extender extends SubsystemBase {
public CANSparkMax m_extenderMotor;
private CANSparkMax m_extenderMotor;
private SparkMaxLimitSwitch m_inLimit;
private SparkMaxLimitSwitch m_outLimit;
public boolean toggle;
/** Creates a new Extender. */
public Extender(CANSparkMax extenderMotor) {
@@ -52,30 +50,20 @@ public class Extender extends SubsystemBase {
SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition());
}
/**
* Runs The Extender-
* @param extended Wether the Extender Is Extended
*/
// public void runExtender(boolean extended) {
// if (!m_serializer.getBeam() && !extended) return;
// double extenderMotorSpeed = extended ? 0.25d : -0.25d;
// m_extenderMotor.set(extenderMotorSpeed);
// }
public void runExtender(double input) {
// if (!m_serializer.getBeam() && input < 0.) return;
m_extenderMotor.set(input);
}
public double getPosition() {
return m_extenderMotor.getEncoder().getPosition();
}
public void setEncoder(double position) {
m_extenderMotor.getEncoder().setPosition(position);
}
public double getCurrent() {
return m_extenderMotor.getOutputCurrent();
}
/**
* Toggles The Extender
*/
// public void toggleExtender() {
// toggle = !toggle;
// runExtender(toggle);
// }
}
@@ -4,27 +4,20 @@
package frc4388.robot.subsystems;
//Imported Limit switch ONLY
import com.revrobotics.SparkMaxLimitSwitch;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.commands.ExtenderIntakeGroup;
import com.revrobotics.CANSparkMax;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
public class Intake extends SubsystemBase {
public WPI_TalonFX m_intakeMotor;
private Serializer m_serializer;
private WPI_TalonFX m_intakeMotor;
/** Creates a new Intake. */
public Intake(WPI_TalonFX intakeMotor, Serializer serializer) {
public Intake(WPI_TalonFX intakeMotor) {
m_intakeMotor = intakeMotor;
m_serializer = serializer;
}
@Override
@@ -39,10 +32,18 @@ public class Intake extends SubsystemBase {
* @param rightTrigger Right Trigger to Run Outward
*/
public void runWithTriggers(double leftTrigger, double rightTrigger) {
m_intakeMotor.set((rightTrigger - leftTrigger) * 0.4);
m_intakeMotor.set((rightTrigger - leftTrigger) * IntakeConstants.INTAKE_SPEED_MULTIPLIER);
SmartDashboard.putNumber("Intake Current Supply", m_intakeMotor.getSupplyCurrent());
SmartDashboard.putNumber("Intake Current Stator", m_intakeMotor.getStatorCurrent());
}
public void runAtOutput(double output, double multiplier) {
m_intakeMotor.set(output * multiplier);
}
public void runAtOutput(double output) {
m_intakeMotor.set(output * IntakeConstants.INTAKE_SPEED_MULTIPLIER);
}
public double getCurrent() {
return m_intakeMotor.getSupplyCurrent();