/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package edu.wpi.first.wpilibj.templates.subsystems;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.OI;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.ShooterStopMotors;
/**
*
* @author Eddie
*/
public class BallShooter extends Subsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
Jaguar leftShooterMotor;
Jaguar rightShooterMotor;
public boolean shooterState;
public BallShooter() {
super("BallShooter");
leftShooterMotor = new Jaguar(RobotMap.SHOOTER_LEFT);
rightShooterMotor = new Jaguar(RobotMap.SHOOTER_RIGHT);
shooterState = false;
}
public void initDefaultCommand() {
// Set the default command for a subsystem here.
setDefaultCommand(new ShooterStopMotors());
}
public void spinWheels(boolean forward) {
int leftMultiplier = 1;
int rightMultiplier = 1;
if(RobotMap.SHOOTER_LEFT_REVERSED) {
leftMultiplier = -1;
}
if(RobotMap.SHOOTER_RIGHT_REVERSED) {
rightMultiplier = -1;
}
if(forward) {
leftShooterMotor.set(RobotMap.SHOOTER_MAX_SPEED * leftMultiplier);
rightShooterMotor.set(RobotMap.SHOOTER_MAX_SPEED * rightMultiplier);
} else {
leftShooterMotor.set(-RobotMap.SHOOTER_MAX_SPEED * leftMultiplier);
rightShooterMotor.set(-RobotMap.SHOOTER_MAX_SPEED * rightMultiplier);
}
}
public void spinWheels(double speed) {
int leftMultiplier = 1;
int rightMultiplier = 1;
if(RobotMap.SHOOTER_LEFT_REVERSED) {
leftMultiplier = -1;
}
if(RobotMap.SHOOTER_RIGHT_REVERSED) {
rightMultiplier = -1;
}
leftShooterMotor.set(speed * leftMultiplier);
rightShooterMotor.set(speed * rightMultiplier);
}
public void stopWheels() {
if(!shooterState) {
leftShooterMotor.set(0.0);
rightShooterMotor.set(0.0);
} else {
spinWheels(RobotMap.SHOOTER_MAX_SPEED);
}
}
public void changeSpeed(double change) {
if(RobotMap.SHOOTER_MAX_SPEED + change <= 1 && RobotMap.SHOOTER_MAX_SPEED >=0) {
RobotMap.SHOOTER_MAX_SPEED += change;
} else {
System.out.println("BallShooter - Value greater than 1 or less than 0");
}
}
public void setSpeed(double speed) {
if(speed > 0 && speed <= 1) {
RobotMap.SHOOTER_MAX_SPEED = speed;
}
}
public void updateStatus() {
SmartDashboard.putDouble("Shooter Left Motor", leftShooterMotor.getSpeed());
SmartDashboard.putDouble("Shooter Right Motor", rightShooterMotor.getSpeed());
}
}