clean up

3 files changed, 40 insertions, 141 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2016/Control.java b/src/org/usfirst/frc/team4272/robot2016/Control.java
index 7713c5b..4baedd4 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Control.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Control.java
@@ -1,20 +1,13 @@
 package org.usfirst.frc.team4272.robot2016;
 
 public class Control {
-	double lDrive1 = 0,
-	       lDrive2 = 0,
-	       rDrive1 = 0,
-	       rDrive2 = 0,
-	       fBall   = 0,
-	       bBall   = 0,
-	       arm   = 0,
-	camRotate = 0,
-	camTilt = 0,
-	usbCam = 0,
-	climber = 0;
-	boolean PCM = true;
-	boolean PCM2 = false;
-	boolean PCM3 =true;
-	boolean PCM4 = false;
+	double lDrive = 0,
+	       rDrive = 0,
+	       camRotate = 0,
+	       camTilt = 0,
+	       climber = 0;
+	boolean highGear = false,
+		PCM3 =true,
+		PCM4 = false;
 	public Control() {}
 }
diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
index 4555adf..8f2515e 100644
--- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
@@ -36,66 +36,45 @@ public class HwRobot {
 	// naming convention: {side_letter}{Thing}{E|M (encoder/motor)}
 
 	private final PIDOutput
-	    rDriveM1 = new Talon(/*PWM*/0), /* PDP 2, 3 */
+	    rDriveM = new Talon(/*PWM*/0), /* PDP 2, 3 */
+	    lDriveM = new Talon(/*PWM*/1), /* PDP 12, 13 */
 	    climber = new Talon(/*PWM*/3), /* PDP 4 */
-	    lDriveM1 = new Talon(/*PWM*/1), /* PDP 12, 13 */
-	    //lDriveM2 = new Talon(3),/*PWM*/ /* PDP 11 */
-	    //rDriveM2 = new Talon(1),
-	    //armM     = new Talon(/*PWM*/8), /* PDP 15 */
-	    //rfBallM  = new Talon(/*PWM*/4), /* PDP 0 */
-	    //lfBallM  = new Talon(/*PWM*/9), /* PDP 14 */
-	    //fBallM   = new PIDOutputSplitter(rfBallM, lfBallM),
-	    //bBallM   = new Talon(/*PWM*/6),/* PDP 10, 5 */
 	    camRotate = new PIDServo(8),
 	    camTilt = new PIDServo(9);
-	public final Encoder   armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2, true);
-	public final DigitalInput armL = new DigitalInput(/*DIO*/3);
-	public final DigitalInput ballL = new DigitalInput(/*DIO*/4);
-	public final Encoder   rDriveE = new Encoder(/*DIO*/5,/*DIO*/6);
-	public final Encoder   lDriveE = new Encoder(/*DIO*/7,/*DIO*/8, true);
+	public final Encoder   lDriveE = new Encoder(/*DIO*/0,/*DIO*/2, /*DIO*/1, /*reverse*/false);
+	public final Encoder   rDriveE = new Encoder(/*DIO*/3,/*DIO*/5, /*DIO*/4, /*reverse*/true);
 	public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */
 
-	public Solenoid PCM = new Solenoid(/*PCM*/ 4);
+	public Solenoid PCM1 = new Solenoid(/*PCM*/ 4);
 	public Solenoid PCM2 = new Solenoid(5);
 	public Solenoid PCM3 = new Solenoid(6);
 	public Solenoid PCM4 = new Solenoid(7);
 	
 	public HwRobot() {
-		armE.setPIDSourceType(PIDSourceType.kDisplacement);
 		lDriveE.setPIDSourceType(PIDSourceType.kRate);
 		rDriveE.setPIDSourceType(PIDSourceType.kRate);
-		//PDP.initTable(NetworkTable.getTable("PDP"));//
+		//PDP.initTable(NetworkTable.getTable("PDP"));
 	}
 
 	public void run(Control c) {
-		lDriveM1.pidWrite( c.lDrive1);
-		rDriveM1.pidWrite(c.rDrive1);
-		//lDriveM2.pidWrite( c.lDrive2);
+		lDriveM.pidWrite(c.lDrive);
+		rDriveM.pidWrite(c.rDrive);
 		climber.pidWrite(c.climber);
-		//rDriveM2.pidWrite(-c.rDrive2);
-		//armM.pidWrite(c.arm * 0.5);
-		//fBallM.pidWrite(-c.fBall);
-		//bBallM.pidWrite(-c.bBall);
 		camRotate.pidWrite(c.camRotate);
 		camTilt.pidWrite(c.camTilt);
-		PCM.set(c.PCM);
-		PCM2.set(c.PCM2);
+
+		PCM1.set(c.highGear);
+		PCM2.set(!c.highGear);
+
 		PCM3.set(c.PCM3);
 		PCM4.set(c.PCM4);
-		SmartDashboard.putNumber("fBall", c.fBall);
-		SmartDashboard.putNumber("bBall", c.bBall);
-		SmartDashboard.putNumber("armM", c.arm);
 
-		SmartDashboard.putNumber("armE", armE.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());
 
-		//PDP.updateTable();**//
-		//SmartDashboard.putData("PDP", PDP);//
+		//PDP.updateTable();
+		//SmartDashboard.putData("PDP", PDP);
 	}
-	
 }
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
index a085d91..09151a8 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
@@ -17,7 +17,6 @@ public class Teleop {
 
 	public Teleop(HwRobot robot) {
 		this.robot = robot;
-		robot.armE.reset();
 	}
 
 	private static double jsScale(Joystick j) {
@@ -36,23 +35,15 @@ public class Teleop {
 	
 	public Control run(Control control, HwOI oi) {
 		/* Drive */
-		control.lDrive1 = -jsScale(oi.lStick);
-		control.rDrive1 = jsScale(oi.rStick);
-		//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;
-			}
+		control.lDrive = -jsScale(oi.lStick);
+		control.rDrive = jsScale(oi.rStick);
+
+		if (oi.lStick.getTrigger()) {
+			control.highGear = false;
+		} else if (oi.rStick.getTrigger()) {
+			control.highGear = true;
+		}
+
 		if (oi.rStick.getRawButton(2) || oi.lStick.getRawButton(2)) { 
 			currentTime = Timer.getMatchTime();
 			if(currentTime-lastTime > 0.15){
@@ -60,82 +51,32 @@ public class Teleop {
 				lastTime = currentTime;
 			}
 			if (state){
-			control.lDrive1 = 0.5;
-			control.rDrive1 = 0;
+				control.lDrive = 0.5;
+				control.rDrive = 0;
 			}
 			else if(!state){
-				control.lDrive1 = 0;
-				control.rDrive1 = 0.5;
+				control.lDrive = 0;
+				control.rDrive = 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 { 
+		} 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.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);
-		control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.5*bBall : bBall, 1);
-
-		/* I'm eyeballing 10 degrees ≈ 50 clicks */
-		if (robot.armE.pidGet() < 50 && !robot.armL.get()) {
-			robot.armE.reset();
-		}
-
-		if (oi.xbox.getBumper(Hand.kLeft))
-			control.arm = 1-(robot.armE.pidGet()/armSoftStop);
-		else if (oi.xbox.getBumper(Hand.kRight))
-			control.arm = robot.armE.pidGet()/armSoftStop;
-
-		if (shootButton.update(oi.xbox.getXButton())) {
-			time.reset();
-			time.start();
-		}
-		if (oi.xbox.getXButton()) {
-			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(oi.xbox.getTriggerAxis(Hand.kRight) > 0)
 			control.camRotate = 1;
@@ -145,20 +86,6 @@ public class Teleop {
 			control.camRotate = 0;
 		control.camTilt = oi.xbox.getX(Hand.kRight);
 
-		//if (robot.armE.piedGet() > armSoftStop && control.arm > 0)
-		//	control.arm = 0;
-
-		/* Take pictures */
-		/*
-		if (camButton.update(oi.xbox.getButton(Button.RT))) {
-			try {
-				ProcessBuilder pb = new ProcessBuilder("/home/lvuser/tweeterbot/bin/snapPhoto");
-				pb.redirectErrorStream(true);
-				pb.start();
-			} catch (Exception e) {};
-		}
-		*/
-
 		return control;
 	}
 }