added the indexer
This commit is contained in:
parent
c7071d409b
commit
11512e140c
@ -14,6 +14,7 @@ import frc.robot.subsystems.ClimberPivot;
|
|||||||
import frc.robot.subsystems.ClimberRollers;
|
import frc.robot.subsystems.ClimberRollers;
|
||||||
import frc.robot.subsystems.Drivetrain;
|
import frc.robot.subsystems.Drivetrain;
|
||||||
import frc.robot.subsystems.Elevator;
|
import frc.robot.subsystems.Elevator;
|
||||||
|
import frc.robot.subsystems.Indexer;
|
||||||
import frc.robot.subsystems.Manipulator;
|
import frc.robot.subsystems.Manipulator;
|
||||||
|
|
||||||
import java.util.function.IntSupplier;
|
import java.util.function.IntSupplier;
|
||||||
@ -42,6 +43,8 @@ public class RobotContainer {
|
|||||||
private Elevator elevator;
|
private Elevator elevator;
|
||||||
//private ElevatorSysID elevator;
|
//private ElevatorSysID elevator;
|
||||||
|
|
||||||
|
private Indexer indexer;
|
||||||
|
|
||||||
private Manipulator manipulator;
|
private Manipulator manipulator;
|
||||||
|
|
||||||
private ManipulatorPivot manipulatorPivot;
|
private ManipulatorPivot manipulatorPivot;
|
||||||
@ -63,6 +66,8 @@ public class RobotContainer {
|
|||||||
elevator = new Elevator();
|
elevator = new Elevator();
|
||||||
//elevator = new ElevatorSysID();
|
//elevator = new ElevatorSysID();
|
||||||
|
|
||||||
|
indexer = new Indexer();
|
||||||
|
|
||||||
manipulator = new Manipulator();
|
manipulator = new Manipulator();
|
||||||
|
|
||||||
manipulatorPivot = new ManipulatorPivot();
|
manipulatorPivot = new ManipulatorPivot();
|
||||||
@ -118,6 +123,10 @@ public class RobotContainer {
|
|||||||
elevator.maintainPosition()
|
elevator.maintainPosition()
|
||||||
);
|
);
|
||||||
|
|
||||||
|
indexer.setDefaultCommand(
|
||||||
|
indexer.runIndexer(() -> 0)
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
manipulatorPivot.setDefaultCommand(
|
manipulatorPivot.setDefaultCommand(
|
||||||
manipulatorPivot.maintainPosition()
|
manipulatorPivot.maintainPosition()
|
||||||
@ -140,6 +149,7 @@ public class RobotContainer {
|
|||||||
|
|
||||||
driver.leftTrigger().whileTrue(
|
driver.leftTrigger().whileTrue(
|
||||||
manipulator.runUntilCollected(() -> 0.75)
|
manipulator.runUntilCollected(() -> 0.75)
|
||||||
|
.alongWith(indexer.runIndexer(() -> .75))
|
||||||
.until(() -> manipulator.getCoralBeamBreak() == false)
|
.until(() -> manipulator.getCoralBeamBreak() == false)
|
||||||
.andThen(manipulator.retractCommand(() -> .1))
|
.andThen(manipulator.retractCommand(() -> .1))
|
||||||
);
|
);
|
||||||
|
15
src/main/java/frc/robot/constants/IndexerConstants.java
Normal file
15
src/main/java/frc/robot/constants/IndexerConstants.java
Normal file
@ -0,0 +1,15 @@
|
|||||||
|
package frc.robot.constants;
|
||||||
|
|
||||||
|
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||||
|
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||||
|
|
||||||
|
public class IndexerConstants {
|
||||||
|
public static final int kIndexerMotorID = 16;
|
||||||
|
|
||||||
|
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
|
||||||
|
|
||||||
|
static{
|
||||||
|
motorConfig.smartCurrentLimit(40)
|
||||||
|
.idleMode(IdleMode.kCoast);
|
||||||
|
};
|
||||||
|
}
|
32
src/main/java/frc/robot/subsystems/Indexer.java
Normal file
32
src/main/java/frc/robot/subsystems/Indexer.java
Normal file
@ -0,0 +1,32 @@
|
|||||||
|
package frc.robot.subsystems;
|
||||||
|
|
||||||
|
import java.util.function.DoubleSupplier;
|
||||||
|
|
||||||
|
import com.revrobotics.spark.SparkMax;
|
||||||
|
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||||
|
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||||
|
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||||
|
|
||||||
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
|
import frc.robot.constants.IndexerConstants;
|
||||||
|
|
||||||
|
public class Indexer extends SubsystemBase {
|
||||||
|
private SparkMax indexerMotor;
|
||||||
|
|
||||||
|
public Indexer() {
|
||||||
|
indexerMotor = new SparkMax(IndexerConstants.kIndexerMotorID, MotorType.kBrushless);
|
||||||
|
|
||||||
|
indexerMotor.configure(
|
||||||
|
IndexerConstants.motorConfig,
|
||||||
|
ResetMode.kResetSafeParameters,
|
||||||
|
PersistMode.kPersistParameters
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
public Command runIndexer(DoubleSupplier speed) {
|
||||||
|
return run(() -> {
|
||||||
|
indexerMotor.set(speed.getAsDouble());
|
||||||
|
});
|
||||||
|
}
|
||||||
|
}
|
Loading…
Reference in New Issue
Block a user