/*
* 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.Servo;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.BridgeArmStop;
/**
*
* @author Driver
*/
public class BridgeArm extends Subsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
private Jaguar bridgeArm_Jag;
public Servo bridgeArmLock;
private double locked = 1.0;
private double unlocked = -1.0;
public BridgeArm() {
super("BridgeArm");
bridgeArm_Jag = new Jaguar(RobotMap.BRIDGE_PUSHER);
bridgeArmLock = new Servo(RobotMap.BRIDGE_ARM_LOCK_MODULE, RobotMap.BRIDGE_ARM_LOCK);
}
public void initDefaultCommand() {
// Set the default command for a subsystem here.
//setDefaultCommand(new MySpecialCommand());
setDefaultCommand(new BridgeArmStop());
}
public void bridgeArmLock() {
bridgeArmLock.set(locked);
}
public void bridgeArmUnlock() {
bridgeArmLock.set(unlocked);
}
public void lockGetCurPosition() {
SmartDashboard.putDouble("Lock", bridgeArmLock.get());
}
public void bridgeArmStop()
{
bridgeArm_Jag.set(0.0);
}
public void raiseArm() {
int flipped = 1;
if(RobotMap.BRIDGE_PUSHER_REVERSED) {
flipped = -1;
}
bridgeArm_Jag.set((RobotMap.BRIDGE_ARM_SPEED * flipped));
}
public void lowerArm() {
int flipped = 1;
if(RobotMap.BRIDGE_PUSHER_REVERSED) {
flipped = -1;
}
bridgeArm_Jag.set(-(RobotMap.BRIDGE_ARM_SPEED * flipped));
}
public void setSpeed(double speed) {
int flipped = 1;
if(RobotMap.BRIDGE_PUSHER_REVERSED) {
flipped = -1;
}
bridgeArm_Jag.set(1.0 * flipped);
}
}