diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2016/Teleop.java')
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Teleop.java | 89 |
1 files changed, 61 insertions, 28 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java index 73b7b60..ed86427 100644 --- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java +++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java @@ -6,6 +6,7 @@ import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis; import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button; import org.usfirst.frc.team4272.robotlib.PushButton; + public class Teleop { private final HwRobot robot; private final double armSoftStop = 95;//295; @@ -15,49 +16,63 @@ public class Teleop { public Teleop(HwRobot robot) { this.robot = robot; - robot.armE.reset(); +// robot.armE.reset(); } - private static double jsScale(Joystick j) { - double y = -j.getY();/* +:forward; -:backward */ - double z = -j.getZ();/* +:more-sensitive; -:less-sensitive */ - return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y); - } - - private static double bound(double min, double v, double max) { - return Math.min(Math.max(v, min), max); - } +// private static double jsScale(Joystick j) { +// double y = -j.getY();/* +:forward; -:backward */ +// double z = -j.getZ();/* +:more-sensitive; -:less-sensitive */ +// return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y); +// } +// +// private static double bound(double min, double v, double max) { +// return Math.min(Math.max(v, min), max); +// } private static double oneIf(boolean b) { return b ? 1 : 0; } + //boolean DriverControl = true; public Control run(Control control, HwOI oi) { /* Drive */ - control.lDrive1 = jsScale(oi.lStick); - control.rDrive1 = jsScale(oi.rStick); - control.lDrive2 = oi.lStick.getTrigger() ? control.lDrive1 : 0; - control.rDrive2 = oi.rStick.getTrigger() ? control.rDrive1 : 0; - control.arm = -oi.xbox.getAxis(Axis.LY); + /*if (oi.lStick.getButton(Joystick.ButtonType.kTop) || oi.rStick.getButton(Joystick.ButtonType.kTop)) { + DriverControl =! DriverControl; + System.out.println("switched"); + }*/ + if (oi.xbox.getAxis(Axis.LY) > 0.2) { + control.lDrive1 = (oi.xbox.getAxis(Axis.LY)); + control.lDrive2 = (oi.xbox.getAxis(Axis.LY)); + } + if (oi.xbox.getAxis(Axis.RY) > 0.2) { + control.rDrive1 = (oi.xbox.getAxis(Axis.RY)); + control.rDrive2 = (oi.xbox.getAxis(Axis.RY)); + } + //control.arm = -oi.xbox.getAxis(Axis.LY); + /*if (DriverControl == true && Math.abs(oi.lStick.getAxis(Joystick.AxisType.kY)) <= 0.1 && (Math.abs(oi.rStick.getAxis(Joystick.AxisType.kY)) <= 0.1) ){ + control.lDrive1 = 0; + control.rDrive1 = 0; + System.out.println("stopped"); + }*/ if (control.arm < 0) control.arm = control.arm*0.2; - double fBall = oi.xbox.getAxis(Axis.RTrigger) - oi.xbox.getAxis(Axis.LTrigger); - double bBall = bound(-1, fBall, 0) + oneIf(oi.xbox.getButton(Button.A)) - oneIf(oi.xbox.getButton(Button.B)); + //double fBall = oi.xbox.getAxis(Axis.RTrigger) - oi.xbox.getAxis(Axis.LTrigger); + //double bBall = bound(-1, fBall, 0) + oneIf(oi.xbox.getButton(Button.A)) - oneIf(oi.xbox.getButton(Button.B)); - boolean ballL = robot.ballL.get(); - control.fBall = bound(-oneIf(ballL), fBall < 0 ? 0.5*fBall : fBall, 1); - control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.5*bBall : bBall, 1); + //boolean ballL = robot.ballL.get(); + //control.fBall = bound(-oneIf(ballL), fBall < 0 ? 0.5*fBall : fBall, 1); + //control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.5*bBall : bBall, 1); /* I'm eyeballing 10 degrees ≈ 50 clicks */ - if (robot.armE.pidGet() < 50 && !robot.armL.get()) { - robot.armE.reset(); - } +// if (robot.armE.pidGet() < 50 && !robot.armL.get()) { +// robot.armE.reset(); +// } - if (oi.xbox.getButton(Button.LBumper)) - control.arm = 1-(robot.armE.pidGet()/armSoftStop); - else if (oi.xbox.getButton(Button.RBumper)) - control.arm = robot.armE.pidGet()/armSoftStop; +// if (oi.xbox.getButton(Button.LBumper)) +// control.arm = 1-(robot.armE.pidGet()/armSoftStop); +// else if (oi.xbox.getButton(Button.RBumper)) +// control.arm = robot.armE.pidGet()/armSoftStop; if (shootButton.update(oi.xbox.getButton(Button.X))) { time.reset(); @@ -74,8 +89,26 @@ public class Teleop { control.fBall = 1; } } + + if(oi.xbox.getAxis(Axis.RTrigger) > 0) + control.camRotate = 1; + else if(oi.xbox.getAxis(Axis.LTrigger) > 0) + control.camRotate = -1; + else + control.camRotate = 0; + +// if(Math.abs(oi.xbox.getAxis(Axis.RY)) > 0.2) +// control.camTilt = oi.xbox.getAxis(Axis.RY); +// else +// control.camTilt = 0; + +// if(oi.xbox.getButton(Button.A)); /* Don't delete me!!*/ +// control.setSolnoid(true) +// else: +// control.PCM. + - //if (robot.armE.pidGet() > armSoftStop && control.arm > 0) + //if (robot.armE.piedGet() > armSoftStop && control.arm > 0) // control.arm = 0; /* Take pictures */ |