diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java index 84525ea..180c4a7 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java @@ -63,6 +63,7 @@ public class PhoenixBot1 { private void setupRobot () { collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance"); + downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance"); leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance"); rightPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Right Pole Distance"); diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java index 6bfef1d..707783a 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -47,6 +47,7 @@ public class PhoenixTeleOPState extends CyberarmState { engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle); engine.telemetry.addData("Drive Power", drivePower); engine.telemetry.addData("Delta Rotation", DeltaRotation); + engine.telemetry.addData("Cone Distance", robot.collectorDistance.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Collector Height", robot.downSensor.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Left Pole Distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Right Pole Distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM));