Drivetrain.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.subsystems;

import com.revrobotics.PersistMode;
import com.revrobotics.ResetMode;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.SparkLowLevel.MotorType;

import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class Drivetrain extends SubsystemBase {
  public final SparkMax frontLeft;
  public final SparkMax frontRight;
  // public final SparkMax backLeft;
  // public final SparkMax backRight;

  public DifferentialDrive drive;

  /** Creates a new Drivetrain. */
  public Drivetrain() {
    frontLeft = new SparkMax(4, MotorType.kBrushed);
    // backLeft = new SparkMax(4, MotorType.kBrushed);
    
    frontRight = new SparkMax(1, MotorType.kBrushed);
    // backRight = new SparkMax(2, MotorType.kBrushed);

    SparkMaxConfig frontLeftConfig = new SparkMaxConfig();
    frontLeftConfig.idleMode(IdleMode.kBrake);
    frontLeftConfig.inverted(false);
    frontLeft.configure(frontLeftConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);

    SparkMaxConfig backLeftConfig = new SparkMaxConfig();
    backLeftConfig.idleMode(IdleMode.kBrake);
    backLeftConfig.inverted(false);
    backLeftConfig.follow(frontLeft.getDeviceId());
    //backLeft.configure(backLeftConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);

    SparkMaxConfig frontRightConfig = new SparkMaxConfig();
    frontRightConfig.idleMode(IdleMode.kBrake);
    frontRightConfig.inverted(false);
    frontRight.configure(frontRightConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);

    SparkMaxConfig backRightConfig = new SparkMaxConfig();
    backRightConfig.idleMode(IdleMode.kBrake);
    backRightConfig.inverted(false);
    backRightConfig.follow(frontRight.getDeviceId());
    //backRight.configure(backRightConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);

    drive = new DifferentialDrive(frontLeft, frontRight);
  }

  public void drive (double ySpeed, double rotSpeed){
    if (Math.abs(rotSpeed) <= 0.05) {
      drive.tankDrive(ySpeed,-0.7*ySpeed);
    } else {
      drive.tankDrive(rotSpeed, 0.7*rotSpeed);
    }
  }

  public void stop()  {
    drive.tankDrive(0, 0);
  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }
}