Add testing stuff

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / subsystems / Scaler.java

package org.usfirst.frc.team3501.robot.subsystems;

import org.usfirst.frc.team3501.robot.Constants;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.command.Subsystem;

public class Scaler extends Subsystem {
  private DoubleSolenoid scaler;
  private CANTalon winch;

  public Scaler() {
    scaler = new DoubleSolenoid(9, Constants.Scaler.FORWARD_CHANNEL,
        Constants.Scaler.REVERSE_CHANNEL);
    winch = new CANTalon(Constants.Scaler.WINCH_MOTOR);
  }

  public Value getSolenoidValue() {
    return scaler.get();
  }

  public void liftScissorLift() {
    scaler.set(DoubleSolenoid.Value.kReverse);
  }

  public void lowerScissorLift() {
    scaler.set(DoubleSolenoid.Value.kForward);
  }

  public void runWinch(double speed) {
    if (speed > 1)
      speed = 1;
    if (speed < -1)
      speed = -1;

    winch.set(speed);
  }

  public void stopWinch() {
    runWinch(Constants.Scaler.WINCH_STOP_SPEED);
  }

  @Override
  protected void initDefaultCommand() {

  }
}