second intake motor
This commit is contained in:
@@ -5,11 +5,13 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
|
||||
public class IntakeRollerConstants {
|
||||
// TODO Real values
|
||||
public static final int kMotorCANID = 20;
|
||||
public static final int kRightMotorCANID = 20;
|
||||
public static final int kLeftMotorCANID = 1;
|
||||
|
||||
public static final int kCurrentLimit = 65;
|
||||
|
||||
public static final boolean kInvertMotors = true;
|
||||
public static final boolean kInvertLeftMotor = true;
|
||||
public static final boolean kInvertRightMotor = false;
|
||||
|
||||
public static final double kSpeed = 1;
|
||||
|
||||
@@ -18,11 +20,17 @@ public class IntakeRollerConstants {
|
||||
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
|
||||
|
||||
public static final SparkMaxConfig leftMotorConfig = new SparkMaxConfig();
|
||||
public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig();
|
||||
|
||||
static {
|
||||
leftMotorConfig
|
||||
.idleMode(kIdleMode)
|
||||
.smartCurrentLimit(kCurrentLimit)
|
||||
.inverted(kInvertMotors);
|
||||
.inverted(kInvertLeftMotor);
|
||||
rightMotorConfig
|
||||
.idleMode(kIdleMode)
|
||||
.smartCurrentLimit(kCurrentLimit)
|
||||
.inverted(kInvertRightMotor)
|
||||
.follow(kRightMotorCANID);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -10,33 +10,41 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.constants.IntakeRollerConstants;
|
||||
|
||||
public class IntakeRoller extends SubsystemBase {
|
||||
private SparkMax motor;
|
||||
private SparkMax leftMotor;
|
||||
private SparkMax rightMotor;
|
||||
|
||||
public IntakeRoller() {
|
||||
motor = new SparkMax(IntakeRollerConstants.kMotorCANID, MotorType.kBrushless);
|
||||
leftMotor = new SparkMax(IntakeRollerConstants.kLeftMotorCANID, MotorType.kBrushless);
|
||||
rightMotor = new SparkMax(IntakeRollerConstants.kRightMotorCANID, MotorType.kBrushless);
|
||||
|
||||
motor.configure(
|
||||
leftMotor.configure(
|
||||
IntakeRollerConstants.leftMotorConfig,
|
||||
ResetMode.kResetSafeParameters,
|
||||
PersistMode.kPersistParameters
|
||||
);
|
||||
|
||||
rightMotor.configure(
|
||||
IntakeRollerConstants.rightMotorConfig,
|
||||
ResetMode.kResetSafeParameters,
|
||||
PersistMode.kPersistParameters
|
||||
);
|
||||
}
|
||||
|
||||
public Command runIn() {
|
||||
return run(() -> {
|
||||
motor.set(IntakeRollerConstants.kSpeed*0.8);
|
||||
leftMotor.set(IntakeRollerConstants.kSpeed*0.8);
|
||||
});
|
||||
}
|
||||
|
||||
public Command runOut() {
|
||||
return run(() -> {
|
||||
motor.set(-IntakeRollerConstants.kSpeed);
|
||||
leftMotor.set(-IntakeRollerConstants.kSpeed);
|
||||
});
|
||||
}
|
||||
|
||||
public Command stop() {
|
||||
return run(() -> {
|
||||
motor.set(0);
|
||||
leftMotor.set(0);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user