summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/Teleop.java24
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 */