diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Teleop.java | 24 |
1 files changed, 23 insertions, 1 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java index 9d0662d..d1db566 100644 --- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java +++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java @@ -1,9 +1,14 @@ package org.usfirst.frc.team4272.robot2016; +import edu.wpi.first.wpilibj.GenericHID.Hand; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.GenericHID.Hand; +import edu.wpi.first.wpilibj.Preferences; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import org.usfirst.frc.team4272.robotlib.PushButton; +import org.usfirst.frc.team4272.robotlib.RollingAvg; public class Teleop { private final HwRobot robot; @@ -14,9 +19,13 @@ public class Teleop { private double lastTime; private double currentTime; private boolean state = false; + private PIDSource lRate, rRate; + private Preferences prefs = Preferences.getInstance(); public Teleop(HwRobot robot) { this.robot = robot; + this.lRate = new RollingAvg(5, robot.lDriveE); + this.rRate = new RollingAvg(5, robot.rDriveE); } private static double jsScale(Joystick j) { @@ -39,10 +48,23 @@ public class Teleop { control.rDrive = -jsScale(oi.rStick); /* shifting */ + double shiftUp = prefs.getDouble("shiftUp", 3.3*3.28); + double shiftDown = prefs.getDouble("shiftDown", 2.7*3.28); + SmartDashboard.putNumber("r-shiftUp", shiftUp); + SmartDashboard.putNumber("r-shiftDown", shiftDown); if (oi.lStick.getTrigger()) { control.highGear = false; } else if (oi.rStick.getTrigger()) { control.highGear = true; + } else { + double speed = Math.abs((lRate.pidGet() + rRate.pidGet()) / 2); + if (!control.highGear) { + if (speed > shiftUp) + control.highGear = true; + } else { + if (speed < shiftDown) + control.highGear = false; + } } /* auto gear wiggle */ |