public Intake() {
preferences = Preferences.getInstance();
leftGate =
new DoubleSolenoid(
ElectricalConstants.rightGateForward, ElectricalConstants.rightGateReverse);
rightGate = new Solenoid(ElectricalConstants.leftGate);
try {
arm = new CANJaguar(ElectricalConstants.armJag);
arm.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
arm.configNeutralMode(CANJaguar.NeutralMode.kBrake);
arm.configFaultTime(0.5);
arm.configPotentiometerTurns(1);
arm.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
arm.setPID(0, 0, 0);
arm.enableControl(0);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
rollers = new Victor(ElectricalConstants.rollerVic);
}
/** Resets the PID values to the values previously set. */
public void resetPID() {
try {
jagFlip.setPID(p, i, d);
jagFlip.enableControl(0);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
public void reInit() {
try {
jagFlip = new CANJaguar(ElectricalConstants.JagFlipper);
jagFlip.changeControlMode(CANJaguar.ControlMode.kPosition);
jagFlip.configFaultTime(0.5);
jagFlip.configNeutralMode(CANJaguar.NeutralMode.kBrake);
jagFlip.configPotentiometerTurns(10);
jagFlip.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
jagFlip.setPID(p, i, d);
jagFlip.enableControl(0);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
/** Sets up the climber closed loop and puts it in Position mode. */
public void enableClosedLoop() {
if (ClimbJag != null) {
try {
ClimbJag.changeControlMode(CANJaguar.ControlMode.kPosition);
ClimbJag.setPID(150.0, 0.0, 0.0);
double position = ClimbJag.getPosition();
ClimbJag.enableControl(position);
m_closedLoop = true;
} catch (CANTimeoutException ex) {
m_fault = true;
System.err.println("CAN timeout");
}
}
}
private Flipper() {
p = PIDConstants.flipperP;
i = PIDConstants.flipperI;
d = PIDConstants.flipperD;
try {
jagFlip = new CANJaguar(ElectricalConstants.JagFlipper);
jagFlip.changeControlMode(CANJaguar.ControlMode.kPosition);
jagFlip.configFaultTime(0.5);
jagFlip.configNeutralMode(CANJaguar.NeutralMode.kBrake);
jagFlip.configPotentiometerTurns(10);
jagFlip.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
jagFlip.setPID(p, i, d);
jagFlip.enableControl(0);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
gotoPosition();
}
public CANJaguar construct() {
CANJaguar jag = null;
try {
jag = new CANJaguar(channel, controlMode);
// System.out.println("-----------CREATING JAGUAR AT CHANNEL " + channel +
// "----------------");
// System.out.println("Control Mode:\t\t" + controlMode);
if (usingPID) {
// System.out.println("Speed Reference:\t\t" + speedReference);
jag.setSpeedReference(speedReference);
// System.out.println("Encoder Codes Per Rev:\t\t" + encoderCodesPerRev);
jag.configEncoderCodesPerRev(encoderCodesPerRev);
// System.out.println("P:\t\t" + p);
// System.out.println("I:\t\t" + i);
// System.out.println("D:\t\t" + d);
// System.out.println("Verifying PID:");
// System.out.println("P:\t\t" + jag.getP());
// System.out.println("I:\t\t" + jag.getI());
// System.out.println("D:\t\t" + jag.getD());
jag.setPID(p, i, d);
jag.enableControl();
if (p == 0) {
System.out.println("[WARNING] Created a CANJaguar with P=0!");
}
if (encoderCodesPerRev == 0) {
System.out.println("[WARNING] Created a CANJaguar with encoder codes per revolution=0!");
}
}
// System.out.println("-----------Motor Create Complete----------");
} catch (CANTimeoutException e) {
e.printStackTrace();
}
return jag;
}