diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2016/Teleop.java')
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Teleop.java | 45 |
1 files changed, 38 insertions, 7 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java index cafbca1..c7029f4 100644 --- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java +++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java @@ -1,14 +1,21 @@ package org.usfirst.frc.team4272.robot2016; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Timer; import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis; import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button; -import edu.wpi.first.wpilibj.Joystick; +import org.usfirst.frc.team4272.robotlib.PushButton; public class Teleop { private final HwRobot robot; + private final double armSoftStop = 95;//295; + + private final PushButton shootButton = new PushButton(); + private final Timer time = new Timer(); public Teleop(HwRobot robot) { this.robot = robot; + robot.armE.reset(); } private static double jsScale(Joystick j) { @@ -36,14 +43,38 @@ public class Teleop { 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 = false; //robot.ballL.get(); - control.fBall = bound(-oneIf(!ballL), fBall, 1); - control.bBall = bound(-oneIf(!ballL), bBall < 0 ? 0.75*bBall : bBall, 1); + boolean ballL = robot.ballL.get(); + control.fBall = bound(-oneIf(ballL), fBall < 0 ? 0.75*fBall : fBall, 1); + control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.85*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 (shootButton.update(oi.xbox.getButton(Button.X))) { + time.reset(); + time.start(); + } + if (oi.xbox.getButton(Button.X)) { + if (time.get() < 0.5) { + control.fBall = control.bBall = -0.5; + } else if (time.get() < 2.5) { + control.bBall = -0.5; + control.fBall = 1; + } else { + control.bBall = 1; + control.fBall = 1; + } + } + + //if (robot.armE.pidGet() > armSoftStop && control.arm > 0) + // control.arm = 0; /* Take pictures */ /* |