diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2016/Teleop.java')
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Teleop.java | 22 |
1 files changed, 16 insertions, 6 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java index cafbca1..19dfc6e 100644 --- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java +++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java @@ -6,9 +6,11 @@ import edu.wpi.first.wpilibj.Joystick; public class Teleop { private final HwRobot robot; + private final double armSoftStop = 95;//295; public Teleop(HwRobot robot) { this.robot = robot; + robot.armE.reset(); } private static double jsScale(Joystick j) { @@ -36,14 +38,22 @@ 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, 1); + control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.75*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 (robot.armE.pidGet() > armSoftStop && control.arm > 0) + // control.arm = 0; /* Take pictures */ /* |