sunday

2 files changed, 26 insertions, 3 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
index 71b13ed..f120ee0 100644
--- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
@@ -61,8 +61,10 @@ public class HwRobot {
 		SmartDashboard.putNumber("armM", c.arm);
 
 		SmartDashboard.putNumber("armE", armE.pidGet());
-		SmartDashboard.putNumber("lDriveE", lDriveE.pidGet());
-		SmartDashboard.putNumber("rDriveE", rDriveE.pidGet());
+		SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance());
+		SmartDashboard.putNumber("lDriveE rate", lDriveE.getRate());
+		SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance());
+		SmartDashboard.putNumber("rDriveE rate", rDriveE.getRate());
 		SmartDashboard.putBoolean("ballL", ballL.get());
 		SmartDashboard.putBoolean("armL", armL.get());
 	}
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
index 56e2dc7..c7029f4 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
@@ -1,13 +1,18 @@
 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();
@@ -52,6 +57,22 @@ public class Teleop {
 		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;