diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2016/HwRobot.java')
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/HwRobot.java | 215 |
1 files changed, 131 insertions, 84 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java index e61d87f..23ae62a 100644 --- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java +++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java @@ -1,84 +1,131 @@ -package org.usfirst.frc.team4272.robot2016; - -import edu.wpi.first.wpilibj.networktables.NetworkTable; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Encoder; -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 org.usfirst.frc.team4272.robotlib.PIDOutputSplitter; - -public class HwRobot { - /* Relay == a Spike */ - /* PCM = Pneumatics Control Module */ - - /* All of the numbered inputs are in the classes: - * - DIO: 0-9 - * - Relay: 0-3 - * - Analog In: 0-3 - * - PWM: 0-9 - * - PCM: 0-7 (the PCM is connected via CAN). - * - CAN - * - * For completeness, the roboRIO also has: i2c, RS-232, SPI, - * RSL, 2xUSB-A, an accelerometer, and an expansion port. - * - * And, for communication: USB-B and Ethernet. - */ - - // 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 */ - - 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 PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */ - - public HwRobot() { - armE.setPIDSourceType(PIDSourceType.kDisplacement); - lDriveE.setPIDSourceType(PIDSourceType.kRate); - rDriveE.setPIDSourceType(PIDSourceType.kRate); - PDP.initTable(NetworkTable.getTable("PDP")); - } - - public void run(Control c) { - lDriveM1.pidWrite( c.lDrive1); - 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); - - 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); - } -} +/**
+* Copyright (c) 2015-2017 Luke Shumaker.
+* Copyright (c) 2017 Cameron Richards.
+* Copyright (c) 2017 Noah Gaeta.
+* Copyright (c) 2017 Thomas Griffith.
+* 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.
+*/
+package org.usfirst.frc.team4272.robot2017;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.PowerDistributionPanel;
+import edu.wpi.first.wpilibj.Talon;
+import edu.wpi.first.wpilibj.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter;
+import org.usfirst.frc.team4272.robotlib.PIDServo;
+import org.usfirst.frc.team4272.robotlib.RollingAvg;
+
+public class HwRobot {
+ /* Relay == a Spike */
+ /* PCM = Pneumatics Control Module */
+
+ /* All of the numbered inputs are in the classes:
+ * - DIO: 0-9
+ * - Relay: 0-3
+ * - Analog In: 0-3
+ * - PWM: 0-9
+ * - PCM: 0-7 (the PCM is connected via CAN).
+ * - CAN
+ *
+ * For completeness, the roboRIO also has: i2c, RS-232, SPI,
+ * RSL, 2xUSB-A, an accelerometer, and an expansion port.
+ *
+ * And, for communication: USB-B and Ethernet.
+ */
+
+ // naming convention: {side_letter}{Thing}{E|M (encoder/motor)}
+
+ private final PIDOutput
+ rDriveM = new Talon(/*PWM*/0), /* PDP 2, 3 */
+ lDriveM = new Talon(/*PWM*/1), /* PDP 12, 13 */
+ climber = new Talon(/*PWM*/3), /* PDP 4 */
+ camRotate = new PIDServo(8),
+ camTilt = new PIDServo(9);
+ public final Encoder lDriveE = new Encoder(/*DIO*/0,/*DIO*/1, /*reverse*/false);
+ public final Encoder rDriveE = new Encoder(/*DIO*/2,/*DIO*/3, /*reverse*/true);
+ public final PIDSource lRate, rRate;
+ public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */
+ public final DriverStation ds = DriverStation.getInstance();
+
+ public DoubleSolenoid
+ shifter = new DoubleSolenoid(/*PCM*/4, /*PCM*/5),
+ ged = new DoubleSolenoid(/*PCM*/6, /*PCM*/7);
+
+ private double maxRate = 0;
+ private double minBatt = 20;
+
+ public HwRobot() {
+ lDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/);
+ rDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/);
+
+ lDriveE.setPIDSourceType(PIDSourceType.kRate);
+ rDriveE.setPIDSourceType(PIDSourceType.kRate);
+ PDP.initTable(NetworkTable.getTable("PDP"));
+
+ lRate = new RollingAvg(5, lDriveE);
+ rRate = new RollingAvg(5, rDriveE);
+ }
+
+ public void run(Control c) {
+ lDriveM.pidWrite(c.lDrive);
+ rDriveM.pidWrite(-c.rDrive);
+ climber.pidWrite(c.climber);
+ camRotate.pidWrite(c.camRotate);
+ camTilt.pidWrite(c.camTilt);
+
+ shifter.set(c.highGear ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward);
+ ged.set(c.gedOut ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward);
+
+ double rate = Math.abs((lDriveE.getRate()+rDriveE.getRate())/2);
+ SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance());
+ SmartDashboard.putNumber("lDriveE rate", Math.abs(lDriveE.getRate()));
+ SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance());
+ SmartDashboard.putNumber("rDriveE rate", Math.abs(rDriveE.getRate()));
+ SmartDashboard.putNumber("rate", rate);
+ if (rate > maxRate)
+ maxRate = rate;
+ SmartDashboard.putNumber("maxRate", maxRate);
+
+ double batt = ds.getBatteryVoltage();
+ SmartDashboard.putNumber("batt", batt);
+ if (batt < minBatt)
+ minBatt = batt;
+ SmartDashboard.putNumber("minBatt", minBatt);
+
+ SmartDashboard.putBoolean("highGear", c.highGear);
+ SmartDashboard.putBoolean("gedOut", c.gedOut);
+
+ PDP.updateTable();
+ SmartDashboard.putData("PDP", PDP);
+ }
+}
\ No newline at end of file |