diff --git a/LICENSE b/LICENSE index bd5d465..513c672 100644 --- a/LICENSE +++ b/LICENSE @@ -1,21 +1,21 @@ -MIT License - -Copyright (c) 2017 Team 2342 - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. +MIT License + +Copyright (c) 2017 Team 2342 + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/src/org/usfirst/frc/team2342/PIDLoops/DistancePIDController.java b/src/org/usfirst/frc/team2342/PIDLoops/DistancePIDController.java index d05a3e5..3684a3f 100644 --- a/src/org/usfirst/frc/team2342/PIDLoops/DistancePIDController.java +++ b/src/org/usfirst/frc/team2342/PIDLoops/DistancePIDController.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team2342.PIDLoops; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import org.usfirst.frc.team2342.util.SmartTalon; import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PIDOutput; @@ -8,8 +8,8 @@ import edu.wpi.first.wpilibj.PIDSourceType; public class DistancePIDController implements PIDOutput, PIDSource { - private WPI_TalonSRX left; - private WPI_TalonSRX right; + private SmartTalon left; + private SmartTalon right; private double Kp = 0.0d; private double Ki = 0.0d; private double Kd = 0.0d; @@ -18,7 +18,7 @@ public class DistancePIDController implements PIDOutput, PIDSource { private PIDSourceType type = PIDSourceType.kDisplacement; private double correction = 0.0d; - public DistancePIDController(double p, double i, double d, double ff, WPI_TalonSRX talonMasterLeft, WPI_TalonSRX talonMasterRight){ + public DistancePIDController(double p, double i, double d, double ff, SmartTalon talonMasterLeft, SmartTalon talonMasterRight){ this.left = talonMasterLeft; this.right = talonMasterRight; this.Kp = p; diff --git a/src/org/usfirst/frc/team2342/PIDLoops/SingleTalonDistancePIDController.java b/src/org/usfirst/frc/team2342/PIDLoops/SingleTalonDistancePIDController.java index 2feee23..45d676a 100644 --- a/src/org/usfirst/frc/team2342/PIDLoops/SingleTalonDistancePIDController.java +++ b/src/org/usfirst/frc/team2342/PIDLoops/SingleTalonDistancePIDController.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team2342.PIDLoops; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import org.usfirst.frc.team2342.util.SmartTalon; import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PIDOutput; @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj.PIDSourceType; public class SingleTalonDistancePIDController implements PIDOutput, PIDSource { - private WPI_TalonSRX talon; + private SmartTalon talon; private double Kp = 0.0d; private double Ki = 0.0d; private double Kd = 0.0d; @@ -17,7 +17,7 @@ public class SingleTalonDistancePIDController implements PIDOutput, PIDSource { private PIDSourceType type = PIDSourceType.kDisplacement; private double correction = 0.0d; - public SingleTalonDistancePIDController(double p, double i, double d, double ff, WPI_TalonSRX talon){ + public SingleTalonDistancePIDController(double p, double i, double d, double ff, SmartTalon talon){ this.talon= talon; this.Kp = p; this.Ki = i; diff --git a/src/org/usfirst/frc/team2342/json/Talon.java b/src/org/usfirst/frc/team2342/json/Talon.java index 0d8079b..065395b 100644 --- a/src/org/usfirst/frc/team2342/json/Talon.java +++ b/src/org/usfirst/frc/team2342/json/Talon.java @@ -9,4 +9,5 @@ public class Talon { public int mode; public PIDGains velocityGains; public PIDGains distanceGains; + public int deviceNumber; } diff --git a/src/org/usfirst/frc/team2342/robot/Robot.java b/src/org/usfirst/frc/team2342/robot/Robot.java index 6f563b8..0610417 100644 --- a/src/org/usfirst/frc/team2342/robot/Robot.java +++ b/src/org/usfirst/frc/team2342/robot/Robot.java @@ -17,15 +17,16 @@ import org.usfirst.frc.team2342.robot.subsystems.WestCoastTankDrive; import org.usfirst.frc.team2342.util.Constants; import org.usfirst.frc.team2342.util.FMS; +import org.usfirst.frc.team2342.util.SmartTalon; import com.ctre.phoenix.ParamEnum; import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.cscore.UsbCamera; import edu.wpi.cscore.VideoSink; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -39,14 +40,18 @@ public class Robot extends IterativeRobot { Joystick gamepad; PCMHandler PCM; - WPI_TalonSRX talonFR; - WPI_TalonSRX talonFL; - WPI_TalonSRX talonBR; - WPI_TalonSRX talonBL; - WPI_TalonSRX talonCascade; - WPI_TalonSRX talonIntakeRight; - WPI_TalonSRX talonIntakeLeft; - WPI_TalonSRX talonTip; + + SmartTalon talonFR; + SmartTalon talonFL; + SmartTalon talonBR; + SmartTalon talonBL; + SmartTalon talonCascade; + SmartTalon talonIntakeRight; + SmartTalon talonIntakeLeft; + SmartTalon talonTip; + //Solenoid solenoidLowGear = new Solenoid(Constants.PCM_CAN_ID ,Constants.PCM_SLOT_LOWGEAR); + //Solenoid solenoidHighGear = new Solenoid(Constants.PCM_CAN_ID ,Constants.PCM_SLOT_HIGHGEAR); + Solenoid solenoid1; WestCoastTankDrive westCoast; Joystick joystickR; @@ -63,15 +68,21 @@ public class Robot extends IterativeRobot { public Robot() { gamepad = new Joystick(0); PCM = new PCMHandler(11); - talonFR = new WPI_TalonSRX(Constants.RIGHT_MASTER_TALON_ID); - talonFL = new WPI_TalonSRX(Constants.LEFT_MASTER_TALON_ID); - talonBR = new WPI_TalonSRX(Constants.RIGHT_SLAVE_TALON_ID); - talonBL = new WPI_TalonSRX(Constants.LEFT_SLAVE_TALON_ID); - talonCascade = new WPI_TalonSRX(Constants.TALON_CASCADE); - talonIntakeRight = new WPI_TalonSRX(Constants.TALON_INTAKE_RIGHT); - talonIntakeLeft = new WPI_TalonSRX(Constants.TALON_INTAKE_LEFT); - talonTip = new WPI_TalonSRX(Constants.TALON_TIP); + + talonFR = new SmartTalon(Constants.RIGHT_MASTER_TALON_ID); + talonFL = new SmartTalon(Constants.LEFT_MASTER_TALON_ID); + talonBR = new SmartTalon(Constants.RIGHT_SLAVE_TALON_ID); + talonBL = new SmartTalon(Constants.LEFT_SLAVE_TALON_ID); + talonCascade = new SmartTalon(Constants.TALON_CASCADE); + talonIntakeRight = new SmartTalon(Constants.TALON_INTAKE_RIGHT); + talonIntakeLeft = new SmartTalon(Constants.TALON_INTAKE_LEFT); + talonTip = new SmartTalon(Constants.TALON_TIP); + //Solenoid solenoidLowGear = new Solenoid(Constants.PCM_CAN_ID ,Constants.PCM_SLOT_LOWGEAR); + //Solenoid solenoidHighGear = new Solenoid(Constants.PCM_CAN_ID ,Constants.PCM_SLOT_HIGHGEAR); + //solenoid1 = new Solenoid(Constants.PCM_CAN_ID, Constants.PCM_BOX_MANIPULATOR); + + westCoast = new WestCoastTankDrive(PCM, talonFL, talonFR, talonBL, talonBR); joystickR = new Joystick(2); XBOX = new Joystick(1); @@ -185,6 +196,49 @@ else if(Math.abs(XBOX.getRawAxis(Constants.XBOX_RIGHTTRIGGER)) > 0.1) { boxManipulator.talonIntakeRight.set(ControlMode.PercentOutput, 0); boxManipulator.talonIntakeLeft.set(ControlMode.PercentOutput, 0); } + + /*Scheduler.getInstance().run(); + //Drive with joystick control in velocity mode + westCoast.outputToSmartDashboard(); + //Buttons 8 & 9 or (gamepad) 5 & 6 are Low & High gear, respectively + if (gamepad.getRawButton(5)) + westCoast.setLowGear(); + else if (gamepad.getRawButton(6)) + westCoast.setHighGear(); + else + westCoast.setNoGear(); + //Sleep for 0.01s + try { + Thread.sleep(100); + } catch(InterruptedException ex) { + Thread.currentThread().interrupt(); + } + //teliopInity + if (joystick1.getRawButton(1)) { + talon1.goDistance(0.25, 0.4); + talon2.goDistance(-0.25, 0.4); + talon3.goDistance(0.25, 0.4); + talon4.goDistance(-0.25, 0.4); + } + + // PCM.turnOn(); + // SmartTalon talon1 = new SmartTalon(0); + // SmartTalon talon2 = new oO(1); + // boxManipulator = new BoxManipulator(talon1, talon2, PCM); + // cascadeElevator = new CascadeElevator(talon1, talon2); + + */ + /*Scheduler.getInstance().run(); + try { + Thread.sleep(10); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + }*/ + + + + } public void disabledInit() { diff --git a/src/org/usfirst/frc/team2342/robot/subsystems/BoxManipulator.java b/src/org/usfirst/frc/team2342/robot/subsystems/BoxManipulator.java index 7d99ee7..67b5669 100644 --- a/src/org/usfirst/frc/team2342/robot/subsystems/BoxManipulator.java +++ b/src/org/usfirst/frc/team2342/robot/subsystems/BoxManipulator.java @@ -1,21 +1,25 @@ package org.usfirst.frc.team2342.robot.subsystems; + import org.usfirst.frc.team2342.robot.PCMHandler; +import org.usfirst.frc.team2342.util.SmartTalon; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class BoxManipulator extends Subsystem { - public WPI_TalonSRX talonIntakeRight; - public WPI_TalonSRX talonIntakeLeft; - private WPI_TalonSRX talonTip; + + public SmartTalon talonIntakeRight; + public SmartTalon talonIntakeLeft; + private SmartTalon talonTip; + + private PCMHandler pcm; + Timer timer = new Timer(); public static final int PULL = 0; @@ -27,7 +31,9 @@ public class BoxManipulator extends Subsystem { private final boolean InvertMotor = false; - public BoxManipulator(WPI_TalonSRX talonIntakeRight, WPI_TalonSRX talonIntakeLeft, WPI_TalonSRX talonTip, PCMHandler pcm) { + + public BoxManipulator(SmartTalon talonIntakeRight, SmartTalon talonIntakeLeft, SmartTalon talonTip, PCMHandler pCM2) { + this.talonIntakeRight = talonIntakeRight; this.talonIntakeLeft = talonIntakeLeft; this.talonTip = talonTip; diff --git a/src/org/usfirst/frc/team2342/robot/subsystems/CascadeElevator.java b/src/org/usfirst/frc/team2342/robot/subsystems/CascadeElevator.java index e85b321..6858408 100644 --- a/src/org/usfirst/frc/team2342/robot/subsystems/CascadeElevator.java +++ b/src/org/usfirst/frc/team2342/robot/subsystems/CascadeElevator.java @@ -1,17 +1,17 @@ package org.usfirst.frc.team2342.robot.subsystems; import org.usfirst.frc.team2342.util.Constants; +import org.usfirst.frc.team2342.util.SmartTalon; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class CascadeElevator extends Subsystem { - public WPI_TalonSRX talonCascade; + public SmartTalon talonCascade; public static final int BASE = 0; public static final int SWITCH = 1; @@ -26,7 +26,7 @@ public class CascadeElevator extends Subsystem { public DigitalInput lowerLimit; public DigitalInput upperLimit; - public CascadeElevator(WPI_TalonSRX talonCascade) { + public CascadeElevator(SmartTalon talonCascade) { this.talonCascade = talonCascade; this.talonCascade.setInverted(false); talonCascade.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, PidLoopIndex, PidTimeOutMs); @@ -109,6 +109,7 @@ public void setVelocity(double speed) { /*System.out.println("limit: " + talonCascade.getSensorCollection().isFwdLimitSwitchClosed() + " limitupper: " + talonCascade.getSensorCollection().isRevLimitSwitchClosed() + " position: " + talonCascade.getSelectedSensorPosition(PidLoopIndex));*/ + System.out.println(speed); } catch (Exception e) { diff --git a/src/org/usfirst/frc/team2342/robot/subsystems/WestCoastTankDrive.java b/src/org/usfirst/frc/team2342/robot/subsystems/WestCoastTankDrive.java index 364913f..9fed2ee 100644 --- a/src/org/usfirst/frc/team2342/robot/subsystems/WestCoastTankDrive.java +++ b/src/org/usfirst/frc/team2342/robot/subsystems/WestCoastTankDrive.java @@ -7,17 +7,17 @@ import org.usfirst.frc.team2342.robot.PCMHandler; import org.usfirst.frc.team2342.robot.TalonNWT; import org.usfirst.frc.team2342.util.Constants; +import org.usfirst.frc.team2342.util.SmartTalon; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class WestCoastTankDrive extends Subsystem { - private WPI_TalonSRX leftA, rightA, leftB, rightB; + private SmartTalon leftA, rightA, leftB, rightB; private PCMHandler m_PCM; public DistancePIDController dpidc; public GyroPIDController pidc; // GyroPIDController @@ -25,7 +25,7 @@ public class WestCoastTankDrive extends Subsystem { private boolean gyroControl = false; // gyro Control private boolean isLeftInner = false; // is the left wheel inner - public WestCoastTankDrive(PCMHandler PCM, WPI_TalonSRX leftFR, WPI_TalonSRX rightFR, WPI_TalonSRX leftBA, WPI_TalonSRX rightBA) { + public WestCoastTankDrive(PCMHandler PCM, SmartTalon leftFR, SmartTalon rightFR, SmartTalon leftBA, SmartTalon rightBA) { //Json config = JsonHelpe.getConfig(); m_PCM = PCM; leftA = leftFR; @@ -137,14 +137,12 @@ public void setPercentage(double left, double right) { public void goDistance(double distanceInFeet){ double distance = distanceInFeet/Constants.TALON_RPS_TO_FPS * Constants.TALON_TICKS_PER_REV; - dpidc.setGoal(distance); - if (!leftA.getControlMode().equals(ControlMode.Velocity)) { - leftA.selectProfileSlot(Constants.TALON_VELOCITY_SLOT_IDX, 0); - } + dpidc.setGoal(distance); + double vel = Constants.WESTCOAST_MAX_SPEED * dpidc.getCorrection(); if (!this.gyroControl) { - leftA.set(ControlMode.Velocity, vel); - rightA.set(ControlMode.Velocity, vel); + leftA.setMode(ControlMode.Velocity, vel); + rightA.setMode(ControlMode.Velocity, vel); } else { SmartDashboard.putString("DB/String 2", String.valueOf(vel)); @@ -158,8 +156,8 @@ public void distanceLoop(){ } double vel = Constants.WESTCOAST_MAX_SPEED * dpidc.getCorrection(); if (!this.gyroControl) { - leftA.set(ControlMode.Velocity, vel); - rightA.set(ControlMode.Velocity, vel); + leftA.ControlModeLoop(dpidc.getCorrection()); + rightA.ControlModeLoop(dpidc.getCorrection()); } else { SmartDashboard.putString("DB/String 2", String.valueOf(vel)); @@ -295,11 +293,11 @@ public void zeroSnesors() { this.pidc.reset(); } - private static void zeroEncoders(WPI_TalonSRX talon) { + private static void zeroEncoders(SmartTalon talon) { talon.setSelectedSensorPosition(0, Constants.TALON_VELOCITY_SLOT_IDX, 0); } - private void loadGains(WPI_TalonSRX talon, int slotIdx, PIDGains talonPID) { + private void loadGains(SmartTalon talon, int slotIdx, PIDGains talonPID) { talon.config_kP(slotIdx, talonPID.p, 0); talon.config_kI(slotIdx, talonPID.i, 0); talon.config_kD(slotIdx, talonPID.d, 0); diff --git a/src/org/usfirst/frc/team2342/util/SmartTalon.java b/src/org/usfirst/frc/team2342/util/SmartTalon.java new file mode 100644 index 0000000..c95ea6e --- /dev/null +++ b/src/org/usfirst/frc/team2342/util/SmartTalon.java @@ -0,0 +1,143 @@ +package org.usfirst.frc.team2342.util; + +import org.usfirst.frc.team2342.json.Talon; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +/* + * Generic smart talon class for use with devices + */ +public class SmartTalon extends WPI_TalonSRX { + + //basic talon information + private int deviceNumber; + private boolean inverted; + private ControlMode mode ; + private PIDGains pidGains; + + private double currentModeValue; + + //WPI_TalonSRX compared to TalonSRX for network table interface + + + public SmartTalon(Talon jsonTalon){ + this(jsonTalon.deviceNumber, jsonTalon.inverted); + this.pidGains.setP(jsonTalon.velocityGains.p); + this.pidGains.setI(jsonTalon.velocityGains.i); + this.pidGains.setD(jsonTalon.velocityGains.d); + this.pidGains.setRr(jsonTalon.velocityGains.rr); + this.pidGains.setFf(jsonTalon.velocityGains.ff); + this.pidGains.setIzone(jsonTalon.velocityGains.izone); + } + + public SmartTalon(int deviceNumber){ + this(deviceNumber, false); + } + + public SmartTalon(int deviceNumber,boolean inverted){ + this(deviceNumber, inverted, ControlMode.PercentOutput); + } + + public SmartTalon(int deviceNumber,boolean inverted,ControlMode mode){ + this(deviceNumber, inverted, mode ,FeedbackDevice.CTRE_MagEncoder_Relative); + } + + public SmartTalon(int deviceNumber, boolean inverted, ControlMode mode, FeedbackDevice feedback){ + super(deviceNumber); + //set variables + this.deviceNumber = deviceNumber; + this.inverted = inverted; + this.mode = mode; + + //create talon + + setFeedBackDevice(feedback); + configAllowableClosedloopError(0, Constants.TALON_DISTANCE_SLOT_IDX, 0); + + //set pid gains to default + this.pidGains = new PIDGains(0.0,0.0,0.0,0.0,0.0,0); + this.currentModeValue = 0.0; + + zeroEncoder(); + } + + //change device type + public void setFeedBackDevice(FeedbackDevice feedback){ + configSelectedFeedbackSensor(feedback, 0, 0); + } + + //set encoder position to 0 + public void zeroEncoder(){ + setSelectedSensorPosition(0, 0, 0); + } + + //load in gains to selected talon + public void loadGains(PIDGains talonPID){ + //load in 0 by default + loadGains(talonPID, 0); + } + + //load in gains to selected talon and idx + public void loadGains(PIDGains talonPID, int slotIdx){ + this.pidGains = talonPID; + config_kP(slotIdx, talonPID.getP(), 0); + config_kI(slotIdx, talonPID.getI(), 0); + config_kD(slotIdx, talonPID.getD(), 0); + config_kF(slotIdx, talonPID.getFf(), 0); + config_IntegralZone(slotIdx, talonPID.getIzone(), 0); + configOpenloopRamp(talonPID.getRr(), 0); + } + + //set the mode and its value + public void setMode(ControlMode mode, double value){ + this.mode = mode; + this.currentModeValue = value; + set(mode, value); + } + + //called with a pid controller in order to control speed over time i.e. distance + public void ControlModeLoop(double controllerValue){ + //cap the controller value to 1.0 in case user doesn't understand proper usage + if(controllerValue > 1.0) + set(this.mode, this.currentModeValue * 1.0); + else if(controllerValue < -1.0) + set(this.mode, this.currentModeValue * -1.0); + else + set(this.mode, this.currentModeValue * controllerValue); + } + + //output data regarding sensors to the network table + public void WriteToNetworkTables(String talonTableName){ + String talonTable = talonTableName + "/"+getDeviceID(); + + //set general values + NetworkTableInterface.setValue(talonTable, "Inverted", getInverted()); + NetworkTableInterface.setValue(talonTable, "Percent Output", getMotorOutputPercent()); + NetworkTableInterface.setValue(talonTable, "Output Voltage", getMotorOutputVoltage()); + NetworkTableInterface.setValue(talonTable, "Bus Voltage", getBusVoltage()); + NetworkTableInterface.setValue(talonTable, "Output Current", getOutputCurrent()); + NetworkTableInterface.setValue(talonTable, "Position", getSelectedSensorPosition(0)); + NetworkTableInterface.setValue(talonTable, "Velocity", getSelectedSensorVelocity(0)); + NetworkTableInterface.setValue(talonTable, "Closed Loop Error", getClosedLoopError(0)); + NetworkTableInterface.setValue(talonTable, "Error Derivative", getErrorDerivative(0)); + NetworkTableInterface.setValue(talonTable, "Integral Acummulator", getIntegralAccumulator(0)); + } + + //only needs to be called after changing pid + private void updateNWTPid(String talonTableName){ + String talonTable = talonTableName + "/"+getDeviceID(); + + //set pid values + NetworkTableInterface.setValue(talonTable, "P", pidGains.getP()); + NetworkTableInterface.setValue(talonTable, "I", pidGains.getI()); + NetworkTableInterface.setValue(talonTable, "D", pidGains.getD()); + NetworkTableInterface.setValue(talonTable, "FF", pidGains.getFf()); + NetworkTableInterface.setValue(talonTable, "RR", pidGains.getRr()); + NetworkTableInterface.setValue(talonTable, "IZone", pidGains.getIzone()); + } + + //getters + +}