diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5e9b2ab..a7f2346 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -29,9 +29,9 @@ public static class MotorIdConstants { public static final int LEFT_FRONT_ID = 1; public static final int RIGHT_BACK_ID = 4; public static final int LEFT_BACK_ID = 2; + } - public static class MotorConstants { - public static final Current NEO_CURRENT_LIMIT = Amps.of(50); - } + public static class MotorConstants { + public static final Current NEO_CURRENT_LIMIT = Amps.of(50); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 22e044b..eac5015 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final Drivetrain drivetrain = new Drivetrain(); public final CommandXboxController commandxboxcontroller = new CommandXboxController(0); + public final CommandXboxController operatorXboxController = new CommandXboxController(1); /*added array bc the code did not repeat the action when it was set to null. /*it fixed it bc it told the code that there was nothing there. @@ -37,14 +38,13 @@ public RobotContainer() { private void setUpDefaultCommands() { drivetrain.setDefaultCommand( - drivetrain.tankDrive(() -> commandxboxcontroller.getRightY(), () -> commandxboxcontroller.getLeftY()) + drivetrain.tankDrive(() -> commandxboxcontroller.getRightY(), () -> commandxboxcontroller.getLeftY(), () -> operatorXboxController.rightTrigger().getAsBoolean()) ); } private void setUpDriverButtonBindings() { - commandxboxcontroller.a().whileTrue(Commands.run(() -> System.out.println("yay we did it"))); commandxboxcontroller.b().whileTrue(Commands.run(() -> System.out.println("yay we did it"))); } @@ -52,7 +52,7 @@ private void setUpDriverButtonBindings() { private void setUpOperatorButtonBindings() { - + operatorXboxController.a().onTrue(Commands.none()).onFalse(Commands.none()); } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index e3754fa..ee6311f 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -5,6 +5,7 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import com.revrobotics.spark.SparkBase.ControlType; @@ -19,104 +20,115 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.MotorIdConstants; -import frc.robot.Constants.MotorIdConstants.MotorConstants; +import frc.robot.Constants.MotorConstants; public class Drivetrain extends SubsystemBase { - private SparkMax rightFrontMotorController = new SparkMax(MotorIdConstants.RIGHT_FRONT_ID, SparkLowLevel.MotorType.kBrushless); - private SparkMax leftFrontMotorController = new SparkMax(MotorIdConstants.LEFT_FRONT_ID, SparkLowLevel.MotorType.kBrushless); - private SparkMax rightBackMotorController = new SparkMax(MotorIdConstants.RIGHT_BACK_ID, SparkLowLevel.MotorType.kBrushless); - private SparkMax leftBackMotorController = new SparkMax(MotorIdConstants.LEFT_BACK_ID, SparkLowLevel.MotorType.kBrushless); - - private final SparkClosedLoopController motorRightClosedLoopController; - private final SparkClosedLoopController motorLeftClosedLoopController; - - private final Distance ENCODER_POSITION_FACTOR = Inches.of(8 * Math.PI * (10./52.) * (30./68.)); - private final Distance ENCODER_VELOCITY_FACTOR = Inches.of((8 * Math.PI * (10./52.) * (30./68.)) / 60.0); //inches per rotation - - private final double drivetrainP = 0.08; - - private final double MAX_SPEED_METERS_PER_SECOND = 4.0; - - public Drivetrain() { - SparkMaxConfig rightFrontConfig = new SparkMaxConfig(); - SparkMaxConfig leftFrontConfig = new SparkMaxConfig(); - SparkMaxConfig rightBackConfig = new SparkMaxConfig(); - SparkMaxConfig leftBackConfig = new SparkMaxConfig(); - - rightFrontConfig.idleMode(IdleMode.kBrake); - leftFrontConfig.idleMode(IdleMode.kBrake); - rightBackConfig.idleMode(IdleMode.kBrake); - leftBackConfig.idleMode(IdleMode.kBrake); - - rightFrontConfig.inverted(false); - rightBackConfig.inverted(false); - leftFrontConfig.inverted(true); - leftBackConfig.inverted(true); - - rightBackConfig.follow(rightFrontMotorController,false); - leftBackConfig.follow(leftFrontMotorController,false); - - rightFrontConfig.smartCurrentLimit((int) MotorConstants.NEO_CURRENT_LIMIT.in(Amps)); - leftFrontConfig.smartCurrentLimit((int) MotorConstants.NEO_CURRENT_LIMIT.in(Amps)); - rightBackConfig.smartCurrentLimit((int) MotorConstants.NEO_CURRENT_LIMIT.in(Amps)); - leftBackConfig.smartCurrentLimit((int) MotorConstants.NEO_CURRENT_LIMIT.in(Amps)); - - - rightFrontConfig.encoder.positionConversionFactor(ENCODER_POSITION_FACTOR.in(Meters)); - leftFrontConfig.encoder.positionConversionFactor(ENCODER_POSITION_FACTOR.in(Meters)); - rightBackConfig.encoder.positionConversionFactor(ENCODER_POSITION_FACTOR.in(Meters)); - leftBackConfig.encoder.positionConversionFactor(ENCODER_POSITION_FACTOR.in(Meters)); - - rightFrontConfig.encoder.velocityConversionFactor(ENCODER_VELOCITY_FACTOR.in(Meters)); - leftFrontConfig.encoder.velocityConversionFactor(ENCODER_VELOCITY_FACTOR.in(Meters)); - rightBackConfig.encoder.velocityConversionFactor(ENCODER_VELOCITY_FACTOR.in(Meters)); - leftBackConfig.encoder.velocityConversionFactor(ENCODER_VELOCITY_FACTOR.in(Meters)); - - rightFrontConfig.closedLoop.p(drivetrainP); - leftFrontConfig.closedLoop.p(drivetrainP); - rightBackConfig.closedLoop.p(drivetrainP); - leftBackConfig.closedLoop.p(drivetrainP); - - - - rightFrontMotorController.configure( - rightFrontConfig, - SparkMax.ResetMode.kResetSafeParameters, - SparkMax.PersistMode.kPersistParameters); - - leftFrontMotorController.configure( - leftFrontConfig, - SparkMax.ResetMode.kResetSafeParameters, - SparkMax.PersistMode.kPersistParameters); - - rightBackMotorController.configure( - rightBackConfig, - SparkMax.ResetMode.kResetSafeParameters, - SparkMax.PersistMode.kPersistParameters); - - leftBackMotorController.configure( - leftBackConfig, - SparkMax.ResetMode.kResetSafeParameters, - SparkMax.PersistMode.kPersistParameters); - - motorRightClosedLoopController = rightFrontMotorController.getClosedLoopController(); - motorLeftClosedLoopController = leftFrontMotorController.getClosedLoopController(); - - } - - public void setRightSpeed(LinearVelocity speed) { - motorRightClosedLoopController.setReference(speed.in(MetersPerSecond), ControlType.kVelocity); - } - - public void setLeftSpeed(LinearVelocity speed) { - motorLeftClosedLoopController.setReference(speed.in(MetersPerSecond), ControlType.kVelocity); - } - public Command tankDrive(DoubleSupplier rightSpeed, DoubleSupplier leftSpeed) { - return this.run(()->{ - setLeftSpeed((MetersPerSecond.of(leftSpeed.getAsDouble() * MAX_SPEED_METERS_PER_SECOND))); - setRightSpeed((MetersPerSecond.of(rightSpeed.getAsDouble() * MAX_SPEED_METERS_PER_SECOND))); - } ); - } + private SparkMax rightFrontMotorController = new SparkMax(MotorIdConstants.RIGHT_FRONT_ID, + SparkLowLevel.MotorType.kBrushless); + private SparkMax leftFrontMotorController = new SparkMax(MotorIdConstants.LEFT_FRONT_ID, + SparkLowLevel.MotorType.kBrushless); + private SparkMax rightBackMotorController = new SparkMax(MotorIdConstants.RIGHT_BACK_ID, + SparkLowLevel.MotorType.kBrushless); + private SparkMax leftBackMotorController = new SparkMax(MotorIdConstants.LEFT_BACK_ID, + SparkLowLevel.MotorType.kBrushless); + + private final SparkClosedLoopController motorRightClosedLoopController; + private final SparkClosedLoopController motorLeftClosedLoopController; + + private final Distance ENCODER_POSITION_FACTOR = Inches.of(8 * Math.PI * (10. / 52.) * (30. / 68.)); + private final Distance ENCODER_VELOCITY_FACTOR = Inches.of((8 * Math.PI * (10. / 52.) * (30. / 68.)) / 60.0); // inches + // per + // rotation + + private final double drivetrainP = 0.08; + + private final double MAX_SPEED_METERS_PER_SECOND = 4.0; + + public Drivetrain() { + SparkMaxConfig rightFrontConfig = new SparkMaxConfig(); + SparkMaxConfig leftFrontConfig = new SparkMaxConfig(); + SparkMaxConfig rightBackConfig = new SparkMaxConfig(); + SparkMaxConfig leftBackConfig = new SparkMaxConfig(); + + rightFrontConfig.idleMode(IdleMode.kBrake); + leftFrontConfig.idleMode(IdleMode.kBrake); + rightBackConfig.idleMode(IdleMode.kBrake); + leftBackConfig.idleMode(IdleMode.kBrake); + + rightFrontConfig.inverted(false); + rightBackConfig.inverted(false); + leftFrontConfig.inverted(true); + leftBackConfig.inverted(true); + + rightBackConfig.follow(rightFrontMotorController, false); + leftBackConfig.follow(leftFrontMotorController, false); + + rightFrontConfig.smartCurrentLimit((int) MotorConstants.NEO_CURRENT_LIMIT.in(Amps)); + leftFrontConfig.smartCurrentLimit((int) MotorConstants.NEO_CURRENT_LIMIT.in(Amps)); + rightBackConfig.smartCurrentLimit((int) MotorConstants.NEO_CURRENT_LIMIT.in(Amps)); + leftBackConfig.smartCurrentLimit((int) MotorConstants.NEO_CURRENT_LIMIT.in(Amps)); + + rightFrontConfig.encoder.positionConversionFactor(ENCODER_POSITION_FACTOR.in(Meters)); + leftFrontConfig.encoder.positionConversionFactor(ENCODER_POSITION_FACTOR.in(Meters)); + rightBackConfig.encoder.positionConversionFactor(ENCODER_POSITION_FACTOR.in(Meters)); + leftBackConfig.encoder.positionConversionFactor(ENCODER_POSITION_FACTOR.in(Meters)); + + rightFrontConfig.encoder.velocityConversionFactor(ENCODER_VELOCITY_FACTOR.in(Meters)); + leftFrontConfig.encoder.velocityConversionFactor(ENCODER_VELOCITY_FACTOR.in(Meters)); + rightBackConfig.encoder.velocityConversionFactor(ENCODER_VELOCITY_FACTOR.in(Meters)); + leftBackConfig.encoder.velocityConversionFactor(ENCODER_VELOCITY_FACTOR.in(Meters)); + + rightFrontConfig.closedLoop.p(drivetrainP); + leftFrontConfig.closedLoop.p(drivetrainP); + rightBackConfig.closedLoop.p(drivetrainP); + leftBackConfig.closedLoop.p(drivetrainP); + + rightFrontMotorController.configure( + rightFrontConfig, + SparkMax.ResetMode.kResetSafeParameters, + SparkMax.PersistMode.kPersistParameters); + + leftFrontMotorController.configure( + leftFrontConfig, + SparkMax.ResetMode.kResetSafeParameters, + SparkMax.PersistMode.kPersistParameters); + + rightBackMotorController.configure( + rightBackConfig, + SparkMax.ResetMode.kResetSafeParameters, + SparkMax.PersistMode.kPersistParameters); + + leftBackMotorController.configure( + leftBackConfig, + SparkMax.ResetMode.kResetSafeParameters, + SparkMax.PersistMode.kPersistParameters); + + motorRightClosedLoopController = rightFrontMotorController.getClosedLoopController(); + motorLeftClosedLoopController = leftFrontMotorController.getClosedLoopController(); + + } + + public void setRightSpeed(LinearVelocity speed) { + motorRightClosedLoopController.setReference(speed.in(MetersPerSecond), ControlType.kVelocity); + } + + public void setLeftSpeed(LinearVelocity speed) { + motorLeftClosedLoopController.setReference(speed.in(MetersPerSecond), ControlType.kVelocity); + } + + public Command tankDrive(DoubleSupplier rightSpeed, DoubleSupplier leftSpeed, + BooleanSupplier runWhenRightTriggerDown) { + return this.run(() -> { + if (runWhenRightTriggerDown.getAsBoolean()) { + setLeftSpeed((MetersPerSecond + .of(leftSpeed.getAsDouble() * MAX_SPEED_METERS_PER_SECOND))); + setRightSpeed((MetersPerSecond + .of(rightSpeed.getAsDouble() * MAX_SPEED_METERS_PER_SECOND))); + } else { + setLeftSpeed((MetersPerSecond.of(0))); + setRightSpeed((MetersPerSecond.of(0))); + } + }); + + } } - -