Drive Subsystem

DriveSubsystem.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.kauailabs.navx.frc.AHRS;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.ControlType;
import com.revrobotics.CANSparkMax.ExternalFollower;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;

import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.DriveConstants;
import frc.robot.RobotContainer.Subsystems;
import frc.robot.libs.drivers.SparkEncoder;

public class DriveSubsystem extends SubsystemBase {

  public CANSparkMax leftMotor1 = new CANSparkMax(DriveConstants.kLeftMotor1Port, MotorType.kBrushless);
  public CANSparkMax leftMotor2 = new CANSparkMax(DriveConstants.kLeftMotor2Port, MotorType.kBrushless);

  private final MotorControllerGroup m_leftMotors = new MotorControllerGroup(leftMotor1, leftMotor2);

  public CANSparkMax rightMotor1 = new CANSparkMax(DriveConstants.kRightMotor1Port, MotorType.kBrushless);
  public CANSparkMax rightMotor2 = new CANSparkMax(DriveConstants.kRightMotor2Port, MotorType.kBrushless);

  private final MotorControllerGroup m_rightMotors = new MotorControllerGroup(rightMotor1, rightMotor2);

  private final DifferentialDrive m_drive;

  private final SparkEncoder m_leftEncoder = new SparkEncoder(
      leftMotor1.getEncoder(),
      DriveConstants.kLeftMotorInverted,
      DriveConstants.gearingRatio,
      Math.PI * DriveConstants.kWheelDiameterMeters);

  private final SparkEncoder m_rightEncoder = new SparkEncoder(
      rightMotor1.getEncoder(),
      !DriveConstants.kRightMotorInverted,
      DriveConstants.gearingRatio,
      Math.PI * DriveConstants.kWheelDiameterMeters);

  private final DifferentialDriveOdometry m_odometry;

  private final AHRS m_gyro = new AHRS(SerialPort.Port.kMXP);

  public DriveSubsystem() {
    reset(leftMotor1);
    reset(leftMotor2);
    reset(rightMotor1);
    reset(rightMotor2);

    // Disable follower mode on left & right master motors, enable follower on
    // follower motors
    leftMotor1.follow(ExternalFollower.kFollowerDisabled, 0);
    leftMotor2.follow(leftMotor1);
    rightMotor1.follow(ExternalFollower.kFollowerDisabled, 0);
    rightMotor2.follow(rightMotor1);

    // Set motors' inverted status
    leftMotor1.setInverted(DriveConstants.kLeftMotorInverted);
    leftMotor2.setInverted(DriveConstants.kLeftMotorInverted);
    rightMotor1.setInverted(DriveConstants.kRightMotorInverted);
    rightMotor2.setInverted(DriveConstants.kRightMotorInverted);


    leftMotor1.getPIDController().setP(DriveConstants.kPDrivePos, 0);

    rightMotor1.getPIDController().setP(DriveConstants.kPDrivePos, 0);

    leftMotor1.getPIDController().setI(DriveConstants.kIDrivePos, 0);

    rightMotor1.getPIDController().setI(DriveConstants.kIDrivePos, 0);
    leftMotor1.getPIDController().setD(DriveConstants.kDDrivePos, 0);

    rightMotor1.getPIDController().setD(DriveConstants.kDDrivePos, 0);


    m_drive = new DifferentialDrive(leftMotor1, rightMotor1);

    m_odometry = new DifferentialDriveOdometry(new Rotation2d(Math.toRadians(getGyroAngle())));
    resetAll();
  }

  public void reset(CANSparkMax m) {
    m.restoreFactoryDefaults();
    m.setIdleMode(IdleMode.kBrake);
    m.setSmartCurrentLimit(40);
    m.setOpenLoopRampRate(60);
  }

  public void arcadeDrive(double forward, double rotation) {
    m_drive.feed();
    m_drive.arcadeDrive(forward, rotation);

    setEffortsFromDashboard();
  }

  @Override
  public void periodic() {

    m_odometry.update(new Rotation2d(Math.toRadians(getGyroAngle())), m_leftEncoder.getDistance(),
        m_rightEncoder.getDistance());
    SmartDashboard.putString("Pose", m_odometry.getPoseMeters().toString());
    SmartDashboard.putNumber("Left Encoder Distance", m_leftEncoder.getDistance());
    SmartDashboard.putNumber("Right Encoder Distance", m_rightEncoder.getDistance());
  }

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

  public double getGyroAngle() {
    return m_gyro.getYaw();
  }

  /**
   * Returns the currently-estimated pose of the robot.
   *
   * @return The pose.
   */
  public Pose2d getPose() {
    return m_odometry.getPoseMeters();
  }


  /**
   * Returns the current wheel speeds of the robot.
   *
   * @return The current wheel speeds.
   */
  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
    return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
  }

  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(
      DriveConstants.ksVolts,
      DriveConstants.kvVoltSecondsPerMeter,
      DriveConstants.kaVoltSecondsSquaredPerMeter);


  public void setDriveStates(TrapezoidProfile.State left, TrapezoidProfile.State right) {
    leftMotor1.getPIDController().setReference(left.position, ControlType.kSmartMotion, 0,
        m_feedforward.calculate(left.velocity));

        leftMotor1.getPIDController().setReference(right.position, ControlType.kSmartMotion, 0,
        m_feedforward.calculate(right.velocity));
  }

  public void resetAll() {
    resetEncoders();
    this.m_gyro.reset(); // reset maybe
    // this.m_gyro.calibrate();
    this.resetOdometry(new Pose2d(0, 0, new Rotation2d(0)));
  }



 

  /** Resets the drive encoders to currently read a position of 0. */
  public void resetEncoders() {
    m_leftEncoder.reset();
    m_rightEncoder.reset();
  }

  /**
   * Resets the odometry to the specified pose.
   *
   * @param pose The pose to which to set the odometry.
   */
  public void resetOdometry(Pose2d pose) {
    resetEncoders();
    m_odometry.resetPosition(pose, new Rotation2d(Math.toRadians(getGyroAngle())));
  }

  /**
   * Controls the left and right sides of the drive directly with voltages.
   *
   * @param leftVolts  the commanded left output
   * @param rightVolts the commanded right output
   */
  public void tankDriveVolts(double leftVolts, double rightVolts) {
    m_leftMotors.setVoltage(-leftVolts);
    m_rightMotors.setVoltage(-rightVolts);
    m_drive.feed();
  }

  /**
   * Gets the average distance of the two encoders.
   *
   * @return the average of the two encoder readings
   */
  public double getAverageEncoderDistance() {
    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
  }

  /**
   * Sets the max output of the drive. Useful for scaling the drive to drive more
   * slowly.
   *
   * @param maxOutput the maximum output to which the drive will be constrained
   */
  public void setMaxOutput(double maxOutput) {
    m_drive.setMaxOutput(maxOutput);
  }

  /** Zeroes the heading of the robot. */
  public void zeroHeading() {
    m_gyro.reset();
  }

  /**
   * Returns the heading of the robot.
   *
   * @return the robot's heading in degrees, from -180 to 180
   */
  public double getHeading() {
    return m_gyro.getRotation2d().getDegrees();
  }

  /**
   * Returns the turn rate of the robot.
   *
   * @return The turn rate of the robot, in degrees per second
   */
  public double getTurnRate() {
    return -m_gyro.getRate();
  }

  public void setEffortsFromDashboard() {
    SmartDashboard.putString("MotorEfforts",
        "l1:" + leftMotor1.getAppliedOutput() + "l2: " + leftMotor2.getAppliedOutput() + " r1: "
            + rightMotor1.getAppliedOutput() + "r2: " + rightMotor2.getAppliedOutput());

    SmartDashboard.putString("followers", "l1: " + leftMotor1.isFollower() + " l2: " + leftMotor2.isFollower() + " r1 "
        + rightMotor1.isFollower() + " r2: " + rightMotor2.isFollower());
  }
}