/*
* 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.AnalogChannel;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.BallPickupStop;
/**
*
* @author Eddie
*/
public class BallPickup extends Subsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
Jaguar leftPickupMotor_Lower;
Jaguar rightPickupMotor_Lower;
Jaguar pickupMotor_Upper;
public void initDefaultCommand() {
// Set the default command for a subsystem here.
//setDefaultCommand(new MySpecialCommand());
setDefaultCommand(new BallPickupStop());
}
public BallPickup() {
super("BallPickup");
leftPickupMotor_Lower = new Jaguar(RobotMap.PICKUP_LEFT_LOWER);
rightPickupMotor_Lower = new Jaguar(RobotMap.PICKUP_RIGHT_LOWER);
pickupMotor_Upper = new Jaguar(RobotMap.PICKUP_UPPER);
}
public void pickupBall() {
int leftMultiplier = 1;
int rightMultiplier = 1;
int upperMultiplier = 1;
if(RobotMap.PICKUP_LEFT_REVERSED) {
leftMultiplier = -1;
}
if(RobotMap.PICKUP_RIGHT_REVERSED) {
rightMultiplier = -1;
}
if(RobotMap.PICKUP_UPPER_REVERSED) {
upperMultiplier = -1;
}
// Use the speed found in robot map and whether or not the motor should be reflected
// To set to pickup motor values
leftPickupMotor_Lower.set(RobotMap.PICKUP_MAX_SPEED * leftMultiplier);
rightPickupMotor_Lower.set(RobotMap.PICKUP_MAX_SPEED * rightMultiplier);
pickupMotor_Upper.set(RobotMap.PICKUP_MAX_SPEED * upperMultiplier);
}
public void pickupLower() {
int leftMultiplier = 1;
int rightMultiplier = 1;
if(RobotMap.PICKUP_LEFT_REVERSED) {
leftMultiplier = -1;
}
if(RobotMap.PICKUP_RIGHT_REVERSED) {
rightMultiplier = -1;
}
// Use the speed found in robot map and whether or not the motor should be reflected
// To set to pickup motor values
leftPickupMotor_Lower.set(RobotMap.PICKUP_MAX_SPEED * leftMultiplier);
rightPickupMotor_Lower.set(RobotMap.PICKUP_MAX_SPEED * rightMultiplier);
}
public void pickupUpper() {
int upperMultiplier = 1;
if(RobotMap.PICKUP_UPPER_REVERSED) {
upperMultiplier = -1;
}
// Use the speed found in robot map and whether or not the motor should be reflected
// To set to pickup motor values
pickupMotor_Upper.set(RobotMap.PICKUP_MAX_SPEED * upperMultiplier);
}
public void reverseLower() {
int leftMultiplier = 1;
int rightMultiplier = 1;
if(RobotMap.PICKUP_LEFT_REVERSED) {
leftMultiplier = -1;
}
if(RobotMap.PICKUP_RIGHT_REVERSED) {
rightMultiplier = -1;
}
// Use the speed found in robot map and whether or not the motor should be reflected
// To set to pickup motor values
leftPickupMotor_Lower.set(-(RobotMap.PICKUP_MAX_SPEED * leftMultiplier));
rightPickupMotor_Lower.set(-(RobotMap.PICKUP_MAX_SPEED * rightMultiplier));
}
public void reverseUpper() {
int upperMultiplier = 1;
if(RobotMap.PICKUP_UPPER_REVERSED) {
upperMultiplier = -1;
}
// Use the speed found in robot map and whether or not the motor should be reflected
// To set to pickup motor values
pickupMotor_Upper.set(-(RobotMap.PICKUP_MAX_SPEED * upperMultiplier));
}
public void reverse() {
int leftMultiplier = 1;
int rightMultiplier = 1;
int upperMultiplier = 1;
if(RobotMap.PICKUP_LEFT_REVERSED) {
leftMultiplier = -1;
}
if(RobotMap.PICKUP_RIGHT_REVERSED) {
rightMultiplier = -1;
}
if(RobotMap.PICKUP_UPPER_REVERSED) {
upperMultiplier = -1;
}
// Use the speed found in robot map and whether or not the motor should be reflected
// To set to pickup motor values
leftPickupMotor_Lower.set(-(RobotMap.PICKUP_MAX_SPEED * leftMultiplier));
rightPickupMotor_Lower.set(-(RobotMap.PICKUP_MAX_SPEED * rightMultiplier));
pickupMotor_Upper.set(-(RobotMap.PICKUP_MAX_SPEED * upperMultiplier));
}
public void stopMotors() {
leftPickupMotor_Lower.set(0.0); //
rightPickupMotor_Lower.set(0.0);
pickupMotor_Upper.set(0.0); //
}
}