diff --git a/.Glass/glass.json b/.Glass/glass.json index b42754f..9201ccc 100644 --- a/.Glass/glass.json +++ b/.Glass/glass.json @@ -1,40 +1,139 @@ { "NetworkTables": { - "Persistent Values": { - "open": false - }, - "Retained Values": { - "open": false - }, - "Transitory Values": { - "open": false - }, "retained": { "SmartDashboard": { - "Auton Chooser": { + "Rotation PID": { "open": true }, "open": true } }, "transitory": { + "LiveWindow": { + "AngledElevator": { + "open": true + }, + "Claw": { + "open": true + }, + "Drivetrain": { + "open": true + }, + "Ungrouped": { + "open": true + } + }, "SmartDashboard": { "Auton Chooser": { "open": true + } + }, + "datatable": { + "Align Pitch": { + "open": true + }, + "Drivetrain": { + "open": true + }, + "Elevator": { + "open": true + }, + "Swerve Module": { + "open": true + }, + "SwervePositionController": { + "open": true }, "open": true } }, "types": { "/FMSInfo": "FMSInfo", + "/LiveWindow/AngledElevator": "Subsystem", + "/LiveWindow/AngledElevator/Limit Switch": "Digital Input", + "/LiveWindow/Claw": "Subsystem", + "/LiveWindow/Drivetrain": "Subsystem", "/LiveWindow/Ungrouped/DigitalInput[0]": "Digital Input", "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[4]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[5]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[6]": "PIDController", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", - "/SmartDashboard/Auton Chooser": "String Chooser" + "/SmartDashboard/AngledElevator": "Subsystem", + "/SmartDashboard/Auton Chooser": "String Chooser", + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/Rotation PID": "String Chooser", + "/SmartDashboard/test controller": "PIDController" + }, + "windows": { + "/LiveWindow/AngledElevator": { + "window": { + "visible": true + } + }, + "/LiveWindow/Claw": { + "window": { + "visible": true + } + }, + "/LiveWindow/Drivetrain": { + "window": { + "visible": true + } + }, + "/SmartDashboard/AngledElevator": { + "window": { + "visible": true + } + }, + "/SmartDashboard/Auton Chooser": { + "window": { + "visible": true + } + } } }, "NetworkTables Settings": { "mode": "Client (NT4)", - "serverTeam": "1072" - } + "serverTeam": "10.10.72.2" + }, + "Plots": { + "Plot <0>": { + "plots": [ + { + "height": 0, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/test controller/p" + }, + { + "color": [ + 0.8666667342185974, + 0.5176470875740051, + 0.32156863808631897, + 1.0 + ], + "id": "NT:/SmartDashboard/kP" + } + ] + } + ] + }, + "Plot <1>": { + "plots": [ + { + "height": 0 + } + ] + } + }, + "enterKey": 345 } diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index f740bae..4e0114b 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -5,6 +5,7 @@ import frc.robot.commands.claw.ToggleClaw; import frc.robot.commands.drivetrain.AlignPitch; import frc.robot.commands.elevator.MoveToPosition; +import frc.robot.commands.elevator.SoftZero; import frc.robot.commands.elevator.ZeroElevator; import frc.robot.subsystems.Drivetrain; import harkerrobolib.joysticks.XboxGamepad; @@ -63,6 +64,7 @@ private void initBindings() { //operator.getRightDPadButton().debounce(0.13, DebounceType.kRising).onTrue(new ToggleClaw()); operator.getRightDPadButton().onTrue(new ToggleClaw()); operator.getRightBumper().onTrue(new ZeroElevator()); + operator.getLeftBumper().onTrue(new SoftZero()); // set A button on OPERATOR controller to set Elevator to POSITION LOW operator.getButtonX().whileTrue(new MoveToPosition(RobotMap.AngledElevator.POSITIONS[3])); operator.getButtonY().whileTrue(new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2])); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4d04bb9..e97c5d4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -22,10 +22,12 @@ import frc.robot.commands.claw.ToggleClaw; import frc.robot.commands.drivetrain.SwerveManual; import frc.robot.commands.elevator.ElevatorManual; +import frc.robot.commands.elevator.ZeroElevator; import frc.robot.subsystems.AngledElevator; import frc.robot.subsystems.Claw; import frc.robot.subsystems.Drivetrain; import frc.robot.util.CameraPoseEstimation; +import frc.robot.util.Telemetry; /** * The VM is configured to automatically run this class, and to call the @@ -40,6 +42,7 @@ public class Robot extends TimedRobot { private SendableChooser autonChooser; private PowerDistribution powerDistribution; + private Telemetry telemetry; /** * This function is run when the robot is first started up and should be used @@ -62,11 +65,16 @@ public void robotInit() { autonChooser.addOption("Bottom Path", "Bottom Path"); autonChooser.addOption("Top Path", "Top Path"); autonChooser.addOption("No auton", "No auton"); + autonChooser.addOption("Bottom Path Middle", "Bottom Path Middle"); + autonChooser.addOption("Top Path Middle", "Top Path Middle"); SmartDashboard.putData("Auton Chooser", autonChooser); CameraPoseEstimation.getInstance().getCamera().setDriverMode(true); + AngledElevator.getInstance().resetEncoders(); // rio voltage and current + telemetry = new Telemetry(); + powerDistribution = new PowerDistribution(); } @@ -77,11 +85,6 @@ public void robotPeriodic() { SmartDashboard.putString("Current Auton:", autonChooser.getSelected()); - // double pdhVoltage = powerDistribution.getVoltage(); - // double pdhCurrent = powerDistribution.getCurrent(20); - // SmartDashboard.putNumber("pdh voltage", pdhVoltage); - // SmartDashboard.putNumber("rio current", pdhCurrent); - // SmartDashboard.putNumber("kP",); // SmartDashboard.putNumber("kI", // Drivetrain.getInstance().thetaController.getI()); @@ -91,6 +94,8 @@ public void robotPeriodic() { // SmartDashboard.putData(Drivetrain.getInstance()); // SmartDashboard.putData(Claw.getInstance()); + + NetworkTableInstance.getDefault().flushLocal(); NetworkTableInstance.getDefault().flush(); } @@ -139,6 +144,16 @@ public void autonomousInit() { .setPose(Trajectories.apply(new Pose2d(1.91, 2.75, Rotation2d.fromDegrees(180)))); Autons.noAuton.schedule(); break; + case "Bottom Path Middle": + Drivetrain.getInstance() + .setPose(Trajectories.apply(new Pose2d(1.91, 1.09, Rotation2d.fromDegrees(180)))); + Autons.bottomPathMid.schedule(); + break; + case "Top Path Middle": + Drivetrain.getInstance() + .setPose(Trajectories.apply(new Pose2d(1.91, 4.44, Rotation2d.fromDegrees(180)))); + Autons.topPathMid.schedule(); + break; default: Drivetrain.getInstance() .setPose(Trajectories.apply(new Pose2d(1.91, 2.75, Rotation2d.fromDegrees(180)))); @@ -159,11 +174,12 @@ public void teleopInit() { Autons.middleAndCross.cancel(); Autons.noAuton.cancel(); Drivetrain.getInstance().setYaw(180); + new ZeroElevator(); } @Override public void teleopPeriodic() { - + telemetry.putButtons(); } @Override diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index a51ecdb..3f1576d 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -143,12 +143,12 @@ public static final class Claw { public static final class AngledElevator { public static final double kP = 0.12; // Based on RoboCode2023; TUNE - public static final double kG = 0.087; // Based on RoboCode2023; TUNE + public static final double kG = 0.073; // Based on RoboCode2023; TUNE - public static final double MAX_ERROR = 100; + public static final double MAX_ERROR = 500; public static final double CRUISE_VELOCITY = 7447; // NEEDS TO BE TUNED (based on RoboCode2023) - public static final double CRUISE_ACCELERATION = 4447; // NEEDS TO BE TUNED (based on RoboCode2023) + public static final double CRUISE_ACCELERATION = 4447; // NEEDS TO BE TUNED (based on RoboCode2023) 4447 public static final int MASTER_ID = 15; // Left Motor public static final int FOLLOWER_ID = 14; // Right Motor @@ -158,15 +158,16 @@ public static final class AngledElevator { public static final boolean FOLLOWER_INVERTED = false; // TODO // Thresholds for soft-limits - public static final double FORWARD_LIMIT = 41000; + public static final double FORWARD_LIMIT = 45500; public static final double REVERSE_LIMIT = 0; // Time for the motor to go from neutral to full public static final double RAMP_TIME = 0.01; + // POSITIONS: Low, Middle, High, Human Player (HP) public static double[] POSITIONS = { - 10018, 27000, 41000, 28750 + 10018, 27000, 45000, 31000 }; // HORIZONTAL Offsets: Middle, High, Human Player (HP) diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index beeede1..3895a70 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -11,102 +11,125 @@ public class Autons { - /** - * topPath : Grab game piece from bot, drop off at node, and move back out of - * community zone - */ + /** + * topPath : Grab game piece from bot, drop off at node, and move back out of + * community zone + */ - public static final SequentialCommandGroup topPath = new SequentialCommandGroup( - new ZeroElevator(), - new CloseClaw(), - new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2]), - new OpenClaw(), - new MoveToPosition(0), - new SwervePositionController( - Trajectories.topPath, - () -> Rotation2d.fromDegrees(180), - () -> Rotation2d.fromDegrees(180))); + public static final SequentialCommandGroup topPath = new SequentialCommandGroup( + new ZeroElevator(), + new CloseClaw(), + new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2]), + new OpenClaw(), + new MoveToPosition(0), + new SwervePositionController( + Trajectories.topPath, + () -> Rotation2d.fromDegrees(180), + () -> Rotation2d.fromDegrees(180))); - /** - * middlePath : Grab game piece from bot, drop off at node, and move back on - * chargePad - */ + /** + * middlePath : Grab game piece from bot, drop off at node, and move back on + * chargePad + */ - public static final SequentialCommandGroup middlePath = new SequentialCommandGroup( - new ZeroElevator(), - new CloseClaw(), - new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2]), - new OpenClaw(), - new MoveToPosition(0), - new SwervePositionController( - Trajectories.chargePad, - () -> Rotation2d.fromDegrees(180), - () -> Rotation2d.fromDegrees(180)), - new AlignPitch()); + public static final SequentialCommandGroup middlePath = new SequentialCommandGroup( + new ZeroElevator(), + new CloseClaw(), + new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2]), + new OpenClaw(), + new ZeroElevator(), + new SwervePositionController( + Trajectories.chargePad, + () -> Rotation2d.fromDegrees(180), + () -> Rotation2d.fromDegrees(180)), + new AlignPitch()); public static final SequentialCommandGroup middleAndCrossMidCube = new SequentialCommandGroup( - new ZeroElevator(), - new CloseClaw(), - new MoveToPosition(RobotMap.AngledElevator.POSITIONS[1]), - new OpenClaw(), - new MoveToPosition(0) - .alongWith( - new SwervePositionController( - Trajectories.middleAndCross1, - () -> Rotation2d.fromDegrees(180), - () -> Rotation2d.fromDegrees(180))), - new SwervePositionController( - Trajectories.middleAndCross2, - () -> Rotation2d.fromDegrees(180), - () -> Rotation2d.fromDegrees(180)), - new AlignPitch()); - /** - * bottomPath : Grab game piece from bot, drop off at node, and move back out of - * community zone - */ + new ZeroElevator(), + new CloseClaw(), + new MoveToPosition(RobotMap.AngledElevator.POSITIONS[1]), + new OpenClaw(), + new ZeroElevator() + .alongWith( + new SwervePositionController( + Trajectories.middleAndCross1, + () -> Rotation2d.fromDegrees(180), + () -> Rotation2d.fromDegrees(180))), + new SwervePositionController( + Trajectories.middleAndCross2, + () -> Rotation2d.fromDegrees(180), + () -> Rotation2d.fromDegrees(180)), + new AlignPitch()); + /** + * bottomPath : Grab game piece from bot, drop off at node, and move back out of + * community zone + */ - public static final SequentialCommandGroup bottomPath = new SequentialCommandGroup( - new ZeroElevator(), - new CloseClaw(), - new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2]), - new OpenClaw(), - new MoveToPosition(0), - new SwervePositionController( - Trajectories.bottomPath, - () -> Rotation2d.fromDegrees(180), - () -> Rotation2d.fromDegrees(180))); + public static final SequentialCommandGroup bottomPath = new SequentialCommandGroup( + new ZeroElevator(), + new CloseClaw(), + new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2]), + new OpenClaw(), + new ZeroElevator(), + new SwervePositionController( + Trajectories.bottomPath, + () -> Rotation2d.fromDegrees(180), + () -> Rotation2d.fromDegrees(180))); - /** - * middleAndCross : Grab game piece from bot, drop off at node, and move back - * over chargePad to cross commmunity line and move back on chargePad (21 point - * auton) - */ + public static final SequentialCommandGroup bottomPathMid = new SequentialCommandGroup( + new ZeroElevator(), + new CloseClaw(), + new MoveToPosition(RobotMap.AngledElevator.POSITIONS[1]), + new OpenClaw(), + new ZeroElevator(), + new SwervePositionController( + Trajectories.bottomPath, + () -> Rotation2d.fromDegrees(180), + () -> Rotation2d.fromDegrees(180))); - public static final SequentialCommandGroup middleAndCross = new SequentialCommandGroup( - new ZeroElevator(), - new CloseClaw(), - new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2]), - new OpenClaw(), - new MoveToPosition(0) - .alongWith( - new SwervePositionController( - Trajectories.middleAndCross1, - () -> Rotation2d.fromDegrees(180), - () -> Rotation2d.fromDegrees(180))), - new SwervePositionController( - Trajectories.middleAndCross2, - () -> Rotation2d.fromDegrees(180), - () -> Rotation2d.fromDegrees(180)), - new AlignPitch()); + public static final SequentialCommandGroup topPathMid = new SequentialCommandGroup( + new ZeroElevator(), + new CloseClaw(), + new MoveToPosition(RobotMap.AngledElevator.POSITIONS[1]), + new OpenClaw(), + new ZeroElevator(), + new SwervePositionController( + Trajectories.topPath, + () -> Rotation2d.fromDegrees(180), + () -> Rotation2d.fromDegrees(180))); - /** - * noAuton : Grab game piece from bot and drop off at node - */ + /** + * middleAndCross : Grab game piece from bot, drop off at node, and move back + * over chargePad to cross commmunity line and move back on chargePad (21 point + * auton) + */ - public static final SequentialCommandGroup noAuton = new SequentialCommandGroup( - new ZeroElevator(), - new CloseClaw(), - new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2]), - new OpenClaw(), - new MoveToPosition(0)); + public static final SequentialCommandGroup middleAndCross = new SequentialCommandGroup( + new ZeroElevator(), + new CloseClaw(), + new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2]), + new OpenClaw(), + new ZeroElevator() + .alongWith( + new SwervePositionController( + Trajectories.middleAndCross1, + () -> Rotation2d.fromDegrees(180), + () -> Rotation2d.fromDegrees(180))), + new SwervePositionController( + Trajectories.middleAndCross2, + () -> Rotation2d.fromDegrees(180), + () -> Rotation2d.fromDegrees(180)), + new AlignPitch()); + + /** + * noAuton : Grab game piece from bot and drop off at node + */ + + public static final SequentialCommandGroup noAuton = new SequentialCommandGroup( + new ZeroElevator(), + new CloseClaw(), + new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2]), + new OpenClaw(), + new MoveToPosition(0)); + // new ZeroElevator()); } diff --git a/src/main/java/frc/robot/commands/claw/ToggleClaw.java b/src/main/java/frc/robot/commands/claw/ToggleClaw.java index 093e061..3b9aeb3 100644 --- a/src/main/java/frc/robot/commands/claw/ToggleClaw.java +++ b/src/main/java/frc/robot/commands/claw/ToggleClaw.java @@ -5,7 +5,7 @@ import frc.robot.OI; import frc.robot.subsystems.Claw; -public class ToggleClaw extends InstantCommand{ +public class ToggleClaw extends InstantCommand { public ToggleClaw() { addRequirements(Claw.getInstance()); } diff --git a/src/main/java/frc/robot/commands/elevator/MoveToPosition.java b/src/main/java/frc/robot/commands/elevator/MoveToPosition.java index 3134d4a..25dca35 100644 --- a/src/main/java/frc/robot/commands/elevator/MoveToPosition.java +++ b/src/main/java/frc/robot/commands/elevator/MoveToPosition.java @@ -27,6 +27,7 @@ public void initialize() { */ public void execute() { AngledElevator.getInstance().moveToPosition(position); + } /** diff --git a/src/main/java/frc/robot/commands/elevator/SoftZero.java b/src/main/java/frc/robot/commands/elevator/SoftZero.java new file mode 100644 index 0000000..5083fc2 --- /dev/null +++ b/src/main/java/frc/robot/commands/elevator/SoftZero.java @@ -0,0 +1,41 @@ +package frc.robot.commands.elevator; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotMap; +import frc.robot.subsystems.AngledElevator; + +public class SoftZero extends CommandBase { + + public SoftZero() { + addRequirements(AngledElevator.getInstance()); + } + + /** + * Slowly move the elevator down + */ + + public void execute() { + AngledElevator.getInstance().setElevatorPower(-0.02); + } + + /** + * if Elevator is at "Zero", stop Power + */ + + @Override + public boolean isFinished() { + return AngledElevator.getInstance().extensionStop(); + } + + /** + * End: Reset Encoders and Move Elevator to lowest position (ground level) + */ + + public void end(boolean interrupted) { + if(!interrupted) { + AngledElevator.getInstance().resetEncoders(); + AngledElevator.getInstance().setDesiredPosition(0); + AngledElevator.getInstance().setElevatorPower(0); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java index 528df98..cfa87be 100644 --- a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java +++ b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java @@ -32,7 +32,11 @@ public boolean isFinished() { */ public void end(boolean interrupted) { - AngledElevator.getInstance().resetEncoders(); - AngledElevator.getInstance().setDesiredPosition(0); + if (!interrupted) { + AngledElevator.getInstance().resetEncoders(); + AngledElevator.getInstance().setDesiredPosition(0); + AngledElevator.getInstance().setElevatorPower(0); + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/AngledElevator.java b/src/main/java/frc/robot/subsystems/AngledElevator.java index 47df9f6..56eff3c 100644 --- a/src/main/java/frc/robot/subsystems/AngledElevator.java +++ b/src/main/java/frc/robot/subsystems/AngledElevator.java @@ -2,6 +2,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.DigitalInput; @@ -42,6 +43,9 @@ private void initElevator() { // make follower motor follow master motor follower.follow(master); + master.configFactoryDefault(); + + master.setNeutralMode(NeutralMode.Brake); master.config_kP(Constants.SLOT_INDEX, RobotMap.AngledElevator.kP); master.configForwardSoftLimitEnable(true); @@ -64,6 +68,7 @@ private void initElevator() { */ public void moveToPosition(double desiredPosition) { + // this.desiredPosition = desiredPosition2; master.set(ControlMode.MotionMagic, desiredPosition, DemandType.ArbitraryFeedForward, RobotMap.AngledElevator.kG); SmartDashboard.putNumber("Elevator Desired", desiredPosition); } @@ -82,7 +87,7 @@ public void setDesiredPosition(double position) { */ public double getDesiredPosition() { - return desiredPosition; + return this.desiredPosition; } /** @@ -92,7 +97,7 @@ public double getDesiredPosition() { */ public boolean checkExtend(double desiredposition) { - return Math.abs(desiredposition - master.getSelectedSensorPosition()) < RobotMap.AngledElevator.MAX_ERROR; + return (Math.abs(desiredposition - getPosition()) < RobotMap.AngledElevator.MAX_ERROR); } /** @@ -163,5 +168,6 @@ public void initSendable(SendableBuilder builder) { builder.setActuator(true); builder.setSafeState(() -> setElevatorPower(0)); builder.addDoubleProperty("Current Elevator Position", this::getPosition, this::moveToPosition); + builder.addBooleanProperty("Limit Switch", ()->limitSwitch.get(), null); } } diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index d327364..edc1873 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -1,34 +1,120 @@ -// package frc.robot.util; - -// import edu.wpi.first.networktables.NetworkTable; -// import edu.wpi.first.networktables.NetworkTableEntry; -// import edu.wpi.first.networktables.NetworkTableInstance; -// import edu.wpi.first.util.sendable.Sendable; -// import edu.wpi.first.util.sendable.SendableBuilder; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import frc.robot.RobotMap; -// import frc.robot.subsystems.Drivetrain; - -// public final class Telemetry { - -// NetworkTableInstance inst = NetworkTableInstance.getDefault(); -// NetworkTable datatable = inst.getTable("SmartDashboard"); - -// public Telemetry() { -// inst.startDSClient(); -// } - -// private static Telemetry instance; - - -// public void DrivetrainSendableBuilder(SendableBuilder builder){ -// builder.addDoubleProperty("Translation kP", D, null); -// } - -// // public static Telemetry getInstance() { -// // if (instance == null) { -// // instance = new Telemetry(); -// // } -// // return instance; -// // } -// } \ No newline at end of file +package frc.robot.util; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableEvent; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableValue; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.OI; +import frc.robot.RobotMap; +import frc.robot.RobotMap.SwervePositionController; +import frc.robot.subsystems.Drivetrain; + +public class Telemetry { + NetworkTable table; + NetworkTableInstance inst; + public Telemetry(){ + inst = NetworkTableInstance.getDefault(); + table = inst.getTable("datatable"); + } + + public void putButtons() { + NetworkTable buttons = table.getSubTable("Button State"); + + NetworkTableEntry rightDPad = buttons.getEntry("Right DPad State"); + + boolean state = (OI.getInstance().getOperator().getRightDPadButtonState()); + if (state) { + rightDPad.setNumber(1); + } + else { + rightDPad.setNumber(0); + } + } + + // public void putSwerveModule(){ + // NetworkTable swervemodule = table.getSubTable("Swerve Module"); + + // NetworkTableEntry rotationkP = swervemodule.getEntry("Rotation kP"); + // rotationkP.setDouble(RobotMap.SwerveModule.ROTATION_KP); + + // NetworkTableEntry translationkP = swervemodule.getEntry("Translation kP"); + // translationkP.setDouble(RobotMap.SwerveModule.TRANSLATION_KP); + + // NetworkTableEntry translationkI = swervemodule.getEntry("Transltion kI"); + // translationkI.setDouble(RobotMap.SwerveModule.TRANSLATION_KI); + + // NetworkTableEntry translationkD = swervemodule.getEntry("Translation kD"); + // translationkD.setDouble(RobotMap.SwerveModule.TRANSLATION_KD); + + // NetworkTableEntry translationkS = swervemodule.getEntry("Translation kS"); + // translationkS.setDouble(RobotMap.SwerveModule.TRANSLATION_KS); + + // NetworkTableEntry translationkV = swervemodule.getEntry("Transltion kV"); + // translationkV.setDouble(RobotMap.SwerveModule.TRANSLATION_KV); + + // NetworkTableEntry translationkA = swervemodule.getEntry("Translation kA"); + // translationkA.setDouble(RobotMap.SwerveModule.TRANSLATION_KA); + // } + // public void putDrivetrain(){ + // NetworkTable drivetrain = table.getSubTable("Drivetrain"); + // NetworkTableEntry kP = drivetrain.getEntry("Pigeon kP"); + // kP.setDouble(RobotMap.Drivetrain.PIGEON_kP); + // } + // public void putElevator(){ + // NetworkTable elevator = table.getSubTable("Elevator"); + + // NetworkTableEntry kP = elevator.getEntry("kP"); + // kP.setDouble(RobotMap.AngledElevator.kP); + + // NetworkTableEntry kG = elevator.getEntry("kG"); + // kG.setDouble(RobotMap.AngledElevator.kG); + // } + + // public void putAlignPitch(){ + // NetworkTable alignpitch = table.getSubTable("Align Pitch"); + + // NetworkTableEntry kP = alignpitch.getEntry("kP"); + // kP.setDouble(RobotMap.AlignPitch.kP); + + // NetworkTableEntry kI = alignpitch.getEntry("kI"); + // kI.setDouble(RobotMap.AlignPitch.kI); + + // NetworkTableEntry kD = alignpitch.getEntry("kD"); + // kD.setDouble(RobotMap.AlignPitch.kD); + // } + + // public void putSwervePositionController(){ + // NetworkTable swervepositioncontroller = table.getSubTable("SwervePositionController"); + + // NetworkTableEntry X_kP = swervepositioncontroller.getEntry("X_kP"); + // X_kP.setDouble(RobotMap.SwervePositionController.X_kP); + + // NetworkTableEntry X_kI = swervepositioncontroller.getEntry("X_kI"); + // X_kI.setDouble(RobotMap.SwervePositionController.X_kI); + + // NetworkTableEntry X_kD = swervepositioncontroller.getEntry("X_kD"); + // X_kD.setDouble(RobotMap.SwervePositionController.X_kD); + + // NetworkTableEntry Y_kP = swervepositioncontroller.getEntry("Y_kP"); + // Y_kP.setDouble(RobotMap.SwervePositionController.Y_kP); + + // NetworkTableEntry Y_kI = swervepositioncontroller.getEntry("Y_kI"); + // Y_kI.setDouble(RobotMap.SwervePositionController.Y_kI); + + // NetworkTableEntry Y_kD = swervepositioncontroller.getEntry("Y_kD"); + // Y_kD.setDouble(RobotMap.SwervePositionController.Y_kD); + + // NetworkTableEntry THETA_kP = swervepositioncontroller.getEntry("THETA_kP"); + // THETA_kP.setDouble(RobotMap.SwervePositionController.THETA_kP); + + // NetworkTableEntry THETA_kI = swervepositioncontroller.getEntry("THETA_kI"); + // THETA_kI.setDouble(RobotMap.SwervePositionController.THETA_kI); + + // NetworkTableEntry THETA_kD = swervepositioncontroller.getEntry("THETA_kD"); + // THETA_kD.setDouble(RobotMap.SwervePositionController.THETA_kD); + // } +} \ No newline at end of file