diff options
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; - } -} |