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.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMTalonFX;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.util.Units;
import edu.wpi.first.wpilibj2.command.CommandGroupBase;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;

/**
 * The VM is configured to automatically run this class, and to call the functions 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 build.gradle file in the
 * project.
 */
public class Robot extends TimedRobot {

  // Create motor controllers.
  private PWMTalonFX l_leader_ = new PWMTalonFX(1);
  private PWMTalonFX r_leader_ = new PWMTalonFX(2);

  // Create encoders and gyro
  private Encoder l_enc_ = new Encoder(0, 1);
  private Encoder r_enc_ = new Encoder(2, 3);
  private ADXRS450_Gyro gyro_ = new ADXRS450_Gyro();

  // Create encoder sim
  private EncoderSim l_enc_sim_ = new EncoderSim(l_enc_);
  private EncoderSim r_enc_sim_ = new EncoderSim(r_enc_);
  private ADXRS450_GyroSim gyro_sim_ = new ADXRS450_GyroSim(gyro_);

  // Create differential drive object.
  private DifferentialDrive drive_ = new DifferentialDrive(l_leader_, r_leader_);

  // Create differential drive sim
  private DifferentialDrivetrainSim drive_sim_ = new DifferentialDrivetrainSim(
      DCMotor.getNEO(2), 7.29, 7.5, 60.0,
      Units.inchesToMeters(3.0), 0.7112, null);

  // Create odometry
  private DifferentialDriveOdometry odometry_ = new DifferentialDriveOdometry(gyro_.getRotation2d(),
      new Pose2d(2, 2, new Rotation2d()));

  // Field visualization
  private Field2d field_ = new Field2d();

  // Resets the pose on the field.
  private void reset(Pose2d pose) {
    // We must reset encoders to 0 whenever we reset odometry.
    l_enc_.reset();
    r_enc_.reset();

    odometry_.resetPosition(pose, gyro_.getRotation2d());
    drive_sim_.setPose(pose);
  }

  @Override
  public void robotInit() {
    SmartDashboard.putData("Field", field_);
    drive_.setSafetyEnabled(false);

    l_enc_.setDistancePerPulse(2 * Math.PI * Units.inchesToMeters(3) / 1440);
    r_enc_.setDistancePerPulse(2 * Math.PI * Units.inchesToMeters(3) / 1440);
  }

  @Override
  public void robotPeriodic() {
    CommandScheduler.getInstance().run();
    odometry_.update(gyro_.getRotation2d(), l_enc_.getDistance(), r_enc_.getDistance());
    field_.setRobotPose(odometry_.getPoseMeters());
  }


  @Override
  public void autonomousInit() {

    // PID Controller for distance.
    PIDController distance_controller = new PIDController(0.9, 0.0, 0.15);

    // PID Controller for angle.
    PIDController angle_controller = new PIDController(0.0052, 0.0, 0.0);

    CommandGroupBase.sequence(
        new RunCommand(() -> {
          double output = distance_controller.calculate(
              (l_enc_.getDistance() + r_enc_.getDistance()) / 2.0, 1.0);
          drive_.arcadeDrive(output, 0);
        }).withTimeout(2.0).andThen(() -> drive_.stopMotor()),

        new InstantCommand(() -> System.out.printf(
            "L: %.2f, R: %.2f\n", l_enc_.getDistance(), r_enc_.getDistance())),

        new RunCommand(() -> {
          double output = angle_controller.calculate(
              gyro_.getAngle(), -90.0);
          drive_.arcadeDrive(0, output);
        }).withTimeout(2.0).andThen(() -> drive_.stopMotor()),

        new InstantCommand(() -> System.out.printf(
            "L: %.2f, R: %.2f\n", l_enc_.getDistance(), r_enc_.getDistance()))
    ).schedule();
  }

  @Override
  public void autonomousPeriodic() {}

  @Override
  public void teleopInit() {}

  @Override
  public void teleopPeriodic() {}

  @Override
  public void disabledInit() {
    drive_.stopMotor();
  }

  @Override
  public void disabledPeriodic() {}

  @Override
  public void testInit() {}

  @Override
  public void testPeriodic() {}

  @Override
  public void simulationPeriodic() {
    // Update drivetrain simulator.
    drive_sim_.setInputs(l_leader_.get() * RobotController.getInputVoltage(),
        -r_leader_.get() * RobotController.getInputVoltage());
    drive_sim_.update(0.02);

    SmartDashboard.putNumber("L Encoder", drive_sim_.getLeftPositionMeters());
    SmartDashboard.putNumber("L Setpoint", l_leader_.get());

    // Update sensors
    l_enc_sim_.setDistance(drive_sim_.getLeftPositionMeters());
    l_enc_sim_.setRate(drive_sim_.getLeftVelocityMetersPerSecond());

    r_enc_sim_.setDistance(drive_sim_.getRightPositionMeters());
    r_enc_sim_.setRate(drive_sim_.getRightVelocityMetersPerSecond());

    gyro_sim_.setAngle(-drive_sim_.getHeading().getDegrees());
//    field_.setRobotPose(drive_sim_.getPose());
  }
}