package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.Servo;

public class Robot extends TimedRobot {
  // variables to hold the objects used
  private XboxController xbox;
  private Servo servo;

  /**
   * This function is run when the robot is first started up and should be
   * used for any initialization code.  Create (instantiate) your objects here.
   */
  @Override
  public void robotInit() {
    // instantiate (create) the contained objects and store them in the variables
    xbox = new XboxController(0);  // First Xbox/game controller
    servo = new Servo(2);          // Servo connected to PWM2
  }

  /**
   * This function is called every robot packet, no matter the mode. Use
   * this for items like diagnostics that you want ran during disabled,
   * autonomous, teleoperated and test.
   *
   * <p>This runs after the mode specific periodic functions, but before
   * LiveWindow and SmartDashboard integrated updating.
   */
  @Override
  public void robotPeriodic() {
  }

  /**
   */
  @Override
  public void autonomousInit() {
  }

  /**
   * This function is called periodically during autonomous.
   */
  @Override
  public void autonomousPeriodic() {
  }

  /**
   * This function is called periodically during operator control.
   */
  @Override
  public void teleopPeriodic() {
      if (xbox.getXButton()) {
          // X button moves servo all the way left
          servo.set(-1.0);
      } else if (xbox.getYButton()) {
          // Y button moves servo all the way right
          servo.set(1.0);
      } else {
        // If neither X nor Y are pressed, servo position is
        // controlled by the left joystick X-axis position
        // Read xbox controller left joystick x axis (axis 0)
        // value returned is between -1.0 and 1.0
        double x = xbox.getRawAxis(0); 
        // Servo motors are controlled on a scale of 0.0 to 1.0
        // so re-scale the X value to that range (-1.0->0, 0->0.5, 1.0->1.0)
        x = (x + 1.0) / 2;
        // Set servo motor position based on joystick
        servo.set(x); 
      }
  }

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