diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/RoadRunner/drive/DriveConstants.java index 65c25be..c41e660 100644 --- a/TeamCode/src/main/java/org/RoadRunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/RoadRunner/drive/DriveConstants.java @@ -18,8 +18,8 @@ public class DriveConstants { /* * These are motor constants that should be listed online for your motors. */ - public static final double TICKS_PER_REV = 28; - public static final double MAX_RPM = 6000; + public static final double TICKS_PER_REV = 560; + public static final double MAX_RPM = 300; /* * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. @@ -30,7 +30,7 @@ public class DriveConstants { * from DriveVelocityPIDTuner. */ public static final boolean RUN_USING_ENCODER = true; - public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(2.5, 0.1, 0.2, + public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0, getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV)); /* @@ -41,9 +41,9 @@ public class DriveConstants { * angular distances although most angular parameters are wrapped in Math.toRadians() for * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ - public static double WHEEL_RADIUS = 1.47637796; // in - public static double GEAR_RATIO = 20; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 8; // in + public static double WHEEL_RADIUS = 1.47637795; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed + public static double TRACK_WIDTH = 10; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using @@ -62,8 +62,8 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VEL = 12; - public static double MAX_ACCEL = 3; + public static double MAX_VEL = 37.10542498; + public static double MAX_ACCEL = 37.10542498; public static double MAX_ANG_VEL = Math.toRadians(60); public static double MAX_ANG_ACCEL = Math.toRadians(60); diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/RoadRunner/drive/SampleMecanumDrive.java index 904dd5a..d0fd65c 100644 --- a/TeamCode/src/main/java/org/RoadRunner/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/RoadRunner/drive/SampleMecanumDrive.java @@ -21,6 +21,7 @@ import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.VoltageSensor; @@ -46,6 +47,7 @@ import org.RoadRunner.trajectorysequence.TrajectorySequence; import org.RoadRunner.trajectorysequence.TrajectorySequenceBuilder; import org.RoadRunner.trajectorysequence.TrajectorySequenceRunner; import org.RoadRunner.util.LynxModuleUtil; +import org.firstinspires.ftc.robotcore.external.Telemetry; /* * Simple mecanum drive hardware implementation for REV hardware. @@ -60,6 +62,7 @@ public class SampleMecanumDrive extends MecanumDrive { public static double VX_WEIGHT = 1; public static double VY_WEIGHT = 1; public static double OMEGA_WEIGHT = 1; + public final StandardTrackingWheelLocalizer localizer; private TrajectorySequenceRunner trajectorySequenceRunner; @@ -116,10 +119,15 @@ public class SampleMecanumDrive extends MecanumDrive { // For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y. // BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y); - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, "Front Left"); + leftRear = hardwareMap.get(DcMotorEx.class, "Back Left"); + rightRear = hardwareMap.get(DcMotorEx.class, "Back Right"); + rightFront = hardwareMap.get(DcMotorEx.class, "Front Right"); + + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + rightRear.setDirection(DcMotorSimple.Direction.FORWARD); + rightFront.setDirection(DcMotorSimple.Direction.FORWARD); motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); @@ -140,9 +148,15 @@ public class SampleMecanumDrive extends MecanumDrive { } // TODO: reverse any motors using DcMotor.setDirection() + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + rightRear.setDirection(DcMotorSimple.Direction.FORWARD); + rightFront.setDirection(DcMotorSimple.Direction.FORWARD); // TODO: if desired, use setLocalizer() to change the localization method // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...)); + localizer = new StandardTrackingWheelLocalizer(hardwareMap); + setLocalizer(localizer); trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID); } @@ -296,6 +310,13 @@ public class SampleMecanumDrive extends MecanumDrive { return imu.getAngularOrientation().firstAngle; } + public void telemetry(Telemetry telemetry) { + telemetry.addData("front left", leftFront.getPower()); + telemetry.addData("back left", leftRear.getPower()); + telemetry.addData("front right", rightFront.getPower()); + telemetry.addData("back right", rightRear.getPower()); + } + @Override public Double getExternalHeadingVelocity() { return (double) imu.getAngularVelocity().zRotationRate; diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/RoadRunner/drive/StandardTrackingWheelLocalizer.java index 3f2e8c9..7c8bb3c 100644 --- a/TeamCode/src/main/java/org/RoadRunner/drive/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/RoadRunner/drive/StandardTrackingWheelLocalizer.java @@ -6,9 +6,11 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import org.RoadRunner.util.Encoder; +import org.firstinspires.ftc.robotcore.external.Telemetry; import java.util.Arrays; import java.util.List; @@ -28,12 +30,18 @@ import java.util.List; */ @Config public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { - public static double TICKS_PER_REV = 0; - public static double WHEEL_RADIUS = 2; // in + public static double TICKS_PER_REV = 8192; + public static double WHEEL_RADIUS = 1.47637795; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - public static double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels - public static double FORWARD_OFFSET = 4; // in; offset of the lateral wheel + public static double LATERAL_DISTANCE = 9.2913393; // in; distance between the left and right wheels + public static double FORWARD_OFFSET = 3.54833; // in; offset of the lateral wheel + + public static double X_MULTIPLIER = 0.8039; // Multiplier in the X direction + // 9.14228723 + // 8.65411042 + + public static double Y_MULTIPLIER = 0.8039; // Multiplier in the Y direction private Encoder leftEncoder, rightEncoder, frontEncoder; @@ -44,11 +52,16 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front )); - leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftEncoder")); - rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightEncoder")); - frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontEncoder")); + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "odometerEncoderL")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "odometerEncoderR")); + frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "odometerEncoderH")); // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) + rightEncoder.setDirection(Encoder.Direction.REVERSE); + leftEncoder.setDirection(Encoder.Direction.REVERSE); + frontEncoder.setDirection(Encoder.Direction.REVERSE); + + } public static double encoderTicksToInches(double ticks) { @@ -59,9 +72,9 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer @Override public List getWheelPositions() { return Arrays.asList( - encoderTicksToInches(leftEncoder.getCurrentPosition()), - encoderTicksToInches(rightEncoder.getCurrentPosition()), - encoderTicksToInches(frontEncoder.getCurrentPosition()) + encoderTicksToInches(leftEncoder.getCurrentPosition()) * X_MULTIPLIER, + encoderTicksToInches(rightEncoder.getCurrentPosition()) * X_MULTIPLIER, + encoderTicksToInches(frontEncoder.getCurrentPosition()) * Y_MULTIPLIER ); } @@ -73,9 +86,16 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer // compensation method return Arrays.asList( - encoderTicksToInches(leftEncoder.getRawVelocity()), - encoderTicksToInches(rightEncoder.getRawVelocity()), - encoderTicksToInches(frontEncoder.getRawVelocity()) + encoderTicksToInches(leftEncoder.getCorrectedVelocity()) * X_MULTIPLIER, + encoderTicksToInches(rightEncoder.getCorrectedVelocity()) * X_MULTIPLIER, + encoderTicksToInches(frontEncoder.getCorrectedVelocity()) * Y_MULTIPLIER ); } + + public void telemetry(Telemetry telemetry){ + telemetry.addData("right", rightEncoder.getCurrentPosition()); + telemetry.addData("left", leftEncoder.getCurrentPosition()); + telemetry.addData("front", frontEncoder.getCurrentPosition()); + } + } diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/LocalizationTest.java index 46659e9..2af1af5 100644 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/LocalizationTest.java +++ b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/LocalizationTest.java @@ -14,7 +14,7 @@ import org.RoadRunner.drive.SampleMecanumDrive; * exercise is to ascertain whether the localizer has been configured properly (note: the pure * encoder localizer heading may be significantly off if the track width has not been tuned). */ -@TeleOp(group = "drive") +@TeleOp(name = "TC LocalizationTest", group = "drive") public class LocalizationTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { @@ -39,6 +39,9 @@ public class LocalizationTest extends LinearOpMode { telemetry.addData("x", poseEstimate.getX()); telemetry.addData("y", poseEstimate.getY()); telemetry.addData("heading", poseEstimate.getHeading()); + + drive.localizer.telemetry(telemetry); + telemetry.update(); } } diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/StraightTest.java index 6ad903f..8e0b0ca 100644 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/StraightTest.java +++ b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/StraightTest.java @@ -38,6 +38,9 @@ public class StraightTest extends LinearOpMode { telemetry.addData("finalX", poseEstimate.getX()); telemetry.addData("finalY", poseEstimate.getY()); telemetry.addData("finalHeading", poseEstimate.getHeading()); + + drive.telemetry(telemetry); + telemetry.update(); while (!isStopRequested() && opModeIsActive()) ;