// Loads the frisbee into the shooter
public void load() throws CANTimeoutException {
if (frontSwitch.get()) {
loadingStick.setX(0);
} else {
loadingStick.setX(.8);
}
}
public void autoShoot() {
try {
tester1.setX(10.3); // 9.5 8.2 2 pt
tester2.setX(7.8);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
/**
* Run the rollers at opposite speeds to articulate the tubes up/down
*
* @param speed The speed to run the rollers
*/
public void articulate(double speed) {
try {
rollerA.setX(-speed * .75); // Motors are different... scale one
rollerB.setX(speed);
} catch (Exception e) {
e.printStackTrace();
}
}
// Resets position in order to reload frisbee later
public void reload() throws CANTimeoutException {
if (backSwitch.get()) {
loadingStick.setX(0);
} else {
loadingStick.setX(-.8);
}
}
/** 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 autoShootThreeSide() { // shoot middle
try {
tester1.setX(10.6);
tester2.setX(8.5);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
public void autoShootMid() { // shoot middle
try {
tester1.setX(10.5);
tester2.setX(8.0); // 9.2
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
/** Disables the climber closed loop puts it into open loop. */
public void disableClosedLoop() {
if (ClimbJag != null) {
try {
ClimbJag.disableControl();
ClimbJag.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
m_closedLoop = false;
} catch (CANTimeoutException ex) {
m_fault = true;
System.err.println("CAN timeout");
}
}
}
/** Constructor */
public RollerClaw() {
try {
rollerA = new CANJaguar(3); // Create CANJaguar objects on addresses
rollerB = new CANJaguar(9); // 3 and 9
rollerA.configNeutralMode(CANJaguar.NeutralMode.kCoast); // Set them in coast mode
rollerB.configNeutralMode(CANJaguar.NeutralMode.kCoast);
} catch (Exception e) {
e.printStackTrace();
System.out.println("ERROR INITIALIZING ROLLER CLAW");
}
limit = new DigitalInput(1); // Roller claw switch on DI1
}
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);
}
/** 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");
}
}
}
public void robotInit() {
try {
drive_control = new Joystick(1);
control = new Joystick(2);
drive = new Drive();
station = DriverStationLCD.getInstance();
camera = AxisCamera.getInstance();
camera.writeResolution(AxisCamera.ResolutionT.k320x240);
camera.writeBrightness(0);
camera.writeMaxFPS(10);
launcher = new CANJaguar(7);
belt1 = new Relay(1);
// belt1.setDirection(Relay.Direction.kBoth);
belt2 = new CANJaguar(8);
turret = new Victor(4);
/*launcher.setPID(0.5, 0.001, 0.0);
launcher.configEncoderCodesPerRev(360);
launcher.changeControlMode(CANJaguar.ControlMode.kSpeed);
launcher.enableControl();*/
launcher.configEncoderCodesPerRev(360);
launcher.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
launcher.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
/*wheelEncoder = new Encoder(1, 2, true, CounterBase.EncodingType.k4X);
wheelEncoder.setDistancePerPulse(1);
wheelEncoder.setReverseDirection(true);
wheelEncoder.start();*/
csc = new CANSpeedController(.02, 0, 0, launcher);
csc.setInputRange(0, 3000);
csc.enable();
timer = new Timer();
// tracking = new EagleEye();
lights = new Relay(3);
/*try {
serial = new SerialPort(115200);
} catch (Exception ex) {
System.out.println("Cannot open serial connection " + ex);
}*/
} catch (CANTimeoutException ex) {
System.out.println(ex);
}
wedge = new wedge(2, 1, 2);
// servo1 = new Servo(1);
// servo2 = new Servo(2);
// wedge = new Relay(2);
}
public void setIntake(double power) {
try {
arm.setX(power);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
/** @param armPos the armPos to set */
public void setArmPos(int armPos) {
this.armPos = armPos;
try {
double p = 0;
switch (armPos) {
case 0:
// target = PIDConstants.armIntake;
target = preferences.getDouble("intake", 0);
p = 2;
break;
case 1:
// target = PIDConstants.armFeed;
target = preferences.getDouble("feed", 0);
p = 2.8;
break;
case 2:
// target = PIDConstants.armStow;
target = preferences.getDouble("stow", 0);
// SmartDashboard.putNumber("armCurrent", arm.getOutputCurrent());
p = 4;
break;
}
double change = arm.getPosition() - target;
// SmartDashboard.putNumber("armPosition", arm.getPosition());
// System.out.println(arm.getPosition());
// arm.setX(change * p*0.9);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
public void setSpeed(double rpm) throws CANTimeoutException {
// Right now, we're using voltage control mode
// guess voltage to rpm relationship
double voltage = rpm / 0;
// Check to see if 'rpm' works
if ((firstShootingMotor.getControlMode() == CANJaguar.ControlMode.kVoltage)
&& (secondShootingMotor.getControlMode() == CANJaguar.ControlMode.kVoltage)) {
firstShootingMotor.setX(voltage);
} else {
firstShootingMotor.setX(rpm);
secondShootingMotor.setX(rpm);
}
}
/**
* Sets the target angle of the flipper in degrees. 0 is parallel to the ground.
*
* @param angle The target angle of the flipper.
*/
public void setFlippers(double angle) {
try {
jagFlip.setX(4.3 + ElectricalConstants.potDtoV * angle);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
public void shoot(double speed) {
try {
launcher.setX(speed);
} catch (CANTimeoutException ex) {
System.out.println("Cannot launch " + ex);
}
}
/**
* Gets the current angle of the flipper.
*
* @return The current angle of the flipper.
*/
public double getPos() {
try {
return jagFlip.getPosition();
} catch (CANTimeoutException ex) {
ex.printStackTrace();
return 0;
}
}
/** Shooter conveyor goes in reverse at feeder station */
public static void feedMode() {
try {
shooterConveyor.setX(Constants.CONV_SHOOTER_POWER);
} catch (CANTimeoutException ex) {
System.out.println("CAN TIMEOUT EXCEPTION ON SHOOTER CONVEYOR");
Log.write("CANJag Timeout Exception on Shooter Conveyor");
}
}
/**
* Get the output current of motor b
*
* @return Current
*/
public double getCurrentB() {
try {
return rollerB.getOutputCurrent();
} catch (Exception e) {
new ExceptionHandler(e, "Roller Claw").print();
return -0;
}
}
/** Ingestor conveyor stops */
public static void stopIngest() {
try {
ingestConveyor.setX(0.0);
} catch (CANTimeoutException ex) {
System.out.println("CAN TIMEOUT EXCEPTION ON INGEST CONVEYOR");
Log.write("CANJag Timeout Exception on Ingest Conveyor");
}
}
/** Stops the shooter */
public static void stopShooter() {
try {
shooterConveyor.setX(0.0);
} catch (CANTimeoutException ex) {
System.out.println("CAN TIMEOUT EXCEPTION ON SHOOTER CONVEYOR");
Log.write("CANJag Timeout Exception on Shooter Conveyor");
}
}
public double getChange() {
try {
return arm.getPosition() - target;
} catch (CANTimeoutException ex) {
ex.printStackTrace();
return 0;
}
}
public double getSpeed(int motorNumber) throws CANTimeoutException {
double speed = -1;
if (motorNumber == 1) {
speed = firstShootingMotor.getSpeed();
table.putNumber("firstSpeed", speed);
}
if (motorNumber == 2) {
speed = secondShootingMotor.getSpeed();
table.putNumber("secondSpeed", speed);
}
return speed;
}
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);
}
/**
* Run a motor at the set speed
*
* @param motor 0 for motorA, 1 for motorB
* @param speed the speed to run said motor
*/
public void setRaw(int motor, double speed) {
switch (motor) {
case 0:
try {
rollerA.setX(speed);
} catch (Exception e) {
e.printStackTrace();
}
break;
case 1:
try {
rollerB.setX(speed);
} catch (Exception e) {
e.printStackTrace();
}
break;
}
}
public void drive() {
try {
magOne = this.oi.getXJoystick().getY();
if (Math.abs(magOne) < deadZone) {
magOne = 0;
}
if (Math.abs(magTwo) < deadZone) {
magTwo = 0;
}
speedOne = magOne * speedmultOne; // 4500
speedTwo = magTwo * speedmultTwo; // 14000
tspeedTwo = magOne * speedmultTwo;
tester1.setX(-speedOne);
tester2.setX(-tspeedTwo);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
/**
* Constructor
*
* @param motor number for the motor
* @param pnumatics pass in a pnumatic object
*/
public Arm(int motor, Pnumatics pnumatics, JoystickListener js) {
try {
jag = new CANJaguar(motor, CANJaguar.ControlMode.kPercentVbus);
jag.configNeutralMode(CANJaguar.NeutralMode.kBrake);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
this.pnumatics = pnumatics;
this.js = js;
}
public Climber() {
try {
ClimbJag = new CANJaguar(Wiring.climberCANID);
if (ClimbJag != null) {
// encoder configuration
ClimbJag.configEncoderCodesPerRev(ENCODER_LINES);
ClimbJag.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
ClimbJag.configNeutralMode(CANJaguar.NeutralMode.kBrake);
// We need this ramp rate. Without it rapid reversal causes transients
ClimbJag.setVoltageRampRate(20);
}
} catch (CANTimeoutException ex) {
m_fault = true;
System.out.println("*** Climber CAN Error ***");
}
calibrateEncoder();
}
/** Unjams the ingestor conveyor */
public static void shakeIngest() {
timer.start();
while (timer.get() < 2) {
if (direction) {
try {
ingestConveyor.setX(Constants.CONV_INGEST_POWER);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
} else {
try {
ingestConveyor.setX(-Constants.CONV_INGEST_POWER);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
}
direction = !direction;
timer.reset();
}
