Changes sitting on the Dell.dell-1

4 files changed, 37 insertions, 232 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2016/Control.java b/src/org/usfirst/frc/team4272/robot2016/Control.java
index 40fcb5a..9efdf34 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Control.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Control.java
@@ -7,7 +7,9 @@ public class Control {
 	       rDrive2 = 0,
 	       fBall   = 0,
 	       bBall   = 0,
-	       arm     = 0;
-
+	       arm     = 0,
+	camRotate = 0,
+	camTilt = 0;
+	boolean PCM = false;
 	public Control() {}
 }
diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
index e61d87f..e4e9b2f 100644
--- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
@@ -9,8 +9,11 @@ import edu.wpi.first.wpilibj.PIDOutput;
 import edu.wpi.first.wpilibj.PIDSourceType;
 import edu.wpi.first.wpilibj.PowerDistributionPanel;
 import edu.wpi.first.wpilibj.Talon;
+import edu.wpi.first.wpilibj.Jaguar;
+import edu.wpi.first.wpilibj.Solenoid;
 
 import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter;
+import org.usfirst.frc.team4272.robotlib.PIDServo;
 
 public class HwRobot {
 	/* Relay == a Spike */
@@ -33,15 +36,17 @@ public class HwRobot {
 	// naming convention: {side_letter}{Thing}{E|M (encoder/motor)}
 
 	private final PIDOutput
-	    rDriveM1 = new Talon(/*PWM*/2), /* PDP 2, 3 */
-	    rDriveM2 = new Talon(/*PWM*/4), /* PDP 4 */
-	    lDriveM1 = new Talon(/*PWM*/1), /* PDP 12, 13 */
-	    lDriveM2 = new Talon(/*PWM*/0), /* PDP 11 */
-	    armM     = new Talon(/*PWM*/8), /* PDP 15 */
-	    rfBallM  = new Talon(/*PWM*/3), /* PDP 0 */
-	    lfBallM  = new Talon(/*PWM*/9), /* PDP 14 */
-	    fBallM   = new PIDOutputSplitter(rfBallM, lfBallM),
-	    bBallM   = new Talon(/*PWM*/6); /* PDP 10, 5 */
+	    rDriveM1 = new Jaguar(/*PWM*/0), /* PDP 2, 3 */
+	    rDriveM2 = new Jaguar(/*PWM*/1), /* PDP 4 */
+	    lDriveM1 = new Jaguar(/*PWM*/3), /* PDP 12, 13 */
+	    lDriveM2 = new Jaguar(2),/*PWM*/ /* PDP 11 */
+	    //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);
@@ -50,6 +55,8 @@ public class HwRobot {
 	public final Encoder   lDriveE = new Encoder(/*DIO*/7,/*DIO*/8, true);
 	public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */
 
+	private final Solenoid PCM = new Solenoid(/*PCM*/ 0);
+
 	public HwRobot() {
 		armE.setPIDSourceType(PIDSourceType.kDisplacement);
 		lDriveE.setPIDSourceType(PIDSourceType.kRate);
@@ -62,10 +69,12 @@ public class HwRobot {
 		lDriveM2.pidWrite( c.lDrive2);
 		rDriveM1.pidWrite(-c.rDrive1);
 		rDriveM2.pidWrite(-c.rDrive2);
-		armM.pidWrite(c.arm * 0.5);
-		fBallM.pidWrite(-c.fBall);
-		bBallM.pidWrite(-c.bBall);
-
+		//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);
 		SmartDashboard.putNumber("fBall", c.fBall);
 		SmartDashboard.putNumber("bBall", c.bBall);
 		SmartDashboard.putNumber("armM", c.arm);
@@ -81,4 +90,5 @@ public class HwRobot {
 		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 73b7b60..d07e5ce 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
@@ -74,8 +74,17 @@ public class Teleop {
 				control.fBall = 1;
 			}
 		}
+		
+		if(oi.xbox.getAxis(Axis.RTrigger) > 0)
+			control.camRotate = 1;
+		else if(oi.xbox.getAxis(Axis.LTrigger) > 0)
+			control.camRotate = -1;
+		else
+			control.camRotate = 0;
+		control.camTilt = oi.xbox.getAxis(Axis.RX);
 
-		//if (robot.armE.pidGet() > armSoftStop && control.arm > 0)
+		control.PCM = oi.xbox.getButton(Button.A);
+		//if (robot.armE.piedGet() > armSoftStop && control.arm > 0)
 		//	control.arm = 0;
 
 		/* Take pictures */
diff --git a/src/org/usfirst/frc/team4272/robotlib/PIDController.java b/src/org/usfirst/frc/team4272/robotlib/PIDController.java
deleted file mode 100644
index 13f5666..0000000
--- a/src/org/usfirst/frc/team4272/robotlib/PIDController.java
+++ /dev/null
@@ -1,216 +0,0 @@
-/**
- * Copyright (c) FIRST 2008-2012. All Rights Reserved.
- * Open Source Software - may be modified and shared by FRC teams. The code
- * must be accompanied by the FIRST BSD license file in the root directory of
- * the project.
- *
- * Copyright (c) 2011-2012, 2015-2016 Luke Shumaker
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the FIRST nor the
- *       names of its contributors may be used to endorse or promote products
- *       derived from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY
- * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- * WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
- * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
- * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * @author Luke Shumaker <lukeshu@sbcglobal.net>
- */
-package org.usfirst.frc.team4272.robotlib;
-
-import edu.wpi.first.wpilibj.PIDOutput;
-import edu.wpi.first.wpilibj.PIDSource;
-import edu.wpi.first.wpilibj.SpeedController;
-//import edu.wpi.first.wpilibj.PIDController;
-
-/**
- * An enhanced variant of {@link edu.wpi.first.wpilibj.PIDController wpilibj PIDController}
- * (implements a PID Control Loop).
- * <p>
- * It is enhanced from
- * {@link edu.wpi.first.wpilibj.PIDController wpilibj PIDController}
- * in that it that also:
- * <ul>
- *  <li> implements SpeedController (and therefore PIDOutput)
- *  <li> disables self if the setpoint is NaN (see {@link PIDNanny}).
- * </ul>
- */
-public class PIDController extends edu.wpi.first.wpilibj.PIDController implements SpeedController {
-	private boolean NaNDisabled = false;
-	private boolean inverted = false;
-	public final PIDSource source;
-	public final PIDOutput output;
-
-	/**
-	 * Allocate a PID object with the given constants for P, I, D, and F
-	 *$
-	 * @param Kp the proportional coefficient
-	 * @param Ki the integral coefficient
-	 * @param Kd the derivative coefficient
-	 * @param Kf the feed forward term
-	 * @param source The PIDSource object that is used to get values
-	 * @param output The PIDOutput object that is set to the output percentage
-	 * @param period the loop time for doing calculations. This particularly
-	 *        effects calculations of the integral and differential terms. The
-	 *        default is 50ms.
-	 */
-	public PIDController(double Kp, double Ki, double Kd, double Kf,
-	                     PIDSource source, PIDOutput output,
-	                     double period) {
-		super(Kp, Ki, Kd, Kf, source, output, period);
-		this.source = source;
-		this.output = output;
-	}
-
-	/* Convenience constructors *******************************************/
-	/* `Kf`, and `period` are optional */
-
-	/**
-	 * Convenience constructor with {@code Kf} defaulting to
-	 * {@code 0.0}.
-	 *
-	 * @param Kp the proportional coefficient
-	 * @param Ki the integral coefficient
-	 * @param Kd the derivative coefficient
-	 * @param source The PIDSource object that is used to get values
-	 * @param output The PIDOutput object that is set to the output percentage
-	 * @param period the loop time for doing calculations. This particularly
-	 *        effects calculations of the integral and differential terms. The
-	 *        default is 50ms.
-	 */
-	public PIDController(double Kp, double Ki, double Kd,
-	                     PIDSource source, PIDOutput output,
-	                     double period) {
-		this(Kp, Ki, Kd, 0.0, source, output, period);
-	}
-
-	/**
-	 * Convenience constructor with {@code period} defaulting to
-	 * {@link #kDefaultPeriod}.
-	 *
-	 * @param Kp the proportional coefficient
-	 * @param Ki the integral coefficient
-	 * @param Kd the derivative coefficient
-	 * @param Kf the feed forward term
-	 * @param source The PIDSource object that is used to get values
-	 * @param output The PIDOutput object that is set to the output percentage
-	 */
-	public PIDController(double Kp, double Ki, double Kd, double Kf,
-	                     PIDSource source, PIDOutput output) {
-		this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
-	}
-
-	/**
-	 * Convenience constructor with {@code Kf} defaulting to
-	 * {@code 0.0}, and {@code period} defaulting to
-	 * {@link #kDefaultPeriod}.
-	 *
-	 * @param Kp the proportional coefficient
-	 * @param Ki the integral coefficient
-	 * @param Kd the derivative coefficient
-	 * @param source The PIDSource object that is used to get values
-	 * @param output The PIDOutput object that is set to the output percentage
-	 */
-	public PIDController(double Kp, double Ki, double Kd,
-	                     PIDSource source, PIDOutput output) {
-		this(Kp, Ki, Kd, 0.0, source, output, kDefaultPeriod);
-	}
-
-	/* Override to auto-disable if setpoint is NaN, and invert ************/
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public synchronized void setSetpoint(double setpoint) {
-		if (Double.isNaN(setpoint)) {
-			NaNDisabled = true;
-			super.disable();
-		} else {
-			if (NaNDisabled && !isEnabled()) {
-				enable();
-			}
-			super.setSetpoint((inverted ? -1 : 1) * setpoint);
-		}
-	}
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public synchronized void enable() {
-		NaNDisabled = false;
-		super.enable();
-	}
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public synchronized void disable() {
-		NaNDisabled = false;
-		super.disable();
-	}
-
-	/* Implement PIDOutput (a parent of SpeedController) *******************/
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public void pidWrite(double output) {
-		setSetpoint(output);
-	}
-
-	/* Implement SpeedController *******************************************/
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public void set(double output) {
-		setSetpoint(output);
-	}
-
-	/**
-	 * Don't use this.
-	 * @deprecated Don't use this; it is leaking up from
-	 *   {@link edu.wpi.first.wpilibj.CANJaguar} through
-	 *   {@link edu.wpi.first.wpilibj.SpeedController}
-	 */
-	@Override
-	public void set(double output, byte syncGroup) {
-		set(output);
-	}
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public void setInverted(boolean isInverted) {
-		inverted = isInverted;
-	}
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public boolean getInverted() {
-		return inverted;
-	}
-}