From afebf842739f1219276610867dd59672137ff3b9 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sun, 22 Jan 2023 09:10:31 -0600 Subject: [PATCH] Implement Robot#unitToTicks and Robot#ticksToUnit- needs testing --- .../minibots/cyberarm/chiron/Robot.java | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java index 1f9cf74..dc21850 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java @@ -55,6 +55,8 @@ public class Robot { private final FieldLocalizer fieldLocalizer; private final double radius, diameter; + private final double wheelRadius, gearRatio, ticksPerRevolution; + private boolean LEDStatusToggle = false; private double lastLEDStatusAnimationTime = 0; @@ -71,6 +73,11 @@ public class Robot { imuAngleOffset = hardwareConfig("imu_angle_offset").value(); + wheelRadius = tuningConfig("wheel_radius").value(); + gearRatio = tuningConfig("wheel_gear_ratio").value(); + ticksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value(); + + // FIXME: Rename motors in configuration // Define hardware backLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "left_drive"); // MOTOR PORT: ? @@ -312,17 +319,17 @@ public class Robot { public int unitToTicks(DistanceUnit unit, double distance) { double inches = unit.toInches(unit.fromUnit(unit, distance)); // NOTE: UNTESTED - // FIXME: This should be stored as a presudo constant at initialization - double wheelRadius = tuningConfig("wheel_radius").value(); - double gearRatio = tuningConfig("wheel_gear_ratio").value(); - double ticksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value(); + double ticks = wheelRadius * 2 * Math.PI * gearRatio * inches * ticksPerRevolution; - return 0; + return (int)ticks; // NOTE: UNTESTED } // For: Drive Wheels public double ticksToUnit(DistanceUnit unit, int ticks) { - return 0; + // Convert to inches, then to unit. + double inches = wheelRadius * 2 * Math.PI * gearRatio * ticks / ticksPerRevolution; + + return unit.fromUnit(DistanceUnit.INCH, inches); // NOTE: UNTESTED } // For: Arm