/*----------------------------------------------------------------------------*/

/* 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.team3498.robot;

import edu.wpi.first.wpilibj.IterativeRobot;

import edu.wpi.first.wpilibj.Joystick;

import edu.wpi.first.wpilibj.Spark;

//import edu.wpi.first.wpilibj.Talon;

import edu.wpi.first.wpilibj.drive.MecanumDrive;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; //GBS Added 02-14-2018

/**

* This is a demo program showing how to use Mecanum control with the RobotDrive

* class.

*/

public class Robot extends IterativeRobot {

private static final int kFrontLeftChannel = 0;

private static final int kRearLeftChannel = 3;

private static final int kFrontRightChannel = 2;

private static final int kRearRightChannel = 1;

private static final int kMotorMain = 4;

private static final int kJoystickChannel = 0;

private MecanumDrive m_robotDrive;

private Joystick m_stick;

@Override

public void robotInit() {

Spark frontLeft = new Spark(kFrontLeftChannel);

Spark rearLeft = new Spark(kRearLeftChannel);

Spark frontRight = new Spark(kFrontRightChannel);

Spark rearRight = new Spark(kRearRightChannel);

// Invert the left side motors.

// You may need to change or remove this to match your robot.

frontLeft.setInverted(true);

rearLeft.setInverted(true);

frontRight.setInverted(true);

rearRight.setInverted(true);

m_robotDrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight);

m_stick = new Joystick(kJoystickChannel);

}

@Override

public void teleopPeriodic() {

// Use the joystick X axis for lateral movement, Y axis for forward

// movement, and Z axis for rotation.

m_robotDrive.driveCartesian(m_stick.getX(), m_stick.getY(), m_stick.getZ(), 0.0);

boolean buttonValue; {

buttonValue = m_stick.getRawButton(1); //Bottom Button

if(buttonValue==true) { SmartDashboard.putNumber("Bottom", 1 ); }

if(buttonValue==false) { SmartDashboard.putNumber("Bottom", 0 ); }

buttonValue = m_stick.getRawButton(2); //Right Button

if(buttonValue==true) { SmartDashboard.putNumber("Right", 1 ); }

if(buttonValue==false) { SmartDashboard.putNumber("Right", 0 ); }

buttonValue = m_stick.getRawButton(3); //Left Button

if(buttonValue==true) { SmartDashboard.putNumber("Left", 1 ); }

if(buttonValue==false) { SmartDashboard.putNumber("Left", 0 ); }

buttonValue = m_stick.getRawButton(4); //Top Button

if(buttonValue==true) { SmartDashboard.putNumber("Top", 1 ); }

if(buttonValue==false) { SmartDashboard.putNumber("Top", 0 ); }

buttonValue = m_stick.getRawButton(5); //Left Trigger Button

if(buttonValue==true) { SmartDashboard.putNumber("Left Trigger", 1 ); }

if(buttonValue==false) { SmartDashboard.putNumber("Left Trigger", 0 ); }

buttonValue = m_stick.getRawButton(6); //Right Trigger Button

if(buttonValue==true) { SmartDashboard.putNumber("Right Trigger", 1 ); }

if(buttonValue==false) { SmartDashboard.putNumber("Right Trigger", 0 ); }

buttonValue = m_stick.getRawButton(7); //Back Button

if(buttonValue==true) { SmartDashboard.putNumber("Back", 1 ); }

if(buttonValue==false) { SmartDashboard.putNumber("Back", 0 ); }

buttonValue = m_stick.getRawButton(8); //Start Button

if(buttonValue==true) { SmartDashboard.putNumber("Start", 1 ); }

if(buttonValue==false) { SmartDashboard.putNumber("Start", 0 ); }

buttonValue = m_stick.getRawButton(9); //Left Joystick Button

if(buttonValue==true) { SmartDashboard.putNumber("Left Joystick", 1 ); }

if(buttonValue==false) { SmartDashboard.putNumber("Left Joystick", 0 ); }

buttonValue = m_stick.getRawButton(10); //Left Joystick Button

if(buttonValue==true) { SmartDashboard.putNumber("Right Joystick", 1 ); }

if(buttonValue==false) { SmartDashboard.putNumber("Right Joystick", 0 ); }

}

buttonValue = m_stick.getRawButton(4); //Top Button

if(buttonValue==true) {

Spark motorMain = new Spark(kMotorMain);

motorMain.set(1);

}

//if(buttonValue==false)

else {

Spark motorMain = new Spark(kMotorMain);

motorMain.set(0);

}

}