add command to robot.java to actually run the LimitSwitchTest

[3501/2015-FRC-Spark] / src / org / usfirst / frc3501 / RiceCatRobot / Robot.java

package org.usfirst.frc3501.RiceCatRobot;

import org.usfirst.frc3501.RiceCatRobot.commands.LimitSwitchTest;
import org.usfirst.frc3501.RiceCatRobot.subsystems.Arm;
import org.usfirst.frc3501.RiceCatRobot.subsystems.Claw;
import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Scheduler;

public class Robot extends IterativeRobot {
  static Timer timer;
  public static OI oi;
  public static DriveTrain driveTrain;
  public static Arm arm;
  public static Claw claw;
  public static Compressor compressor;

  public void robotInit() {
    RobotMap.init();
    driveTrain = new DriveTrain();
    arm = new Arm();
    claw = new Claw();
    oi = new OI();
    compressor = new Compressor(RobotMap.COMPRESSOR_PORT);
  }

  public void autonomousInit() {
  }

  public void autonomousPeriodic() {
    Scheduler.getInstance().run();
  }

  public void teleopInit() {
    System.out.println("running teleopInit");
    Scheduler.getInstance().add(new LimitSwitchTest());
  }

  public void teleopPeriodic() {
    Scheduler.getInstance().run();

  }

  public void operate() {
    driveTrain
        .arcadeDrive(OI.rightJoystick.getY(), OI.rightJoystick.getTwist());
    claw.doTriggerAction();
    if (OI.leftJoystick.getRawButton(8)) {
      arm.setArmSpeeds(0.3);
    } else if (OI.leftJoystick.getRawButton(9)) {
      arm.setArmSpeeds(-0.3);
    } else if (OI.leftJoystick.getRawButton(6)) {
      arm.setLeft(0.3);
    } else if (OI.leftJoystick.getRawButton(7)) {
      arm.setLeft(-0.3);
    } else if (OI.leftJoystick.getRawButton(11)) {
      arm.setRight(-0.3);
    } else if (OI.leftJoystick.getRawButton(10)) {
      arm.setRight(0.3);
    }
    if (Math.abs(OI.leftJoystick.getY()) < 0.05) {
      arm.setArmSpeeds(0);

    } else {
      arm.fineTuneControl(OI.leftJoystick.getY());
    }
  }
}