Andrew-Code

/*----------------------------------------------------------------------------*/
/* 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 org.usfirst.frc.team6468.robot;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import com.ctre.phoenix.motorcontrol.can.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the build.properties file in the
 * project.
 */
@SuppressWarnings("unused")
public class Robot extends IterativeRobot {
	private static final String kDefaultAuto = "Default";
	private static final String kCustomAuto = "My Auto";
	private String m_autoSelected;
	private SendableChooser<String> m_chooser = new SendableChooser<>();
	private XboxController xbox = new XboxController(0); 
	private Joystick joystick = new Joystick(0); 
	private JoystickButton button = new JoystickButton(joystick, 0);
	private Servo servo = new Servo(0);
	
	WPI_TalonSRX _leftSlave1 = new WPI_TalonSRX(11);
	WPI_TalonSRX _rightSlave1 = new WPI_TalonSRX(12);
	WPI_TalonSRX _frontLeftMotor = new WPI_TalonSRX(13);
	WPI_TalonSRX _frontRightMotor = new WPI_TalonSRX(14);
	WPI_TalonSRX _slideTrainMotor = new WPI_TalonSRX(15);
	DifferentialDrive _drive = new DifferentialDrive(_frontLeftMotor, _frontRightMotor);

	/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	@Override
	public void robotInit() {
		m_chooser.addDefault("Default Auto", kDefaultAuto);
		m_chooser.addObject("My Auto", kCustomAuto);
		SmartDashboard.putData("Auto choices", m_chooser);
		_rightSlave1.follow(_frontRightMotor);
		_leftSlave1.follow(_frontLeftMotor);
		
	}

	/**
	 * This autonomous (along with the chooser code above) shows how to select
	 * between different autonomous modes using the dashboard. The sendable
	 * chooser code works with the Java SmartDashboard. If you prefer the
	 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
	 * getString line to get the auto name from the text box below the Gyro
	 *
	 * <p>You can add additional auto modes by adding additional comparisons to
	 * the switch structure below with additional strings. If using the
	 * SendableChooser make sure to add them to the chooser code above as well.
	 */
	@Override
	public void autonomousInit() {
		m_autoSelected = m_chooser.getSelected();
		// autoSelected = SmartDashboard.getString("Auto Selector",
		// defaultAuto);
		System.out.println("Auto selected: " + m_autoSelected);
	}

	/**
	 * This function is called periodically during autonomous.
	 */
	@Override
	public void autonomousPeriodic() {
		switch (m_autoSelected) {
			case kCustomAuto:
				// Put custom auto code here
				break;
			case kDefaultAuto:
			default:
				// Put default auto code here
				break;
		}
	}

	/**
	 * This function is called periodically during operator control.
	 */
	@Override
	public void teleopPeriodic() {
		double LYaxis = xbox.getY(GenericHID.Hand.kLeft);
		double RXaxis = xbox.getX(GenericHID.Hand.kRight);
		boolean XButton = xbox.getXButton();
		boolean YButton = xbox.getYButton();
		boolean AButton = xbox.getAButton();
		boolean BButton = xbox.getBButton();
		//booleans
		double RTrigger = xbox.getTriggerAxis(GenericHID.Hand.kRight);
		double LTrigger = xbox.getTriggerAxis(GenericHID.Hand.kLeft);
		
		//Arcade Drive
		_drive.arcadeDrive(LYaxis, RXaxis);
		
		//Sliding Drive Train
		if(RTrigger != 0 && LTrigger == 0) _slideTrainMotor.set(RTrigger);
		if(LTrigger != 0 && RTrigger == 0) _slideTrainMotor.set(-LTrigger);
		if(RTrigger == 0 && LTrigger == 0) _slideTrainMotor.set(0);
		
		//Claw
		int D;
		int L;
		double degrees = 1;
		if(AButton == true) for(D = 1; D >= 1; D++) servo.setAngle(degrees++);
		if(AButton == false) D = 0;
		if(BButton == true) for(L = 1; L >= 1; L++) servo.setAngle(degrees--);
		if(BButton == false) L = 0;
		
		//Vertical Lift
		

		}

	}
	//public void driveStraight(double LYaxis, double RXaxis) {
	//	
	//	if(LYaxis != 0) {
	//		_drive.arcadeDrive(LYaxis, rotation, true);
	//	}
	//	if(RXaxis > 0) {
	//		_drive.tankDrive(RXaxis, -RXaxis);
	//	}												As of now is obsolete.
	//	if(RXaxis < 0) {
	//		_drive.tankDrive(-RXaxis, RXaxis);
	//	}
	//		
//	}
	
	//public void slideTrain(double RBumper, double LBumper) {
	//	if(RBumper != 0) {
	//		_slideTrainMotor.set(RBumper);
	//	}
	//	if(LBumper != 0) {
	//		_slideTrainMotor.set(LBumper);
	//	}
	//}
	//public void Claw(boolean AButton, boolean BButton) {
	//	double degrees = 1;
	//
	//	if(AButton == true) {
	//		for(int D = 1; D >= 1; D++) {
	//			servo.setAngle(degrees++);
	//		}
	//	}
	//	if(BButton == true) {
	//		for(int L = 1; L >= 1; L++) {
	//			servo.setAngle(degrees--);
	//		}
	//	}
	//}
	
	@Override
	public void testPeriodic() {
		
	}
}