public void autoShoot() {
try {
tester1.setX(10.3); // 9.5 8.2 2 pt
tester2.setX(7.8);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
// Resets position in order to reload frisbee later
public void reload() throws CANTimeoutException {
if (backSwitch.get()) {
loadingStick.setX(0);
} else {
loadingStick.setX(-.8);
}
}
/**
* 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();
}
}
// Loads the frisbee into the shooter
public void load() throws CANTimeoutException {
if (frontSwitch.get()) {
loadingStick.setX(0);
} else {
loadingStick.setX(.8);
}
}
public void autoShootMid() { // shoot middle
try {
tester1.setX(10.5);
tester2.setX(8.0); // 9.2
} 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 setIntake(double power) {
try {
arm.setX(power);
} 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);
}
}
/** 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");
}
}
/** 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");
}
}
/** 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");
}
}
/**
* 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();
}
}
/** Unjams the shooter conveyor */
public static void shakeShooter() {
timer.start();
while (timer.get() < 2) {
if (direction) {
try {
shooterConveyor.setX(Constants.CONV_SHOOTER_POWER);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
} else {
try {
shooterConveyor.setX(-Constants.CONV_SHOOTER_POWER);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
}
direction = !direction;
timer.reset();
}
/** 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();
}
/**
* Open loop control for the climb mast, using voltage percentage.
*
* @param power The voltage of the climb motor specified from -1.0 to 1.0.
*/
public void ClimbOpenLoop(double power) {
if (ClimbJag != null) {
try {
// play with stuff under to see if it needs to be inverted
ClimbJag.setX(power * defaultMaxSpeed);
} catch (CANTimeoutException ex) {
m_fault = true;
System.err.println("****************CAN timeout***********");
}
}
}
/**
* Move the arm
*
* @param rpm the speed of the arm
*/
public void moveArm(double speed) {
try {
if (turbomode) {
jag.setX(.6 * (speed));
try {
CANJaguar.updateSyncGroup(syncGroup);
} catch (CANNotInitializedException e) {
}
} else {
jag.setX(.3 * (speed));
try {
CANJaguar.updateSyncGroup(syncGroup);
} catch (CANNotInitializedException e) {
}
}
} catch (CANTimeoutException ex) {
// ex.printStackTrace();
System.out.println("CANNDriveTrain.java:133");
}
if (m_safetyHelper != null) m_safetyHelper.feed();
}
/** This function is called periodically during autonomous */
public void autonomousPeriodic() {
wedge.moveDown();
lights.set(Relay.Value.kForward);
// tracking.run();
try {
driveMainBelt(true, true);
csc.setSetpoint(2300); // 15' = ~2400rpm
if (launcher.getSpeed() < -2250) {
try {
belt2.setX(-1);
} catch (Exception e) {
}
} else {
belt2.setX(0);
}
station.println(DriverStationLCD.Line.kMain6, 1, "RPM: " + String.valueOf(csc.tmpResult));
} catch (CANTimeoutException ex) {
}
/*try{
if(tracking.foundTarget()){
station.println(DriverStationLCD.Line.kUser2, 1, "Distance: " + String.valueOf(tracking.getTarget("HIGH").distance));
station.println(DriverStationLCD.Line.kUser3, 1, "Suggested RPM: " + String.valueOf(tracking.getTarget("HIGH").distance*62.8));
if(x>0)
station.println(DriverStationLCD.Line.kUser4, 1, "Rotation: <--");
else if(x<0)
station.println(DriverStationLCD.Line.kUser4, 1, "Roation: -->");
else
station.println(DriverStationLCD.Line.kUser4, 1, "I don't need to rotate");
}
else{
station.println(DriverStationLCD.Line.kUser2, 1, "Help, I can't find target by myself!");
station.println(DriverStationLCD.Line.kUser3, 1, "");
station.println(DriverStationLCD.Line.kUser4, 1, "");
}
}catch(Exception e){}*/
station.updateLCD();
}
/**
* Closed loop control for the climb mast.
*
* @param vertical The vertical distance to drive to. A distance of 0 is the lower limit of the
* mast. The distance is specified in ??? and should always be positive.
*/
public void ClimbClosedLoop(double vertical) {
// Need to change vertical distance (specified in inches) to something
// the Jaguar can use (revs)
// Also ensure that vertical is positive.
// convert from inches to revolutions
double revolution = vertical / distPerRev;
m_setPoint = revolution + revolutionHome;
if (ClimbJag != null) {
try {
// the formula below will probably be subject to change
// may need to be inverted (is this inversion correct?) 2/9
ClimbJag.setX(m_setPoint);
} catch (CANTimeoutException ex) {
m_fault = true;
System.err.println("****************CAN timeout***********");
}
}
}
/**
* 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.
*/
public void setX(double outputValue) throws CANTimeoutException {
setX(outputValue, (byte) 0);
}
/**
* Set the output set-point value.
*
* <p>Needed by the SpeedControl interface (swallows CANTimeoutExceptions).
*
* @deprecated Use setX instead.
* @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 set(double outputValue, byte syncGroup) {
try {
setX(outputValue, syncGroup);
} catch (CANTimeoutException e) {
}
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
lights.set(Relay.Value.kForward);
// tracking.run();
/*try {
readString = serial.readString();
readString = readString.substring(readString.indexOf("data{"), readString.indexOf("}"));
xTurn = readString.substring(readString.indexOf("x("), readString.indexOf(")"));
xTurnVal = Integer.parseInt(xTurn);
} catch (VisaException ex) {
System.out.println("Error Reading string - " + ex);
}*/
y = drive_control.getRawAxis(1);
x = drive_control.getRawAxis(2);
t = drive_control.getRawAxis(3);
if (y > 1) y = 1;
else if (y < -1) y = -1;
if (x > 1) x = 1;
else if (x < -1) x = -1;
drive.setDriveJ(x, y, t);
/*if(drive_control.getRawButton(12))
//wheelsOn = true;
else if(drive_control.getRawButton(11));
wheelsOn = false;*/
/*try{
launcher.setX((drive_control.getRawAxis(4)-1)/2);
}catch(Exception e){}*/
// System.out.println((drive_control.getRawAxis(4)-1)*-1350);
if (!autoWheels) {
csc.setSetpoint((drive_control.getRawAxis(4) - 1) * -1500);
try {
// launcher.setX((drive_control.getRawAxis(4)+1)*2000);
System.out.println(
-launcher.getSpeed()
+ " -- "
+ csc.tmpResult
+ " -- "
+ (drive_control.getRawAxis(4) - 1) * -1500);
} catch (Exception e) {
System.out.println(e);
}
if (drive_control.getRawButton(11)) autoWheels = true;
} else {
csc.setSetpoint(2300);
if (drive_control.getRawButton(11)) autoWheels = false;
}
/*if(wheelsOn)
shoot(1);
else
shoot(0);*/
if (drive_control.getRawButton(5)) {
wedge.moveDown();
// servo1.set(1);
// servo2.set(0);
// wedge.set(Relay.Value.kForward);
} else if (drive_control.getRawButton(6)) {
wedge.moveUp();
// servo1.set(0);
// servo2.set(1);
// wedge.set(Relay.Value.kReverse);
} else {
// servo1.set(.5);
// servo2.set(.5);
// wedge.set(Relay.Value.kOff);
}
if (drive_control.getRawButton(3)) turret.set(.5);
else if (drive_control.getRawButton(4)) turret.set(-.5);
else turret.set(0);
if (drive_control.getRawButton(2)) driveMainBelt(true, true);
else if (drive_control.getRawButton(7)) driveMainBelt(false, true);
else driveMainBelt(false, false);
if (drive_control.getTrigger()) {
try {
belt2.setX(-1);
} catch (Exception e) {
}
} else if (drive_control.getRawButton(8)) {
try {
belt2.setX(1);
} catch (Exception e) {
}
} else {
try {
belt2.setX(0);
} catch (Exception e) {
}
}
if (control.getRawButton(3)) {
turret.set(.25);
} else if (control.getRawButton(4)) {
turret.set(-.25);
} else {
turret.set(0);
}
try {
station.println(
DriverStationLCD.Line.kMain6, 1, "RPM: " + String.valueOf(-launcher.getSpeed()));
} catch (CANTimeoutException ex) {
}
/*try{
if(tracking.foundTarget()){
station.println(DriverStationLCD.Line.kUser2, 1, "Distance: " + String.valueOf(tracking.getTarget("HIGH").distance));
station.println(DriverStationLCD.Line.kUser3, 1, "Suggested RPM: " + String.valueOf(tracking.getTarget("HIGH").distance*62.8));
if(x>0)
station.println(DriverStationLCD.Line.kUser4, 1, "Rotation: <--");
else if(x<0)
station.println(DriverStationLCD.Line.kUser4, 1, "Roation: -->");
else
station.println(DriverStationLCD.Line.kUser4, 1, "I don't need to rotate");
}
else{
station.println(DriverStationLCD.Line.kUser2, 1, "Help, I can't find target by myself!");
station.println(DriverStationLCD.Line.kUser3, 1, "");
station.println(DriverStationLCD.Line.kUser4, 1, "");
}
}catch(Exception e){}*/
station.updateLCD();
}
// Stops all motors once frisbee is thrown
public void stop() throws CANTimeoutException {
angleMotor.setX(0);
firstShootingMotor.setX(0);
secondShootingMotor.setX(0);
}
// Shoots the frisbee once it is angled and loaded
public void shoot() throws CANTimeoutException {
firstShootingMotor.setX(0.7);
secondShootingMotor.setX(0.7);
}
