diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java index 8e701347..91bf6a09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java @@ -60,7 +60,7 @@ public static double getMotorVelocityF(double ticksPerSecond) { public static double DW_WHEEL_RADIUS = 0.944882; // in public static double DW_GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - public static double LATERAL_DISTANCE = 8.128; // in; distance between the left and right wheels + public static double LATERAL_DISTANCE = 8.136; // in; distance between the left and right wheels //public static double FORWARD_OFFSET = -6.5; // in; offset of the lateral wheel public static double FORWARD_OFFSET = -7.5; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/tuning/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/tuning/TrackingWheelLateralDistanceTuner.java index 4edda8b9..9da3e26c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/tuning/TrackingWheelLateralDistanceTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/tuning/TrackingWheelLateralDistanceTuner.java @@ -7,6 +7,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.RobotLog; +import org.firstinspires.ftc.teamcode.drive.PsiBot; import org.firstinspires.ftc.teamcode.drive.Robot; import org.firstinspires.ftc.teamcode.drive.RoboticaBot; import org.firstinspires.ftc.teamcode.drive.TrackingWheelLocalizer; @@ -125,7 +126,7 @@ public void runOpMode() throws InterruptedException { telemetry.clearAll(); telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°"); telemetry.addLine("Effective LATERAL_DISTANCE: " + - (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * RoboticaBot.LATERAL_DISTANCE); + (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * PsiBot.LATERAL_DISTANCE); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/HWTEST.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/HWTEST.java index 687f0d78..443f4c79 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/HWTEST.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/HWTEST.java @@ -6,6 +6,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.Func; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.drive.Robot; import org.firstinspires.ftc.teamcode.drive.TrackingWheelLocalizer; import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive; @@ -23,6 +25,9 @@ public void runOpMode() throws InterruptedException { Robot robot = Robot.thisRobot(hardwareMap); TrajectoryDrive drive = robot.getDrive(); waitForStart(); + int leftI = ((TrackingWheelLocalizer) drive.getLocalizer()).leftEncoder.getCurrentPosition(); + int rightI = ((TrackingWheelLocalizer) drive.getLocalizer()).rightEncoder.getCurrentPosition(); + int frontI = ((TrackingWheelLocalizer) drive.getLocalizer()).frontEncoder.getCurrentPosition(); while (opModeIsActive()) { Vector2d input = new Vector2d(-gamepad1.left_stick_y, -gamepad1.left_stick_x); drive.setWeightedDrivePower(new Pose2d(input.getX(), input.getY(), -gamepad1.right_stick_x)); @@ -31,6 +36,19 @@ public void runOpMode() throws InterruptedException { android.util.Log.i("TESTODO", "LEFT " + ((TrackingWheelLocalizer) drive.getLocalizer()).leftEncoder.getCurrentPosition()); android.util.Log.i("TESTODO", "RIGHT " + ((TrackingWheelLocalizer) drive.getLocalizer()).rightEncoder.getCurrentPosition()); android.util.Log.i("TESTODO", "CENTER " + ((TrackingWheelLocalizer) drive.getLocalizer()).frontEncoder.getCurrentPosition()); + telemetry.addData("LEFT", ((TrackingWheelLocalizer) drive.getLocalizer()).leftEncoder.getCurrentPosition()); + telemetry.addData("RIGHT", ((TrackingWheelLocalizer) drive.getLocalizer()).rightEncoder.getCurrentPosition()); + telemetry.addData("CENTER", ((TrackingWheelLocalizer) drive.getLocalizer()).frontEncoder.getCurrentPosition()); + + telemetry.addData("DELTA LEFT", ((TrackingWheelLocalizer) drive.getLocalizer()).leftEncoder.getCurrentPosition() - leftI); + telemetry.addData("DELTA RIGHT", ((TrackingWheelLocalizer) drive.getLocalizer()).rightEncoder.getCurrentPosition()- rightI); + telemetry.addData("DELTA CENTER", ((TrackingWheelLocalizer) drive.getLocalizer()).frontEncoder.getCurrentPosition() - frontI); + + telemetry.addData("DELTA L-R", (((TrackingWheelLocalizer) drive.getLocalizer()).leftEncoder.getCurrentPosition() - leftI)- (((TrackingWheelLocalizer) drive.getLocalizer()).rightEncoder.getCurrentPosition()- rightI)); + + telemetry.update(); + +