47 lines
1.4 KiB
Java
47 lines
1.4 KiB
Java
package frc.robot.subsystems;
|
|
|
|
import java.util.function.DoubleSupplier;
|
|
|
|
import com.ctre.phoenix.motorcontrol.can.VictorSPX;
|
|
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
|
|
|
|
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
|
|
public class Drivetrain extends SubsystemBase {
|
|
private WPI_VictorSPX frontLeft;
|
|
private WPI_VictorSPX frontRight;
|
|
private WPI_VictorSPX rearLeft;
|
|
private WPI_VictorSPX rearRight;
|
|
|
|
private DifferentialDrive dt;
|
|
|
|
public Drivetrain() {
|
|
frontLeft = new WPI_VictorSPX(4);
|
|
frontRight = new WPI_VictorSPX(2);
|
|
rearLeft = new WPI_VictorSPX(3);
|
|
rearRight = new WPI_VictorSPX(1);
|
|
|
|
frontLeft.setInverted(true);
|
|
rearLeft.setInverted(true);
|
|
|
|
rearLeft.follow(frontLeft);
|
|
rearRight.follow(frontRight);
|
|
|
|
dt = new DifferentialDrive(frontLeft, frontRight);
|
|
}
|
|
|
|
public Command arcadeDrive(DoubleSupplier forwardBackward, DoubleSupplier leftRight) {
|
|
return run(() -> {
|
|
dt.arcadeDrive(forwardBackward.getAsDouble(), leftRight.getAsDouble());
|
|
});
|
|
}
|
|
|
|
public Command tankDrive(DoubleSupplier leftSpeed, DoubleSupplier rightSpeed) {
|
|
return run(() -> {
|
|
dt.tankDrive(leftSpeed.getAsDouble(), rightSpeed.getAsDouble());
|
|
});
|
|
}
|
|
}
|