Robot.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;

import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;


import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.studica.frc.AHRS;

import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import frc.robot.Constants.*;

/**
 * The methods in this class are called automatically corresponding to each mode, as described in
 * the TimedRobot documentation. If you change the name of this class or the package after creating
 * this project, you must also update the Main.java file in the project.
 */
@SuppressWarnings("PMD.RedundantFieldInitializer")
public class Robot extends TimedRobot {
  // Field- Orientated Drive
  //gyro
  AHRS ahrs;  

  //Limelight
  private boolean m_LimelightHasValidTarget = false;
  private double m_LimelightDriveCommand = 0.0;
  private double m_LimelightSteerCommand = 0.0;
  private double m_LimelightStrafeCommand = 0.0;

  Shuffleboard shuffleboard;
  private DigitalInput myInput = new DigitalInput(4);
  SlewRateLimiter filter = new SlewRateLimiter(0.5);

  //Timed Robot/Autonomous Variables
  private static final String kDefaultAuto = "Default";
  private static final String kLeftAuto = "Left Auto";
  private static final String kRightAuto = "Right Auto";
  private String m_autoSelected;
  private final SendableChooser<String> m_chooserAuto = new SendableChooser<>();

  //Mecanum Drive with Gyro variables

  private final MecanumDrive m_robotDrive;

  private final XboxController driverController = new XboxController(3);

  //Drive subsystem for Path planner
  private final Encoder m_frontLeftEncoder = new Encoder(0, 1, 0);
  private final Encoder m_frontRightEncoder = new Encoder(0, 1, 0);
  private final Encoder m_rearLeftEncoder = new Encoder(0, 1, 0);
  private final Encoder m_rearRightEncoder = new Encoder(0, 1, 0);

  private final MecanumDriveKinematics m_DriveKinematics = 
  new MecanumDriveKinematics(
    OdometryConstants.frontLeftWheelMeters, 
    OdometryConstants.frontRightWheelMeters, 
    OdometryConstants.rearLeftWheelMeters, 
    OdometryConstants.rearRightWheelMeters);

  private final MecanumDriveOdometry m_Odometry;
  
  //Elevator PID

  private final XboxController operatorController = new XboxController(2);
  private final Encoder m_encoder = new Encoder(6, 7);
  private final SparkMax m_motor = new SparkMax(1, MotorType.kBrushless);

  //Declare DIO pins
   

  // Create a PID controller whose setpoint's change is subject to maximum
  // velocity and acceleration constraints.
  private final TrapezoidProfile.Constraints m_constraints =
      new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocity, ElevatorConstants.kMaxAcceleration);
  private final ProfiledPIDController m_controller =
      new ProfiledPIDController(ElevatorConstants.kP, 
      ElevatorConstants.kI, 
      ElevatorConstants.kD,
       m_constraints, 
      ElevatorConstants.kDt);

  private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward(ElevatorConstants.kS, 
  ElevatorConstants.kG, 
  ElevatorConstants.kV);

  // This function is run when the robot is first started up and should be used for any initialization code.
  public Robot() {    
    // Field Orientated Drive
    try {
      ahrs = new AHRS(AHRS.NavXComType.kMXP_SPI);
  } catch (Exception e) {
      System.out.println("Error initializing AHRS: " + e.getMessage());
  }
  
  
    //Timed Robot - useful for picking multiple autonomous
    m_chooserAuto.setDefaultOption("Default Auto", kDefaultAuto);
    m_chooserAuto.addOption("Left Auto", kLeftAuto);
    m_chooserAuto.addOption("Right Auto", kRightAuto);

    SmartDashboard.putData("Auto choices", m_chooserAuto);

    //Mecanum 
    SparkMax frontLeft = new SparkMax(DriveConstants.kFrontLeftChannel, MotorType.kBrushless);
    SparkMax rearLeft = new SparkMax(DriveConstants.kRearLeftChannel, MotorType.kBrushless);
    SparkMax frontRight = new SparkMax(DriveConstants.kFrontRightChannel, MotorType.kBrushless);
    SparkMax rearRight = new SparkMax(DriveConstants.kRearRightChannel, MotorType.kBrushless);

     // pathplanner set up
    //ts pmo
    m_frontLeftEncoder.setDistancePerPulse(DriveConstants.driveDistancePerPulse);
    m_frontRightEncoder.setDistancePerPulse(DriveConstants.driveDistancePerPulse);
    m_rearLeftEncoder.setDistancePerPulse(DriveConstants.driveDistancePerPulse);
    m_rearRightEncoder.setDistancePerPulse(DriveConstants.driveDistancePerPulse);

    resetOdometry(getPose());
    m_Odometry = new MecanumDriveOdometry(m_DriveKinematics, 
    ahrs.getRotation2d(),
    new MecanumDriveWheelPositions(
      m_frontLeftEncoder.getDistance(), 
      m_frontRightEncoder.getDistance(), 
      m_rearLeftEncoder.getDistance(), 
      m_rearRightEncoder.getDistance())
  );

    //mecanum
    m_robotDrive = new MecanumDrive(frontLeft::set, rearLeft::set, frontRight::set, rearRight::set);

    SendableRegistry.addChild(m_robotDrive, frontLeft);
    SendableRegistry.addChild(m_robotDrive, rearLeft);
    SendableRegistry.addChild(m_robotDrive, frontRight);
    SendableRegistry.addChild(m_robotDrive, rearRight);

    //Elevator PID
    m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);



  }


   // This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
   //that you want ran during disabled, autonomous, teleoperated and test.

  @Override
  public void robotPeriodic() {
    m_Odometry.update(ahrs.getRotation2d(), 
    new MecanumDriveWheelPositions(
      m_frontLeftEncoder.getDistance(), 
      m_frontRightEncoder.getDistance(), 
      m_rearLeftEncoder.getDistance(), 
      m_rearRightEncoder.getDistance())
  );
}

  @Override
  public void autonomousInit() {
    m_autoSelected = m_chooserAuto.getSelected();
    System.out.println("Auto selected: " + m_autoSelected);
  }

  /** This function is called periodically during autonomous. */
  @Override
  public void autonomousPeriodic() {
    switch (m_autoSelected) {
      case kLeftAuto:
        // Put custom auto code here
        break;
        case kRightAuto:
        // Put custom auto code here
         
          m_robotDrive.driveCartesian(0, -0.5, 0);
          Timer.delay(2.0);
          m_robotDrive.driveCartesian(0, 0, 0);
          m_controller.setGoal(5);
          

        break;
      case kDefaultAuto:
      default:
      
        // Put default auto code here
        //strafe

        m_robotDrive.driveCartesian(-0.5, 0, 0);
        Timer.delay(2.0);
        m_robotDrive.driveCartesian(0, 0, 0);
        
        break;
        
    }
  }


  /** This function is called once when teleop is enabled. */
  @Override
  public void teleopInit() {
    if (m_autoSelected != null)
    {
      m_autoSelected = null;
    }
  }

  /** This function is called periodically during operator control. */
   /** Mecanum drive is used with the gyro angle as an input. */
   @Override
   public void teleopPeriodic() {
    //Limelight
    Update_Limelight_Tracking();

    boolean auto = driverController.getAButton();

    if (auto)
    {
      if (m_LimelightHasValidTarget)
      {
        System.out.println("yeah");
        m_robotDrive.driveCartesian(-m_LimelightStrafeCommand, -m_LimelightDriveCommand, m_LimelightSteerCommand);
       System.out.println("drive: " + m_LimelightDriveCommand);
       System.out.println("strafe: " + m_LimelightStrafeCommand);

       System.out.println("steer: " + m_LimelightSteerCommand);

      }
      else
      {
        m_robotDrive.driveCartesian(0,0,0);
      }
    }
    else
    {
      m_robotDrive.driveCartesian(
        -driverController.getLeftX(), 
        driverController.getLeftY(), 
        -driverController.getRightX(), 
        ahrs.getRotation2d());
    }


    SmartDashboard.putBoolean("Presence", myInput.get());
      //Mecanum
      if (driverController.getXButtonPressed()) {
        ahrs.zeroYaw();
      }

      //Elevator
      
      if (operatorController.getAButtonPressed()) {
        m_controller.setGoal(5);
      } else if (operatorController.getBButtonPressed()) {
        m_controller.setGoal(0);
      }
  
      // Run controller and update motor output
      m_motor.setVoltage( 
          m_controller.calculate(m_encoder.getDistance())
              + m_feedforward.calculate(m_controller.getSetpoint().velocity));
              
    }

  /** This function is called once when the robot is disabled. */
  @Override
  public void disabledInit() {}

  /** This function is called periodically when disabled. */
  @Override
  public void disabledPeriodic() {}

  /** This function is called once when test mode is enabled. */
  @Override
  public void testInit() {}

  /** This function is called periodically during test mode. */
  @Override
  public void testPeriodic() {}

  /** This function is called once when the robot is first started up. */
  @Override
  public void simulationInit() {}

  /** This function is called periodically whilst in simulation. */
  @Override
  public void simulationPeriodic() {}
  public void Update_Limelight_Tracking()
  {       
        double tv = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
        double tx = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
        double ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
        double ta = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0);

        if (tv < 1.0)
        {
          m_LimelightHasValidTarget = false;
          m_LimelightDriveCommand = 0.0;
          m_LimelightSteerCommand = 0.0;
         

          return;
        }
        //  System.out.println("tv:" + tv);
        //  System.out.println("tx:" + tx);
       //   System.out.println("ta:" + ta);
        m_LimelightHasValidTarget = true;

        // Start with proportional steering
        double steer_cmd = tx * DriveConstants.STEER_K;
        m_LimelightSteerCommand = steer_cmd;

        double strafe_cmd = ty * DriveConstants.DRIVE_K;

        // try to drive forward until the target area reaches our desired area
        double drive_cmd = (DriveConstants.DESIRED_TARGET_AREA - ta) * DriveConstants.DRIVE_K;

        // don't let the robot drive too fast into the goal
        if (drive_cmd > DriveConstants.MAX_DRIVE)
        {
          drive_cmd = DriveConstants.MAX_DRIVE;
        }
        if (strafe_cmd > DriveConstants.MAX_DRIVE)
        {
          strafe_cmd = DriveConstants.MAX_DRIVE;
        }
        else if(strafe_cmd < -DriveConstants.MAX_DRIVE)
        {
          strafe_cmd = -DriveConstants.MAX_DRIVE;
        }
        m_LimelightStrafeCommand = strafe_cmd;

        m_LimelightDriveCommand = drive_cmd;

  }

  public Pose2d getPose() {
    return m_Odometry.getPoseMeters();
  }

   public MecanumDriveWheelSpeeds getWheelSpeeds() {
    return new MecanumDriveWheelSpeeds
    (m_frontLeftEncoder.getRate(), 
    m_frontRightEncoder.getRate(), 
    m_rearLeftEncoder.getRate(), 
    m_rearRightEncoder.getRate());
  }

  public void resetOdometry(Pose2d pose) {
    System.out.println("Resetted Odometry");
    m_Odometry.resetPosition(ahrs.getRotation2d(), new MecanumDriveWheelPositions(
      m_frontLeftEncoder.getDistance(), 
      m_frontRightEncoder.getDistance(), 
      m_rearLeftEncoder.getDistance(), 
      m_rearRightEncoder.getDistance()), pose);
  }

}