/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Talon;

/**
 * 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 {
  private Talon leftMotor, rightMotor;
  private Encoder leftEncoder, rightEncoder;

  /**
   * This function is run when the robot is first started up and should be
   * used for any initialization code.
   */
  @Override
  public void robotInit() {
   // Quadrature encoder has A and B channels connected to DIO0,1, DIO2,3
   leftEncoder = new Encoder(0, 1);
   rightEncoder = new Encoder(2, 3);
   // Peanut wheels are 7.5" in diameter
   // Encoders provide 1 count per degree of rotation
   leftEncoder.setDistancePerPulse((3.141592*7.5)/360);
   rightEncoder.setDistancePerPulse((3.141592*7.5)/360);
   // PWM motor controllers connected to PWM0, PWM1
   leftMotor = new Talon(0);
   rightMotor = new Talon(1);
  }

  /**
   * 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() {
    SmartDashboard.putNumber("Left", leftEncoder.getDistance());
    SmartDashboard.putNumber("Right", rightEncoder.getDistance());
  }

  /**
   */
  @Override
  public void autonomousInit() {
    // Reset encoder counts to 0
    leftEncoder.reset();
    rightEncoder.reset();
    // start motors turning forward at 20% power
    leftMotor.set(0.20);
    // to go forward, left motor turns clockwise, right motor counter-clockwise
    rightMotor.set(-0.20);
  }

  /**
   * This function is called periodically during autonomous.
   */
  @Override
  public void autonomousPeriodic() {
    if ((leftEncoder.getDistance() > 36.0) || 
        (rightEncoder.getDistance() > 36.0)) {
      leftMotor.stopMotor();
      rightMotor.stopMotor();
    }        
  }

  /**
   * This function is called periodically during operator control.
   */
  @Override
  public void teleopPeriodic() {
  }

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