public Shooter() throws CANTimeoutException {
loadingStick = new CANJaguar(5);
// http://www.chiefdelphi.com/forums/showthread.php?t=101737
// Configuring firstShootMotor in order to control speed with encoder
firstShootingMotor = new CANJaguar(6, CANJaguar.ControlMode.kSpeed);
firstShootingMotor.configEncoderCodesPerRev(256);
firstShootingMotor.enableControl();
firstShootingMotor.changeControlMode(CANJaguar.ControlMode.kVoltage);
firstShootingMotor.enableControl();
// Configuring secondShootMotor in order to control speed with encoder
secondShootingMotor = new CANJaguar(7, CANJaguar.ControlMode.kSpeed);
secondShootingMotor.configEncoderCodesPerRev(256);
secondShootingMotor.enableControl();
secondShootingMotor.changeControlMode(CANJaguar.ControlMode.kVoltage);
secondShootingMotor.enableControl();
angleMotor = new CANJaguar(8);
frontSwitch = new DigitalInput(1);
backSwitch = new DigitalInput(1);
table = NetworkTable.getTable("shooter");
table.putNumber("firstSpeed", 0);
table.putNumber("angle", 0);
}
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);
}
/** Common initialization code called by all constructors. */
private void initCANJaguar() throws CANTimeoutException {
if (m_deviceNumber < 1 || m_deviceNumber > 63) {
throw new RuntimeException(
"Invalid CAN device number \"" + m_deviceNumber + "\" - must be between 1 and 63.");
}
// VxWorks semaphore for calling async CAN receive API.
// Semaphore.Options options = new Semaphore.Options();
// options.setPrioritySorted(true);
// m_receiveSemaphore = new Semaphore(options, false);
int fwVer = getFirmwareVersion();
if (fwVer >= CANJaguarVersionException.kMinRDKFirmwareVersion
|| fwVer < CANJaguarVersionException.kMinLegalFIRSTFirmwareVersion) {
throw new CANJaguarVersionException(m_deviceNumber, fwVer);
}
switch (m_controlMode.value) {
case ControlMode.kPercentVbus_val:
case ControlMode.kVoltage_val:
enableControl();
break;
default:
break;
}
m_safetyHelper = new MotorSafetyHelper(this);
}
/** 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();
}
}
/**
* Set the output set-point value.
*
* <p>The scale and the units depend on the mode the Jaguar is in. In PercentVbus Mode, the
* outputValue is from -1.0 to 1.0 (same as PWM Jaguar). In Voltage Mode, the outputValue is in
* Volts. In Current Mode, the outputValue is in Amps. In Speed Mode, the outputValue is in
* Rotations/Minute. In Position Mode, the outputValue is in Rotations.
*
* @param outputValue The set-point to sent to the motor controller.
* @param syncGroup The update group to add this set() to, pending updateSyncGroup(). If 0, update
* immediately.
*/
public void setX(double outputValue, byte syncGroup) throws CANTimeoutException {
int messageID = 0;
byte[] dataBuffer = new byte[8];
byte dataSize = 0;
if (!m_safetyHelper.isAlive()) {
enableControl();
}
switch (m_controlMode.value) {
case ControlMode.kPercentVbus_val:
messageID = JaguarCANProtocol.LM_API_VOLT_T_SET;
if (outputValue > 1.0) outputValue = 1.0;
if (outputValue < -1.0) outputValue = -1.0;
packPercentage(dataBuffer, outputValue);
dataSize = 2;
break;
case ControlMode.kSpeed_val:
{
messageID = JaguarCANProtocol.LM_API_SPD_T_SET;
dataSize = packFXP16_16(dataBuffer, outputValue);
}
break;
case ControlMode.kPosition_val:
{
messageID = JaguarCANProtocol.LM_API_POS_T_SET;
dataSize = packFXP16_16(dataBuffer, outputValue);
}
break;
case ControlMode.kCurrent_val:
{
messageID = JaguarCANProtocol.LM_API_ICTRL_T_SET;
dataSize = packFXP8_8(dataBuffer, outputValue);
}
break;
case ControlMode.kVoltage_val:
{
messageID = JaguarCANProtocol.LM_API_VCOMP_T_SET;
dataSize = packFXP8_8(dataBuffer, outputValue);
}
break;
default:
return;
}
if (syncGroup != 0) {
dataBuffer[dataSize] = syncGroup;
dataSize++;
}
setTransaction(messageID, dataBuffer, dataSize);
m_safetyHelper.feed();
}
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;
}
/**
* Enable the closed loop controller.
*
* <p>Start actually controlling the output based on the feedback.
*/
public void enableControl() throws CANTimeoutException {
enableControl(0.0);
}