From ccff41552314e7a649ca0ec5ac02bd063d947b39 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Sun, 29 Jan 2023 12:10:04 -0600 Subject: [PATCH] Added Horizontal Encoder, Renamed "OdometerEncoder" to "OdometerEncoderRight" and made sure there weren't any issues when building (everything is good) --- .../States/CollectorDistanceState.java | 4 ++-- .../timecrafters/Autonomous/States/TopArm.java | 2 +- .../TeleOp/states/PhoenixBot1.java | 18 +++++++++++++----- .../TeleOp/states/PhoenixTeleOPState.java | 1 + 4 files changed, 17 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java index c7cc16e..eab12b1 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -77,7 +77,7 @@ public class CollectorDistanceState extends CyberarmState { robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.OdometerEncoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - robot.OdometerEncoderHorizonatal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); @@ -85,7 +85,7 @@ public class CollectorDistanceState extends CyberarmState { robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - robot.OdometerEncoderHorizonatal.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.collectorLeft.setPower(1); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java index ed25616..081a466 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java @@ -27,7 +27,7 @@ public class TopArm extends CyberarmState { @Override public void start() { - up = robot.HighRiseMotor.getCurrentPosition() < UpperRiserLeftPos; + up = robot.ArmMotor.getCurrentPosition() < UpperRiserLeftPos; } @Override 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 e79a7cb..e72e4c6 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java @@ -46,7 +46,7 @@ public class PhoenixBot1 { public Rev2mDistanceSensor collectorDistance, /*downSensor, */leftPoleDistance, rightPoleDistance; - public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoder, OdometerEncoderLeft, ArmMotor, armMotorEncoder; + public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizontal, ArmMotor, armMotorEncoder; public CRServo collectorLeft, collectorRight; @@ -174,11 +174,11 @@ public class PhoenixBot1 { // Dead Wheel encoder for driving - OdometerEncoder = engine.hardwareMap.dcMotor.get("odometerEncoderR"); + OdometerEncoderRight = engine.hardwareMap.dcMotor.get("odometerEncoderR"); - OdometerEncoder.setDirection(DcMotorSimple.Direction.REVERSE); - OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + OdometerEncoderRight.setDirection(DcMotorSimple.Direction.REVERSE); + OdometerEncoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); OdometerEncoderLeft = engine.hardwareMap.dcMotor.get("odometerEncoderL"); @@ -186,6 +186,14 @@ public class PhoenixBot1 { OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + OdometerEncoderHorizontal = engine.hardwareMap.dcMotor.get("odometerEncoderH"); + + OdometerEncoderHorizontal.setDirection(DcMotorSimple.Direction.REVERSE); + OdometerEncoderHorizontal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + OdometerEncoderHorizontal.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + + // HighRiserLeft.setDirection(Servo.Direction.REVERSE); // HighRiserRight.setDirection(Servo.Direction.FORWARD); 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 26ea9b7..20ceaed 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -61,6 +61,7 @@ public class PhoenixTeleOPState extends CyberarmState { engine.telemetry.addData("Right Pole Distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Odometer Encoder, Right", robot.OdometerEncoderRight.getCurrentPosition()); engine.telemetry.addData("Odometer Encoder, Left", robot.OdometerEncoderLeft.getCurrentPosition()); + engine.telemetry.addData("Odometer Encoder, Horizontal", robot.OdometerEncoderHorizontal.getCurrentPosition()); } @Override