From 4e2b11e4df2f724a0e2d66c28bc465126a1c9d37 Mon Sep 17 00:00:00 2001 From: Deveveloper Date: Thu, 23 Mar 2017 01:47:29 -0400 Subject: Changes sitting on netbook --- .../usfirst/frc/team4272/robot2016/HwRobot.java | 215 +++++++++++++-------- 1 file changed, 131 insertions(+), 84 deletions(-) (limited to 'src/org/usfirst/frc/team4272/robot2016/HwRobot.java') 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 -- cgit v1.2.3