1 files changed, 86 insertions, 16 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
index 73b7b60..a085d91 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
@@ -2,8 +2,7 @@ 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.GenericHID.Hand; 
 import org.usfirst.frc.team4272.robotlib.PushButton;
 
 public class Teleop {
@@ -12,6 +11,9 @@ public class Teleop {
 
 	private final PushButton shootButton = new PushButton();
 	private final Timer time = new Timer();
+	private double lastTime;
+	private double currentTime;
+	private boolean state = false;
 
 	public Teleop(HwRobot robot) {
 		this.robot = robot;
@@ -19,8 +21,8 @@ public class Teleop {
 	}
 
 	private static double jsScale(Joystick j) {
-		double y = -j.getY();/* +:forward; -:backward */
-		double z = -j.getZ();/* +:more-sensitive; -:less-sensitive */
+		double y = j.getY();/* +:forward; -:backward */
+		double z = j.getZ();/* +:more-sensitive; -:less-sensitive */
 		return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y);
 	}
 
@@ -31,19 +33,79 @@ public class Teleop {
 	private static double oneIf(boolean b) {
 		return b ? 1 : 0;
 	}
-
+	
 	public Control run(Control control, HwOI oi) {
 		/* Drive */
-		control.lDrive1 = jsScale(oi.lStick);
+		control.lDrive1 = -jsScale(oi.lStick);
 		control.rDrive1 = jsScale(oi.rStick);
-		control.lDrive2 = oi.lStick.getTrigger() ? control.lDrive1 : 0;
-		control.rDrive2 = oi.rStick.getTrigger() ? control.rDrive1 : 0;
-		control.arm = -oi.xbox.getAxis(Axis.LY);
+		//control.lDrive2 = -jsScale(oi.lStick);
+		//control.rDrive2 = jsScale(oi.rStick);
+		if (control.PCM == true) {
+			control.PCM2 = oi.lStick.getTrigger();
+			control.PCM = oi.rStick.getTrigger();
+			}
+		if (oi.lStick.getTrigger()){
+			control.PCM2 = false;
+			control.PCM = true;
+//			SmartDashboard.putData()
+			}
+		if (oi.rStick.getTrigger()) {
+			control.PCM = false;
+			control.PCM2 = true;
+			}
+		if (oi.rStick.getRawButton(2) || oi.lStick.getRawButton(2)) { 
+			currentTime = Timer.getMatchTime();
+			if(currentTime-lastTime > 0.15){
+				state = !state;
+				lastTime = currentTime;
+			}
+			if (state){
+			control.lDrive1 = 0.5;
+			control.rDrive1 = 0;
+			}
+			else if(!state){
+				control.lDrive1 = 0;
+				control.rDrive1 = 0.5;
+			}
+			
+		}
+		
+		if(oi.xbox.getRawAxis(1) > 0.2){
+			control.climber = oi.xbox.getRawAxis(1) ;
+		}
+		
+		else if (oi.xbox.getRawAxis(1) < -0.2) {
+			control.climber = oi.xbox.getRawAxis(1);
+		}
+		else { 
+			control.climber = 0;
+		}
+		if (oi.xbox.getAButton() == true) {
+			control.PCM3 = true;
+			control.PCM4 = false;
+			}
+		if(oi.xbox.getBButton()==true) {
+			control.PCM3 = false;
+			control.PCM4 =true;
+		}
+		
+//		if(oi.xbox.getTriggerAxis(Hand.kRight/*rTrigger*/) > 0 ){
+//			control.PCM3 = false;
+//			control.PCM4 = true;
+//			}
+//		else {
+//			control.PCM3 = false;
+//			control.PCM4 =true;
+//		}
+		
+		/*control.lDrive2 = oi.lStick.getTrigger() ? control.lDrive1 : 0;
+		control.rDrive2 = oi.rStick.getTrigger() ? control.rDrive1 : 0;*/
+		control.arm = -oi.xbox.getY(Hand.kLeft);
 		if (control.arm < 0)
 			control.arm = control.arm*0.2;
 		
-		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));
+		double fBall = oi.xbox.getTriggerAxis(Hand.kRight) - oi.xbox.getTriggerAxis(Hand.kLeft);
+		double bBall = bound(-1, fBall, 0) + oneIf(oi.xbox.getAButton()) - oneIf(oi.xbox.getBButton());
 
 		boolean ballL = robot.ballL.get();
 		control.fBall = bound(-oneIf(ballL), fBall < 0 ? 0.5*fBall : fBall, 1);
@@ -54,16 +116,16 @@ public class Teleop {
 			robot.armE.reset();
 		}
 
-		if (oi.xbox.getButton(Button.LBumper))
+		if (oi.xbox.getBumper(Hand.kLeft))
 			control.arm = 1-(robot.armE.pidGet()/armSoftStop);
-		else if (oi.xbox.getButton(Button.RBumper))
+		else if (oi.xbox.getBumper(Hand.kRight))
 			control.arm = robot.armE.pidGet()/armSoftStop;
 
-		if (shootButton.update(oi.xbox.getButton(Button.X))) {
+		if (shootButton.update(oi.xbox.getXButton())) {
 			time.reset();
 			time.start();
 		}
-		if (oi.xbox.getButton(Button.X)) {
+		if (oi.xbox.getXButton()) {
 			if (time.get() < 0.5) {
 				control.fBall = control.bBall = -0.5;
 			} else if (time.get() < 2.5) {
@@ -74,8 +136,16 @@ public class Teleop {
 				control.fBall = 1;
 			}
 		}
+		
+		if(oi.xbox.getTriggerAxis(Hand.kRight) > 0)
+			control.camRotate = 1;
+		else if(oi.xbox.getTriggerAxis(Hand.kLeft) > 0)
+			control.camRotate = -1;
+		else
+			control.camRotate = 0;
+		control.camTilt = oi.xbox.getX(Hand.kRight);
 
-		//if (robot.armE.pidGet() > armSoftStop && control.arm > 0)
+		//if (robot.armE.piedGet() > armSoftStop && control.arm > 0)
 		//	control.arm = 0;
 
 		/* Take pictures */