From 9845d4e48d897d453accc0dbecb046c3ad615b8b Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sun, 22 Nov 2020 11:39:17 -0500 Subject: [PATCH 01/29] Added autonomous tests --- .../API/StandardTrackingWheelLocalizer.java | 81 +++++++ .../Autonomous/AutomaticFeedforwardTuner.java | 220 ++++++++++++++++++ .../OpMode/Autonomous/BackAndForth.java | 52 +++++ .../Autonomous/DriveVelocityPIDTuner.java | 180 ++++++++++++++ .../OpMode/Autonomous/FollowerPIDTuner.java | 51 ++++ .../OpMode/Autonomous/LocalizationTest.java | 47 ++++ .../Autonomous/ManualFeedforwardTuner.java | 150 ++++++++++++ .../OpMode/Autonomous/MaxVelocityTuner.java | 78 +++++++ .../OpMode/Autonomous/SplineTest.java | 38 +++ .../OpMode/Autonomous/StrafeTest.java | 41 ++++ .../OpMode/Autonomous/StraightTest.java | 41 ++++ .../OpMode/Autonomous/TrackWidthTuner.java | 87 +++++++ .../TrackingWheelLateralDistanceTuner.java | 130 +++++++++++ .../teamcode/OpMode/Autonomous/TurnTest.java | 27 +++ 14 files changed, 1223 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/AutomaticFeedforwardTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/BackAndForth.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/DriveVelocityPIDTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/FollowerPIDTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/LocalizationTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/ManualFeedforwardTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/MaxVelocityTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/SplineTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StrafeTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StraightTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackWidthTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java new file mode 100644 index 0000000..93a88e5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java @@ -0,0 +1,81 @@ +package org.firstinspires.ftc.teamcode.API; + +import androidx.annotation.NonNull; + +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.HardwareMap; + +import org.firstinspires.ftc.teamcode.API.Util.Encoder; + +import java.util.Arrays; +import java.util.List; + +/* + * Sample tracking wheel localizer implementation assuming the standard configuration: + * + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | + * | | + * | | + * \--------------/ + * + */ +@Config +public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { + public static double TICKS_PER_REV = 0; + public static double WHEEL_RADIUS = 2; // 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 + + private Encoder leftEncoder, rightEncoder, frontEncoder; + + public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) { + super(Arrays.asList( + new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left + new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right + 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")); + + // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) + } + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + @NonNull + @Override + public List getWheelPositions() { + return Arrays.asList( + encoderTicksToInches(leftEncoder.getCurrentPosition()), + encoderTicksToInches(rightEncoder.getCurrentPosition()), + encoderTicksToInches(frontEncoder.getCurrentPosition()) + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other + // competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a + // compensation method + + return Arrays.asList( + encoderTicksToInches(leftEncoder.getRawVelocity()), + encoderTicksToInches(rightEncoder.getRawVelocity()), + encoderTicksToInches(frontEncoder.getRawVelocity()) + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/AutomaticFeedforwardTuner.java new file mode 100644 index 0000000..fec906a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/AutomaticFeedforwardTuner.java @@ -0,0 +1,220 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.API.Util.LoggingUtil; +import org.firstinspires.ftc.teamcode.API.Util.RegressionUtil; + +import java.util.ArrayList; +import java.util.List; + +import static org.firstinspires.ftc.teamcode.API.Config.Constants.MAX_RPM; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.rpmToVelocity; + +/* + * Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an + * outline of the procedure: + * 1. Slowly ramp the motor power and record encoder values along the way. + * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV) + * and an optional intercept (kStatic). + * 3. Accelerate the robot (apply constant power) and record the encoder counts. + * 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear + * regression. + */ +@Config +@Autonomous(group = "drive") +public class AutomaticFeedforwardTuner extends LinearOpMode { + public static final double MAX_POWER = 0.7; + public static final double DISTANCE = 100; // in + + @Override + public void runOpMode() throws InterruptedException { + if (RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " + + "when using the built-in drive motor velocity PID."); + } + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Press play to begin the feedforward tuning routine"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Would you like to fit kStatic?"); + telemetry.addLine("Press (A) for yes, (B) for no"); + telemetry.update(); + + boolean fitIntercept = false; + while (!isStopRequested()) { + if (gamepad1.a) { + fitIntercept = true; + while (!isStopRequested() && gamepad1.a) { + idle(); + } + break; + } else if (gamepad1.b) { + while (!isStopRequested() && gamepad1.b) { + idle(); + } + break; + } + idle(); + } + + telemetry.clearAll(); + telemetry.addLine(Misc.formatInvariant( + "Place your robot on the field with at least %.2f in of room in front", DISTANCE)); + telemetry.addLine("Press (A) to begin"); + telemetry.update(); + + while (!isStopRequested() && !gamepad1.a) { + idle(); + } + while (!isStopRequested() && gamepad1.a) { + idle(); + } + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + double maxVel = rpmToVelocity(MAX_RPM); + double finalVel = MAX_POWER * maxVel; + double accel = (finalVel * finalVel) / (2.0 * DISTANCE); + double rampTime = Math.sqrt(2.0 * DISTANCE / accel); + + List timeSamples = new ArrayList<>(); + List positionSamples = new ArrayList<>(); + List powerSamples = new ArrayList<>(); + + drive.setPoseEstimate(new Pose2d()); + + double startTime = clock.seconds(); + while (!isStopRequested()) { + double elapsedTime = clock.seconds() - startTime; + if (elapsedTime > rampTime) { + break; + } + double vel = accel * elapsedTime; + double power = vel / maxVel; + + timeSamples.add(elapsedTime); + positionSamples.add(drive.getPoseEstimate().getX()); + powerSamples.add(power); + + drive.setDrivePower(new Pose2d(power, 0.0, 0.0)); + drive.updatePoseEstimate(); + } + drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0)); + + RegressionUtil.RampResult rampResult = RegressionUtil.fitRampData( + timeSamples, positionSamples, powerSamples, fitIntercept, + LoggingUtil.getLogFile(Misc.formatInvariant( + "DriveRampRegression-%d.csv", System.currentTimeMillis()))); + + telemetry.clearAll(); + telemetry.addLine("Quasi-static ramp up test complete"); + if (fitIntercept) { + telemetry.addLine(Misc.formatInvariant("kV = %.5f, kStatic = %.5f (R^2 = %.2f)", + rampResult.kV, rampResult.kStatic, rampResult.rSquare)); + } else { + telemetry.addLine(Misc.formatInvariant("kV = %.5f (R^2 = %.2f)", + rampResult.kStatic, rampResult.rSquare)); + } + telemetry.addLine("Would you like to fit kA?"); + telemetry.addLine("Press (A) for yes, (B) for no"); + telemetry.update(); + + boolean fitAccelFF = false; + while (!isStopRequested()) { + if (gamepad1.a) { + fitAccelFF = true; + while (!isStopRequested() && gamepad1.a) { + idle(); + } + break; + } else if (gamepad1.b) { + while (!isStopRequested() && gamepad1.b) { + idle(); + } + break; + } + idle(); + } + + if (fitAccelFF) { + telemetry.clearAll(); + telemetry.addLine("Place the robot back in its starting position"); + telemetry.addLine("Press (A) to continue"); + telemetry.update(); + + while (!isStopRequested() && !gamepad1.a) { + idle(); + } + while (!isStopRequested() && gamepad1.a) { + idle(); + } + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + double maxPowerTime = DISTANCE / maxVel; + + timeSamples.clear(); + positionSamples.clear(); + powerSamples.clear(); + + drive.setPoseEstimate(new Pose2d()); + drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0)); + + startTime = clock.seconds(); + while (!isStopRequested()) { + double elapsedTime = clock.seconds() - startTime; + if (elapsedTime > maxPowerTime) { + break; + } + + timeSamples.add(elapsedTime); + positionSamples.add(drive.getPoseEstimate().getX()); + powerSamples.add(MAX_POWER); + + drive.updatePoseEstimate(); + } + drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0)); + + RegressionUtil.AccelResult accelResult = RegressionUtil.fitAccelData( + timeSamples, positionSamples, powerSamples, rampResult, + LoggingUtil.getLogFile(Misc.formatInvariant( + "DriveAccelRegression-%d.csv", System.currentTimeMillis()))); + + telemetry.clearAll(); + telemetry.addLine("Constant power test complete"); + telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)", + accelResult.kA, accelResult.rSquare)); + telemetry.update(); + } + + while (!isStopRequested()) { + idle(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/BackAndForth.java new file mode 100644 index 0000000..ead3520 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/BackAndForth.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; + +/* + * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base + * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the + * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer + * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're + * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once + * you've successfully connected, start the program, and your robot will begin moving forward and + * backward. You should observe the target position (green) and your pose estimate (blue) and adjust + * your follower PID coefficients such that you follow the target position as accurately as possible. + * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. + * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. + * These coefficients can be tuned live in dashboard. + * + * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It + * is recommended that you use the FollowerPIDTuner opmode for further fine tuning. + */ +@Config +@Autonomous(group = "drive") +public class BackAndForth extends LinearOpMode { + + public static double DISTANCE = 50; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d()) + .forward(DISTANCE) + .build(); + + Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end()) + .back(DISTANCE) + .build(); + + waitForStart(); + + while (opModeIsActive() && !isStopRequested()) { + drive.followTrajectory(trajectoryForward); + drive.followTrajectory(trajectoryBackward); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/DriveVelocityPIDTuner.java new file mode 100644 index 0000000..726c034 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/DriveVelocityPIDTuner.java @@ -0,0 +1,180 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.teamcode.API.Config.Constants; +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; + +import java.util.List; + +import static org.firstinspires.ftc.teamcode.API.Config.Constants.MOTOR_VELO_PID; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.kV; + +/* + * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed- + * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as + * important as the positional parameters. Like the other manual tuning routines, this op mode + * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's + * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC + * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully + * connected, start the program, and your robot will begin moving forward and backward according to + * a motion profile. Your job is to graph the velocity errors over time and adjust the PID + * coefficients (note: the tuning variable will not appear until the op mode finishes initializing). + * Once you've found a satisfactory set of gains, add them to the Constants.java file under the + * MOTOR_VELO_PID field. + * + * Recommended tuning process: + * + * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to + * mitigate oscillations. + * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached. + * 3. Back off kP and kD a little until the response is less oscillatory (but without lag). + * + * Pressing X (on the Xbox and Logitech F310 gamepads, square on the PS4 Dualshock gamepad) will + * pause the tuning process and enter driver override, allowing the user to reset the position of + * the bot in the event that it drifts off the path. + * Pressing A (on the Xbox and Logitech F310 gamepads, X on the PS4 Dualshock gamepad) will cede + * control back to the tuning process. + */ +@Config +@Autonomous(group = "drive") +public class DriveVelocityPIDTuner extends LinearOpMode { + public static double DISTANCE = 72; // in + + private FtcDashboard dashboard = FtcDashboard.getInstance(); + + private SampleMecanumDrive drive; + + enum Mode { + DRIVER_MODE, + TUNING_MODE + } + + private Mode mode; + + private double lastKp = MOTOR_VELO_PID.p; + private double lastKi = MOTOR_VELO_PID.i; + private double lastKd = MOTOR_VELO_PID.d; + private double lastKf = MOTOR_VELO_PID.f; + + private static MotionProfile generateProfile(boolean movingForward) { + MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); + MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, + Constants.BASE_CONSTRAINTS.maxVel, + Constants.BASE_CONSTRAINTS.maxAccel, + Constants.BASE_CONSTRAINTS.maxJerk); + } + + @Override + public void runOpMode() { + if (!RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" + + "PID is not in use", getClass().getSimpleName()); + } + + telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry()); + + drive = new SampleMecanumDrive(hardwareMap); + + mode = Mode.TUNING_MODE; + + drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Ready!"); + telemetry.update(); + telemetry.clearAll(); + + waitForStart(); + + if (isStopRequested()) return; + + boolean movingForwards = true; + MotionProfile activeProfile = generateProfile(true); + double profileStart = clock.seconds(); + + + while (!isStopRequested()) { + telemetry.addData("mode", mode); + + switch (mode) { + case TUNING_MODE: + if (gamepad1.x) { + mode = Mode.DRIVER_MODE; + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + // calculate and set the motor power + double profileTime = clock.seconds() - profileStart; + + if (profileTime > activeProfile.duration()) { + // generate a new profile + movingForwards = !movingForwards; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + MotionState motionState = activeProfile.get(profileTime); + double targetPower = kV * motionState.getV(); + drive.setDrivePower(new Pose2d(targetPower, 0, 0)); + + List velocities = drive.getWheelVelocities(); + + // update telemetry + telemetry.addData("targetVelocity", motionState.getV()); + for (int i = 0; i < velocities.size(); i++) { + telemetry.addData("velocity" + i, velocities.get(i)); + telemetry.addData( + "error" + i, + motionState.getV() - velocities.get(i) + ); + } + break; + case DRIVER_MODE: + if (gamepad1.a) { + drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + mode = Mode.TUNING_MODE; + movingForwards = true; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + break; + } + + if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d + || lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) { + drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + + lastKp = MOTOR_VELO_PID.p; + lastKi = MOTOR_VELO_PID.i; + lastKd = MOTOR_VELO_PID.d; + lastKf = MOTOR_VELO_PID.f; + } + + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/FollowerPIDTuner.java new file mode 100644 index 0000000..ed0e179 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/FollowerPIDTuner.java @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; + +/* + * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base + * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the + * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer + * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're + * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once + * you've successfully connected, start the program, and your robot will begin driving in a square. + * You should observe the target position (green) and your pose estimate (blue) and adjust your + * follower PID coefficients such that you follow the target position as accurately as possible. + * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. + * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. + * These coefficients can be tuned live in dashboard. + */ +@Config +@Autonomous(group = "drive") +public class FollowerPIDTuner extends LinearOpMode { + public static double DISTANCE = 48; // in + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0); + + drive.setPoseEstimate(startPose); + + waitForStart(); + + if (isStopRequested()) return; + + while (!isStopRequested()) { + Trajectory traj = drive.trajectoryBuilder(startPose) + .forward(DISTANCE) + .build(); + drive.followTrajectory(traj); + drive.turn(Math.toRadians(90)); + + startPose = traj.end().plus(new Pose2d(0, 0, Math.toRadians(90))); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/LocalizationTest.java new file mode 100644 index 0000000..b6a353e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/LocalizationTest.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; + +/** + * This is a simple teleop routine for testing localization. Drive the robot around like a normal + * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight + * errors are not out of the ordinary, especially with sudden drive motions). The goal of this + * 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). + */ +@Config +@TeleOp(group = "drive") +public class LocalizationTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + waitForStart(); + + while (!isStopRequested()) { + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + + drive.update(); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("x", poseEstimate.getX()); + telemetry.addData("y", poseEstimate.getY()); + telemetry.addData("heading", poseEstimate.getHeading()); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/ManualFeedforwardTuner.java new file mode 100644 index 0000000..d6f5021 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/ManualFeedforwardTuner.java @@ -0,0 +1,150 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.kinematics.Kinematics; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.teamcode.API.Config.Constants; +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; + +import java.util.Objects; + +import static org.firstinspires.ftc.teamcode.API.Config.Constants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.kA; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.kStatic; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.kV; + +/* + * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary, + * tuning these coefficients is just as important as the positional parameters. Like the other + * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard, + * connect your computer to the RC's WiFi network. In your browser, navigate to + * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if + * you are using the Control Hub. Once you've successfully connected, start the program, and your + * robot will begin moving forward and backward according to a motion profile. Your job is to graph + * the velocity errors over time and adjust the feedforward coefficients. Once you've found a + * satisfactory set of gains, add them to the appropriate fields in the Constants.java file. + * + * Pressing X (on the Xbox and Logitech F310 gamepads, square on the PS4 Dualshock gamepad) will + * pause the tuning process and enter driver override, allowing the user to reset the position of + * the bot in the event that it drifts off the path. + * Pressing A (on the Xbox and Logitech F310 gamepads, X on the PS4 Dualshock gamepad) will cede + * control back to the tuning process. + */ +@Config +@Autonomous(group = "drive") +public class ManualFeedforwardTuner extends LinearOpMode { + public static double DISTANCE = 72; // in + + private FtcDashboard dashboard = FtcDashboard.getInstance(); + + private SampleMecanumDrive drive; + + enum Mode { + DRIVER_MODE, + TUNING_MODE + } + + private Mode mode; + + private static MotionProfile generateProfile(boolean movingForward) { + MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); + MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, + Constants.BASE_CONSTRAINTS.maxVel, + Constants.BASE_CONSTRAINTS.maxAccel, + Constants.BASE_CONSTRAINTS.maxJerk); + } + + @Override + public void runOpMode() { + if (RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " + + "when using the built-in drive motor velocity PID."); + } + + telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry()); + + drive = new SampleMecanumDrive(hardwareMap); + + mode = Mode.TUNING_MODE; + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Ready!"); + telemetry.update(); + telemetry.clearAll(); + + waitForStart(); + + if (isStopRequested()) return; + + boolean movingForwards = true; + MotionProfile activeProfile = generateProfile(true); + double profileStart = clock.seconds(); + + + while (!isStopRequested()) { + telemetry.addData("mode", mode); + + switch (mode) { + case TUNING_MODE: + if (gamepad1.x) { + mode = Mode.DRIVER_MODE; + } + + // calculate and set the motor power + double profileTime = clock.seconds() - profileStart; + + if (profileTime > activeProfile.duration()) { + // generate a new profile + movingForwards = !movingForwards; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + MotionState motionState = activeProfile.get(profileTime); + double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic); + + drive.setDrivePower(new Pose2d(targetPower, 0, 0)); + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + double currentVelo = poseVelo.getX(); + + // update telemetry + telemetry.addData("targetVelocity", motionState.getV()); + telemetry.addData("poseVelocity", currentVelo); + telemetry.addData("error", currentVelo - motionState.getV()); + break; + case DRIVER_MODE: + if (gamepad1.a) { + mode = Mode.TUNING_MODE; + movingForwards = true; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + break; + } + + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/MaxVelocityTuner.java new file mode 100644 index 0000000..4b501e8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/MaxVelocityTuner.java @@ -0,0 +1,78 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.API.Config.Constants; +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; + +import java.util.Objects; + +/** + * This routine is designed to calculate the maximum velocity your bot can achieve under load. It + * will also calculate the effective kF value for your velocity PID. + *

+ * Upon pressing start, your bot will run at max power for RUNTIME seconds. + *

+ * Further fine tuning of kF may be desired. + */ +@Config +@Autonomous(group = "drive") +public class MaxVelocityTuner extends LinearOpMode { + public static double RUNTIME = 2.0; + + private ElapsedTime timer; + private double maxVelocity = 0.0; + + private VoltageSensor batteryVoltageSensor; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds."); + telemetry.addLine("Please ensure you have enough space cleared."); + telemetry.addLine(""); + telemetry.addLine("Press start when ready."); + telemetry.update(); + + waitForStart(); + + telemetry.clearAll(); + telemetry.update(); + + drive.setDrivePower(new Pose2d(1, 0, 0)); + timer = new ElapsedTime(); + + while (!isStopRequested() && timer.seconds() < RUNTIME) { + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + + maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity); + } + + drive.setDrivePower(new Pose2d()); + + double effectiveKf = Constants.getMotorVelocityF(veloInchesToTicks(maxVelocity)); + + telemetry.addData("Max Velocity", maxVelocity); + telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) idle(); + } + + private double veloInchesToTicks(double inchesPerSec) { + return inchesPerSec / (2 * Math.PI * Constants.WHEEL_RADIUS) / Constants.GEAR_RATIO * Constants.TICKS_PER_REV; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/SplineTest.java new file mode 100644 index 0000000..f3969c5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/SplineTest.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; + +/* + * This is an example of a more complex path to really test the tuning. + */ +@Autonomous(group = "drive") +public class SplineTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + if (isStopRequested()) return; + + Trajectory traj = drive.trajectoryBuilder(new Pose2d()) + .splineTo(new Vector2d(30, 30), 0) + .build(); + + drive.followTrajectory(traj); + + sleep(2000); + + drive.followTrajectory( + drive.trajectoryBuilder(traj.end(), true) + .splineTo(new Vector2d(0, 0), Math.toRadians(180)) + .build() + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StrafeTest.java new file mode 100644 index 0000000..78bc81c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StrafeTest.java @@ -0,0 +1,41 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; + +/* + * This is a simple routine to test translational drive capabilities. + */ +@Config +@Autonomous(group = "drive") +public class StrafeTest extends LinearOpMode { + public static double DISTANCE = 60; // in + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) + .strafeRight(DISTANCE) + .build(); + + waitForStart(); + + if (isStopRequested()) return; + + drive.followTrajectory(trajectory); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("finalX", poseEstimate.getX()); + telemetry.addData("finalY", poseEstimate.getY()); + telemetry.addData("finalHeading", poseEstimate.getHeading()); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) ; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StraightTest.java new file mode 100644 index 0000000..303ed6b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StraightTest.java @@ -0,0 +1,41 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; + +/* + * This is a simple routine to test translational drive capabilities. + */ +@Config +@Autonomous(group = "drive") +public class StraightTest extends LinearOpMode { + public static double DISTANCE = 60; // in + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) + .forward(DISTANCE) + .build(); + + waitForStart(); + + if (isStopRequested()) return; + + drive.followTrajectory(trajectory); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("finalX", poseEstimate.getX()); + telemetry.addData("finalY", poseEstimate.getY()); + telemetry.addData("finalHeading", poseEstimate.getHeading()); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) ; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackWidthTuner.java new file mode 100644 index 0000000..be5686d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackWidthTuner.java @@ -0,0 +1,87 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.MovingStatistics; + +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.API.Config.Constants; +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; + +/* + * This routine determines the effective track width. The procedure works by executing a point turn + * with a given angle and measuring the difference between that angle and the actual angle (as + * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient + * given angle / actual angle gives a multiplicative adjustment to the estimated track width + * (effective track width = estimated track width * given angle / actual angle). The routine repeats + * this procedure a few times and averages the values for additional accuracy. Note: a relatively + * accurate track width estimate is important or else the angular constraints will be thrown off. + */ +@Config +@Autonomous(group = "drive") +public class TrackWidthTuner extends LinearOpMode { + public static double ANGLE = 180; // deg + public static int NUM_TRIALS = 5; + public static int DELAY = 1000; // ms + + @Override + public void runOpMode() throws InterruptedException { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + // TODO: if you haven't already, set the localizer to something that doesn't depend on + // drive encoders for computing the heading + + telemetry.addLine("Press play to begin the track width tuner routine"); + telemetry.addLine("Make sure your robot has enough clearance to turn smoothly"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS); + for (int i = 0; i < NUM_TRIALS; i++) { + drive.setPoseEstimate(new Pose2d()); + + // it is important to handle heading wraparounds + double headingAccumulator = 0; + double lastHeading = 0; + + drive.turnAsync(Math.toRadians(ANGLE)); + + while (!isStopRequested() && drive.isBusy()) { + double heading = drive.getPoseEstimate().getHeading(); + headingAccumulator += Angle.norm(heading - lastHeading); + lastHeading = heading; + + drive.update(); + } + + double trackWidth = Constants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator; + trackWidthStats.add(trackWidth); + + sleep(DELAY); + } + + telemetry.clearAll(); + telemetry.addLine("Tuning complete"); + telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)", + trackWidthStats.getMean(), + trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS))); + telemetry.update(); + + while (!isStopRequested()) { + idle(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java new file mode 100644 index 0000000..7fbcd7e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java @@ -0,0 +1,130 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.API.StandardTrackingWheelLocalizer; + +/** + * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s + * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel + * wheels. + * + * Tuning Routine: + * + * 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical + * measured value. This need only be an estimated value as you will be tuning it anyways. + * + * 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an + * similar mark right below the indicator on your bot. This will be your reference point to + * ensure you've turned exactly 360°. + * + * 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help + * identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard, + * connect your computer to the RC's WiFi network. In your browser, navigate to + * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash + * if you are using the Control Hub. + * Ensure the field is showing (select the field view in top right of the page). + * + * 4. Press play to begin the tuning routine. + * + * 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise. + * + * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns. + * + * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators + * on the bot and on the ground you created earlier should be lined up. + * + * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your + * StandardTrackingWheelLocalizer.java class. + * + * 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value + * yourself. Read the heading output and follow the advice stated in the note below to manually + * nudge the values yourself. + * + * Note: + * It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with + * a line from the circumference to the center should be present, representing the bot. The line + * indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in + * dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than + * the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the + * actual bot, the LATERAL_DISTANCE should be increased. + * + * If your drawn bot oscillates around a point in dashboard, don't worry. This is because the + * position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the + * effective center of rotation. You can ignore this effect. The center of rotation will be offset + * slightly but your heading will still be fine. This does not affect your overall tracking + * precision. The heading should still line up. + */ +@Config +@TeleOp(group = "drive") +public class TrackingWheelLateralDistanceTuner extends LinearOpMode { + public static int NUM_TURNS = 10; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) { + RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the " + + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" + + "(hardwareMap));\" is called in SampleMecanumDrive.java"); + } + + telemetry.addLine("Prior to beginning the routine, please read the directions " + + "located in the comments of the opmode file."); + telemetry.addLine("Press play to begin the tuning routine."); + telemetry.addLine(""); + telemetry.addLine("Press Y/△ to stop the routine."); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.update(); + + double headingAccumulator = 0; + double lastHeading = 0; + + boolean tuningFinished = false; + + while (!isStopRequested() && !tuningFinished) { + Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x); + drive.setDrivePower(vel); + + drive.update(); + + double heading = drive.getPoseEstimate().getHeading(); + double deltaHeading = heading - lastHeading; + + headingAccumulator += Angle.normDelta(deltaHeading); + lastHeading = heading; + + telemetry.clearAll(); + telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator)); + telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading)); + telemetry.addLine(); + telemetry.addLine("Press Y/△ to conclude routine"); + telemetry.update(); + + if (gamepad1.y) + tuningFinished = true; + } + + telemetry.clearAll(); + telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°"); + telemetry.addLine("Effective LATERAL_DISTANCE: " + + (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE); + + telemetry.update(); + + while (!isStopRequested()) idle(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java new file mode 100644 index 0000000..44ca34e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; + +/* + * This is a simple routine to test turning capabilities. + */ +@Config +@Autonomous(group = "drive") +public class TurnTest extends LinearOpMode { + public static double ANGLE = 90; // deg + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + if (isStopRequested()) return; + + drive.turn(Math.toRadians(ANGLE)); + } +} From a7348b119b475d599587a76a3cc8fed86fc32ef2 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sun, 22 Nov 2020 11:46:11 -0500 Subject: [PATCH 02/29] Add license --- LICENSE.md | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 LICENSE.md diff --git a/LICENSE.md b/LICENSE.md new file mode 100644 index 0000000..f58996b --- /dev/null +++ b/LICENSE.md @@ -0,0 +1,7 @@ +Copyright © 2020 FTC Team BotsBurgh 11792 + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. From 244c7de1f35c3398a05e221702a4704a5576d8cd Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sun, 6 Dec 2020 11:03:45 -0500 Subject: [PATCH 03/29] Add dead wheel encoder base --- .../ftc/teamcode/API/Config/Naming.java | 4 ++ .../teamcode/API/{Util => HW}/Encoder.java | 2 +- .../ftc/teamcode/API/InitRobot.java | 38 +++++++++++++------ .../ftc/teamcode/API/Movement.java | 12 +++--- .../ftc/teamcode/API/SampleMecanumDrive.java | 12 +++--- .../ftc/teamcode/API/Sensor.java | 23 ++++++----- .../API/StandardTrackingWheelLocalizer.java | 18 ++++----- 7 files changed, 65 insertions(+), 44 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/{Util => HW}/Encoder.java (98%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java index 7ff15f1..fd04a87 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java @@ -36,5 +36,9 @@ public class Naming { public static final int SIDE_RED = -1; public static final int SIDE_BLUE = 1; + + public static final String ENCODER_LATERAL = "lateralEncoder"; + public static final String ENCODER_LEFT = "leftEncoder"; + public static final String ENCODER_RIGHT = "rightEncoder"; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Util/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/HW/Encoder.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Util/Encoder.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/HW/Encoder.java index 789d6ee..9226092 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Util/Encoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/HW/Encoder.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.API.Util; +package org.firstinspires.ftc.teamcode.API.HW; import com.acmerobotics.roadrunner.util.NanoClock; import com.qualcomm.robotcore.hardware.DcMotorEx; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index ff64c8e..054b8d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -9,6 +9,7 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.teamcode.API.Config.Naming; +import org.firstinspires.ftc.teamcode.API.HW.Encoder; import org.firstinspires.ftc.teamcode.API.HW.SmartMotor; import org.firstinspires.ftc.teamcode.API.HW.SmartServo; @@ -21,6 +22,8 @@ */ public class InitRobot { public static final boolean MODE_4x4 = true; // True if you are using 4x4 drive + private static SmartMotor bl, br, fl, fr; + private static Encoder leftEncoder, rightEncoder, lateralEncoder; // TODO: JavaDoc public static void init(LinearOpMode l) { @@ -35,7 +38,6 @@ public static void init(LinearOpMode l) { */ // Get motors - SmartMotor bl, br, fl, fr; bl = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_BL_NAME)); br = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_BR_NAME)); if (MODE_4x4) { @@ -122,19 +124,31 @@ public static void init(LinearOpMode l) { //gyros.put(Naming.GYRO_0_NAME, gyro0); //gyros.put(Naming.GYRO_1_NAME, gyro1); + // Get dead wheel encoders + leftEncoder = new Encoder(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); + rightEncoder = new Encoder(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_RIGHT)); + lateralEncoder = new Encoder(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LATERAL)); + + // Add encoders to list + HashMap encoders = new HashMap<>(); + encoders.put(Naming.ENCODER_LEFT, leftEncoder); + encoders.put(Naming.ENCODER_RIGHT, rightEncoder); + encoders.put(Naming.ENCODER_LATERAL, lateralEncoder); + // Add lists into the movement class - Movement movement = Movement.builder() - .motors(motors) - .servos(servos) - .crServos(crServos) - .build(); - - Robot.sensor = Sensor.builder() - .colorSensors(colorSensors) - .webcams(webcams) - .gyros(gyros) - .build(); + Movement movement = new Movement(); + Movement.motors = motors; + Movement.servos = servos; + Movement.crServos = crServos; + + Sensor sensor = new Sensor(); + Sensor.gyros = gyros; + Sensor.colorSensors = colorSensors; + Sensor.webcams = webcams; + Sensor.encoders = encoders; + Robot.movement = movement; + Robot.sensor = sensor; Robot.linearOpMode = l; // Send power to servos so they don't move diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java index 32fa97f..6edbdc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java @@ -18,16 +18,14 @@ associated documentation files (the "Software"), to deal in the Software without import com.qualcomm.robotcore.hardware.CRServo; -import org.firstinspires.ftc.teamcode.API.HW.*; - import org.firstinspires.ftc.teamcode.API.Config.Naming; +import org.firstinspires.ftc.teamcode.API.HW.SmartMotor; +import org.firstinspires.ftc.teamcode.API.HW.SmartServo; import java.util.HashMap; import java.util.Objects; -import lombok.AccessLevel; import lombok.Builder; -import lombok.Getter; /** * The Movement class. Interfaces with servos and motors so you don't have to @@ -51,9 +49,9 @@ public class Movement { private final static double FOUNDATION_OPEN = 0.3; private final static double FOUNDATION_CLOSE = 0.95; - @Getter(AccessLevel.PUBLIC) private HashMap motors; - @Getter(AccessLevel.PUBLIC) private HashMap servos; - @Getter(AccessLevel.PUBLIC) private HashMap crServos; + public static HashMap motors; + public static HashMap servos; + public static HashMap crServos; // Getters diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java index 6f23e66..5cdf759 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java @@ -29,6 +29,7 @@ import com.qualcomm.robotcore.hardware.VoltageSensor; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; +import org.firstinspires.ftc.teamcode.API.Config.Naming; import org.firstinspires.ftc.teamcode.API.Util.DashboardUtil; import org.firstinspires.ftc.teamcode.API.Util.LynxModuleUtil; @@ -83,7 +84,7 @@ public enum Mode { private LinkedList poseHistory; - private SmartMotor leftFront, leftRear, rightRear, rightFront; + private static SmartMotor leftFront, leftRear, rightRear, rightFront; private List motors; private BNO055IMU imu; @@ -128,10 +129,10 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { // upward (normal to the floor) using a command like the following: // BNO055IMUUtil.remapAxes(imu, AxesOrder.XYZ, AxesSigns.NPN); - leftFront = hardwareMap.get(SmartMotor.class, "leftFront"); - leftRear = hardwareMap.get(SmartMotor.class, "leftRear"); - rightRear = hardwareMap.get(SmartMotor.class, "rightRear"); - rightFront = hardwareMap.get(SmartMotor.class, "rightFront"); + leftFront = Robot.movement.getMotor(Naming.MOTOR_FL_NAME); + leftRear = Robot.movement.getMotor(Naming.MOTOR_BL_NAME); + rightRear = Robot.movement.getMotor(Naming.MOTOR_BR_NAME); + rightFront = Robot.movement.getMotor(Naming.MOTOR_FR_NAME); motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); @@ -155,6 +156,7 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { // TODO: if desired, use setLocalizer() to change the localization method // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...)); + setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap)); } public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java index 769a613..7299db4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java @@ -31,6 +31,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; import org.firstinspires.ftc.robotcore.internal.system.AppUtil; +import org.firstinspires.ftc.teamcode.API.HW.Encoder; import java.io.File; import java.util.HashMap; @@ -38,8 +39,6 @@ import java.util.Locale; import java.util.Objects; -import lombok.AccessLevel; -import lombok.Builder; import lombok.Getter; import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection.BACK; @@ -49,7 +48,6 @@ * This is a class which interfaces with sensors, so you don't have to. See the README for more * information. */ -@Builder public class Sensor { // Potentiometer configuration private static final int POT_MAX = 270; // Max range in degrees @@ -141,12 +139,13 @@ public class Sensor { private boolean targetVisible; // TODO: Add more sensor capability - @Getter(AccessLevel.PUBLIC) private HashMap gyros; // Initialize gyroscopes - @Getter(AccessLevel.PUBLIC) private HashMap pots; // Initialize potentiometers - @Getter(AccessLevel.PUBLIC) private HashMap buttons; // Initialize buttons - @Getter(AccessLevel.PUBLIC) private HashMap colorSensors; // Initialize color sensors - @Getter(AccessLevel.PUBLIC) private HashMap distances; // Initialize distance sensors - @Getter(AccessLevel.PUBLIC) private HashMap webcams; // Initialize webcams + public static HashMap gyros; // Initialize gyroscopes + public static HashMap pots; // Initialize potentiometers + public static HashMap buttons; // Initialize buttons + public static HashMap colorSensors; // Initialize color sensors + public static HashMap distances; // Initialize distance sensors + public static HashMap webcams; // Initialize webcams + public static HashMap encoders; // Special encoders /** * Gets the RGB value of the color sensor @@ -181,7 +180,7 @@ public int getRGB(String id) { return getRGB(id, RED_THRESH, GREEN_THRESH, BLUE_THRESH); } - public BNO055IMU getGyro(String id) { + public static BNO055IMU getGyro(String id) { return gyros.get(id); } @@ -230,6 +229,10 @@ public double getPotDeg(String id) { return (POT_MAX/(Vmax-Vmin))*(Objects.requireNonNull(pots.get(id)).getVoltage()-Vmin); // Converts voltage to angle (degrees) } + public static Encoder getEncoder(String id) { + return encoders.get(id); + } + /** * Initializes the gyroscope. * @param id ID of the gyroscope diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java index 93a88e5..0eacb01 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java @@ -5,10 +5,10 @@ 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.HardwareMap; -import org.firstinspires.ftc.teamcode.API.Util.Encoder; +import org.firstinspires.ftc.teamcode.API.Config.Naming; +import org.firstinspires.ftc.teamcode.API.HW.Encoder; import java.util.Arrays; import java.util.List; @@ -28,14 +28,14 @@ */ @Config public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { - public static double TICKS_PER_REV = 0; + public static double TICKS_PER_REV = 8192; public static double WHEEL_RADIUS = 2; // 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 - private Encoder leftEncoder, rightEncoder, frontEncoder; + private static Encoder leftEncoder, rightEncoder, lateralEncoder; public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) { super(Arrays.asList( @@ -44,9 +44,9 @@ public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) { 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 = Sensor.getEncoder(Naming.ENCODER_LEFT); + rightEncoder = Sensor.getEncoder(Naming.ENCODER_RIGHT); + lateralEncoder = Sensor.getEncoder(Naming.ENCODER_LATERAL); // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) } @@ -61,7 +61,7 @@ public List getWheelPositions() { return Arrays.asList( encoderTicksToInches(leftEncoder.getCurrentPosition()), encoderTicksToInches(rightEncoder.getCurrentPosition()), - encoderTicksToInches(frontEncoder.getCurrentPosition()) + encoderTicksToInches(lateralEncoder.getCurrentPosition()) ); } @@ -75,7 +75,7 @@ public List getWheelVelocities() { return Arrays.asList( encoderTicksToInches(leftEncoder.getRawVelocity()), encoderTicksToInches(rightEncoder.getRawVelocity()), - encoderTicksToInches(frontEncoder.getRawVelocity()) + encoderTicksToInches(lateralEncoder.getRawVelocity()) ); } } From 14271ac437aab3fd70212b388e031968d86fcaf5 Mon Sep 17 00:00:00 2001 From: Varun Damarla <57463486+damarlavarun2004@users.noreply.github.com> Date: Fri, 11 Dec 2020 20:01:15 -0500 Subject: [PATCH 04/29] Update README.md --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index fe62f46..57d6e8e 100644 --- a/README.md +++ b/README.md @@ -2,3 +2,8 @@ # BotsBurgh 2020-21 +## Welcome +BotsBurgh is a community team in South Hills, Pennsylvania and strives to provide the students to take lead and excel in the field of STEAM. The team BotsBurgh is a platform for students to work towards a joint goal and develop solutions that are larger and greater than an individual's capabilities. The team actively takes parts in challenges presented by the FIRST Tech Challenge (FTC) by applying knowledge gained in their experience within the team. + +## General Overview +Our code is written in a Modular approach. This means that you can copy over the "Big Three" files (Sensor, Movement, and Robot) over to your folder and utilize the sheer power of a well-written API for your robot. These files are great for beginners and experts alike. Better yet, the "Big Three" can be used year after year (with some exceptions. We wrote it like this because a few years ago, we realized that it was difficult to change variables all across the code, such as when we wanted the robot to go slower during both Autonomous and TeleOp. From there, Nitesh and Myself (Sambhav) decided to centralize the most-used parts of our code, such as moving the robot around and moving the lift up and down. Because of this, the programmers have had to do less work every year, leading to a better robot. This year, we incorporated ACME'S RoadRunner into our code to enhance the Mecanum driving mechanism of our robot. Using RoadRunner, we are programming an odometry system to give the robot a "sense of awareness" in a 2D plane and make autonomous programming more efficient. From a1039eb62f427b8156b99f5693f9369f24be5984 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Fri, 11 Dec 2020 20:04:21 -0500 Subject: [PATCH 05/29] Add newlines for readme --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 57d6e8e..58906b7 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,9 @@ # BotsBurgh 2020-21 ## Welcome + BotsBurgh is a community team in South Hills, Pennsylvania and strives to provide the students to take lead and excel in the field of STEAM. The team BotsBurgh is a platform for students to work towards a joint goal and develop solutions that are larger and greater than an individual's capabilities. The team actively takes parts in challenges presented by the FIRST Tech Challenge (FTC) by applying knowledge gained in their experience within the team. ## General Overview + Our code is written in a Modular approach. This means that you can copy over the "Big Three" files (Sensor, Movement, and Robot) over to your folder and utilize the sheer power of a well-written API for your robot. These files are great for beginners and experts alike. Better yet, the "Big Three" can be used year after year (with some exceptions. We wrote it like this because a few years ago, we realized that it was difficult to change variables all across the code, such as when we wanted the robot to go slower during both Autonomous and TeleOp. From there, Nitesh and Myself (Sambhav) decided to centralize the most-used parts of our code, such as moving the robot around and moving the lift up and down. Because of this, the programmers have had to do less work every year, leading to a better robot. This year, we incorporated ACME'S RoadRunner into our code to enhance the Mecanum driving mechanism of our robot. Using RoadRunner, we are programming an odometry system to give the robot a "sense of awareness" in a 2D plane and make autonomous programming more efficient. From c64d61d9b2d1715f1abb5787e913244b401f1392 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Fri, 11 Dec 2020 20:24:48 -0500 Subject: [PATCH 06/29] Update odometry wheel distances --- .../ftc/teamcode/API/StandardTrackingWheelLocalizer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java index 0eacb01..505e88a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java @@ -32,8 +32,8 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer public static double WHEEL_RADIUS = 2; // 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 = 15.1; // in; distance between the left and right wheels + public static double FORWARD_OFFSET = 8.3; // in; offset of the lateral wheel private static Encoder leftEncoder, rightEncoder, lateralEncoder; From 5b28ca41e9334f9f14a6629c0731e77fa05b85ef Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Fri, 11 Dec 2020 21:39:48 -0500 Subject: [PATCH 07/29] Add dashboard capability, add init functionality --- .../internal/FtcRobotControllerActivity.java | 14 +++++++++++--- .../TrackingWheelLateralDistanceTuner.java | 2 ++ 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java index c4dd90c..9426bad 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java @@ -47,9 +47,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE import android.os.Bundle; import android.os.IBinder; import android.preference.PreferenceManager; -import androidx.annotation.NonNull; -import androidx.annotation.Nullable; -import androidx.annotation.StringRes; import android.view.Menu; import android.view.MenuItem; import android.view.MotionEvent; @@ -62,6 +59,11 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE import android.widget.PopupMenu; import android.widget.TextView; +import androidx.annotation.NonNull; +import androidx.annotation.Nullable; +import androidx.annotation.StringRes; + +import com.acmerobotics.dashboard.FtcDashboard; import com.google.blocks.ftcrobotcontroller.ProgrammingWebHandlers; import com.google.blocks.ftcrobotcontroller.runtime.BlocksOpMode; import com.qualcomm.ftccommon.ClassManagerFactory; @@ -310,6 +312,7 @@ public boolean onMenuItemClick(MenuItem item) { } }); popupMenu.inflate(R.menu.ftc_robot_controller); + FtcDashboard.populateMenu(popupMenu.getMenu()); popupMenu.show(); } }); @@ -379,6 +382,7 @@ public boolean onMenuItemClick(MenuItem item) { } FtcAboutActivity.setBuildTimeFromBuildConfig(BuildConfig.BUILD_TIME); + FtcDashboard.start(); } protected UpdateUI createUpdateUI() { @@ -458,6 +462,7 @@ protected void onDestroy() { if (preferencesHelper != null) preferencesHelper.getSharedPreferences().unregisterOnSharedPreferenceChangeListener(sharedPreferencesListener); RobotLog.cancelWriteLogcatToDisk(); + FtcDashboard.stop(); } protected void bindToService() { @@ -523,6 +528,7 @@ public void onWindowFocusChanged(boolean hasFocus) { @Override public boolean onCreateOptionsMenu(Menu menu) { getMenuInflater().inflate(R.menu.ftc_robot_controller, menu); + FtcDashboard.populateMenu(menu); return true; } @@ -675,6 +681,7 @@ public EventLoopManager getEventLoopManager() { return service.getRobot().eventLoopManager; } }); + FtcDashboard.attachWebServer(service.getWebServer()); } private void updateUIAndRequestRobotSetup() { @@ -714,6 +721,7 @@ private void requestRobotSetup(@Nullable Runnable runOnComplete) { passReceivedUsbAttachmentsToEventLoop(); AndroidBoard.showErrorIfUnknownControlHub(); + FtcDashboard.attachEventLoop(eventLoop); } protected OpModeRegister createOpModeRegister() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java index 7fbcd7e..7a0c400 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java @@ -7,6 +7,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.RobotLog; +import org.firstinspires.ftc.teamcode.API.InitRobot; import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.API.StandardTrackingWheelLocalizer; @@ -68,6 +69,7 @@ public class TrackingWheelLateralDistanceTuner extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { + InitRobot.init(this); SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) { From 8774c03adca94380484909808e43d486f0949b87 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sat, 12 Dec 2020 21:07:50 -0500 Subject: [PATCH 08/29] Attempt to fix road runner movement --- .../ftc/teamcode/API/Config/Constants.java | 4 ++-- .../ftc/teamcode/API/InitRobot.java | 4 ++-- .../ftc/teamcode/API/SampleMecanumDrive.java | 21 ++++++++----------- .../API/StandardTrackingWheelLocalizer.java | 4 ++-- .../TrackingWheelLateralDistanceTuner.java | 2 +- .../teamcode/OpMode/Autonomous/TurnTest.java | 2 ++ .../{MecanumDrive.java => TeleOpMain.java} | 4 ++-- 7 files changed, 20 insertions(+), 21 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/{MecanumDrive.java => TeleOpMain.java} (96%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java index ebf3ff0..365b0fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java @@ -8,7 +8,7 @@ public class Constants { * These are motor constants that should be listed online for your motors. */ public static final double TICKS_PER_REV = 1; - public static final double MAX_RPM = 1; + public static final double MAX_RPM = 340; /* * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. @@ -18,7 +18,7 @@ public class Constants { * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients * from DriveVelocityPIDTuner. */ - public static final boolean RUN_USING_ENCODER = true; + public static final boolean RUN_USING_ENCODER = false; public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0, getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV)); /* diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index 054b8d6..51e6175 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -78,10 +78,10 @@ public static void init(LinearOpMode l) { // Most robots need the motor on one side to be reversed to drive forward // Reverse the motor that runs backwards when connected directly to the battery - bl.setDirection(DcMotor.Direction.REVERSE); + bl.setDirection(DcMotor.Direction.FORWARD); br.setDirection(DcMotor.Direction.FORWARD); if (MODE_4x4) { - fl.setDirection(DcMotor.Direction.REVERSE); + fl.setDirection(DcMotor.Direction.FORWARD); fr.setDirection(DcMotor.Direction.FORWARD); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java index 5cdf759..12350e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java @@ -30,11 +30,10 @@ import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.teamcode.API.Config.Naming; +import org.firstinspires.ftc.teamcode.API.HW.SmartMotor; import org.firstinspires.ftc.teamcode.API.Util.DashboardUtil; import org.firstinspires.ftc.teamcode.API.Util.LynxModuleUtil; -import org.firstinspires.ftc.teamcode.API.HW.*; - import java.util.ArrayList; import java.util.Arrays; import java.util.LinkedList; @@ -84,7 +83,7 @@ public enum Mode { private LinkedList poseHistory; - private static SmartMotor leftFront, leftRear, rightRear, rightFront; + private static SmartMotor fl, bl, br, fr; private List motors; private BNO055IMU imu; @@ -129,12 +128,12 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { // upward (normal to the floor) using a command like the following: // BNO055IMUUtil.remapAxes(imu, AxesOrder.XYZ, AxesSigns.NPN); - leftFront = Robot.movement.getMotor(Naming.MOTOR_FL_NAME); - leftRear = Robot.movement.getMotor(Naming.MOTOR_BL_NAME); - rightRear = Robot.movement.getMotor(Naming.MOTOR_BR_NAME); - rightFront = Robot.movement.getMotor(Naming.MOTOR_FR_NAME); + fl = Robot.movement.getMotor(Naming.MOTOR_FL_NAME); + bl = Robot.movement.getMotor(Naming.MOTOR_BL_NAME); + br = Robot.movement.getMotor(Naming.MOTOR_BR_NAME); + fr = Robot.movement.getMotor(Naming.MOTOR_FR_NAME); - motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); + motors = Arrays.asList(fl, bl, br, fr); for (SmartMotor motor : motors) { MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); @@ -152,8 +151,6 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); } - // TODO: reverse any motors using DcMotor.setDirection() - // TODO: if desired, use setLocalizer() to change the localization method // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...)); setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap)); @@ -374,8 +371,8 @@ public List getWheelVelocities() { } @Override - public void setMotorPowers(double v, double v1, double v2, double v3) { - Robot.movement.move4x4(v, v3, v2, v1); + public void setMotorPowers(double fl, double bl, double br, double fr) { + Robot.movement.move4x4(fl, bl, br, fr); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java index 505e88a..dd95b79 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java @@ -29,10 +29,10 @@ @Config public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { public static double TICKS_PER_REV = 8192; - public static double WHEEL_RADIUS = 2; // in + public static double WHEEL_RADIUS = 0.66; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - public static double LATERAL_DISTANCE = 15.1; // in; distance between the left and right wheels + public static double LATERAL_DISTANCE = 14.3; // in; distance between the left and right wheels public static double FORWARD_OFFSET = 8.3; // in; offset of the lateral wheel private static Encoder leftEncoder, rightEncoder, lateralEncoder; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java index 7a0c400..2244cc2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java @@ -98,7 +98,7 @@ public void runOpMode() throws InterruptedException { boolean tuningFinished = false; while (!isStopRequested() && !tuningFinished) { - Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x); + Pose2d vel = new Pose2d(gamepad1.left_stick_x, gamepad1.left_stick_y, gamepad1.right_stick_x); drive.setDrivePower(vel); drive.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java index 44ca34e..e75bb39 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.API.InitRobot; import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; /* @@ -16,6 +17,7 @@ public class TurnTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { + InitRobot.init(this); SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); waitForStart(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/MecanumDrive.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java index 2f49a80..607302f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java @@ -23,8 +23,8 @@ associated documentation files (the "Software"), to deal in the Software without import org.firstinspires.ftc.teamcode.API.InitRobot; import org.firstinspires.ftc.teamcode.API.Robot; -@TeleOp(name = "Mecanum Drive", group = "Linear OpMode") -public class MecanumDrive extends LinearOpMode { +@TeleOp(name = "TeleOp Main", group = "Linear OpMode") +public class TeleOpMain extends LinearOpMode { private static final double MAX_SPEED = 0.8; @Override public void runOpMode() { From a27ae6d93cea6c858fb992e3c7b1fb5246f70a8b Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Wed, 16 Dec 2020 19:14:27 -0500 Subject: [PATCH 09/29] Add kotlin files + attempt to fix mecanum drive --- FtcRobotController/build.gradle | 2 + TeamCode/build.gradle | 15 ++- TeamCode/build.release.gradle | 3 + .../ftc/teamcode/API/Config/Constants.java | 4 +- .../ftc/teamcode/API/InitRobot.java | 12 +- .../ftc/teamcode/API/SampleMecanumDrive.java | 21 ++- .../API/StandardTrackingWheelLocalizer.java | 13 +- .../ftc/teamcode/API/Util/MecanumDrive.kt | 123 ++++++++++++++++++ .../teamcode/API/Util/MecanumKinematics.kt | 95 ++++++++++++++ .../OpMode/Autonomous/BackAndForth.java | 2 + .../OpMode/Autonomous/StraightTest.java | 2 + .../OpMode/Autonomous/TrackWidthTuner.java | 3 + .../TrackingWheelLateralDistanceTuner.java | 11 ++ .../teamcode/OpMode/Autonomous/TurnTest.java | 2 +- .../ftc/teamcode/OpMode/TeleOp/Forward.java | 52 ++++++++ .../teamcode/OpMode/TeleOp/TeleOpMain.java | 12 +- build.gradle | 4 +- 17 files changed, 350 insertions(+), 26 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Util/MecanumDrive.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Util/MecanumKinematics.kt create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/Forward.java diff --git a/FtcRobotController/build.gradle b/FtcRobotController/build.gradle index 3ff4cea..48c1ebc 100644 --- a/FtcRobotController/build.gradle +++ b/FtcRobotController/build.gradle @@ -26,7 +26,9 @@ android { } repositories { + jcenter() maven { url = "https://dl.bintray.com/first-tech-challenge/ftcsdk/" } + google() flatDir { dirs '../libs' diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 83bf811..498bf05 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -15,6 +15,7 @@ apply from: '../build.common.gradle' repositories { + google() maven { url = "https://dl.bintray.com/first-tech-challenge/ftcsdk/" } } @@ -22,4 +23,16 @@ dependencies { annotationProcessor files('lib/OpModeAnnotationProcessor.jar') compileOnly 'org.projectlombok:lombok:1.18.16' annotationProcessor 'org.projectlombok:lombok:1.18.16' -} \ No newline at end of file +} + +buildscript { + ext.kotlinVersion = '1.4.21' + repositories { + jcenter() + google() + } + dependencies { + classpath 'com.android.tools.build:gradle:3.0.0' + classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlinVersion" + } +} diff --git a/TeamCode/build.release.gradle b/TeamCode/build.release.gradle index 5eaaaf5..ce141c0 100644 --- a/TeamCode/build.release.gradle +++ b/TeamCode/build.release.gradle @@ -1,3 +1,6 @@ +apply plugin: 'com.android.application' +apply plugin: 'kotlin-android' + dependencies { implementation project(':FtcRobotController') implementation 'org.firstinspires.ftc:RobotCore:6.0.1' diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java index 365b0fb..3d80e5b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java @@ -7,7 +7,7 @@ public class Constants { /* * These are motor constants that should be listed online for your motors. */ - public static final double TICKS_PER_REV = 1; + public static final double TICKS_PER_REV = 537.6; public static final double MAX_RPM = 340; /* @@ -31,7 +31,7 @@ public class Constants { */ public static double WHEEL_RADIUS = 2; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 1; // in + public static double TRACK_WIDTH = 2; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index 51e6175..38f90d8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -79,10 +79,10 @@ public static void init(LinearOpMode l) { // Most robots need the motor on one side to be reversed to drive forward // Reverse the motor that runs backwards when connected directly to the battery bl.setDirection(DcMotor.Direction.FORWARD); - br.setDirection(DcMotor.Direction.FORWARD); + br.setDirection(DcMotor.Direction.REVERSE); if (MODE_4x4) { fl.setDirection(DcMotor.Direction.FORWARD); - fr.setDirection(DcMotor.Direction.FORWARD); + fr.setDirection(DcMotor.Direction.REVERSE); } // Set motors to spin in the correct direction @@ -116,13 +116,13 @@ public static void init(LinearOpMode l) { //webcams.put(Naming.WEBCAM_0_NAME, webcam1); // Get gyros - //BNO055IMU gyro0 = l.hardwareMap.get(BNO055IMU.class, Naming.GYRO_0_NAME); - //BNO055IMU gyro1 = l.hardwareMap.get(BNO055IMU.class, Naming.GYRO_1_NAME); + BNO055IMU gyro0 = l.hardwareMap.get(BNO055IMU.class, Naming.GYRO_0_NAME); + BNO055IMU gyro1 = l.hardwareMap.get(BNO055IMU.class, Naming.GYRO_1_NAME); // Add gyros to list HashMap gyros = new HashMap<>(); - //gyros.put(Naming.GYRO_0_NAME, gyro0); - //gyros.put(Naming.GYRO_1_NAME, gyro1); + gyros.put(Naming.GYRO_0_NAME, gyro0); + gyros.put(Naming.GYRO_1_NAME, gyro1); // Get dead wheel encoders leftEncoder = new Encoder(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java index 12350e4..a894f9e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java @@ -9,7 +9,6 @@ import com.acmerobotics.roadrunner.control.PIDCoefficients; import com.acmerobotics.roadrunner.control.PIDFController; import com.acmerobotics.roadrunner.drive.DriveSignal; -import com.acmerobotics.roadrunner.drive.MecanumDrive; import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower; import com.acmerobotics.roadrunner.followers.TrajectoryFollower; import com.acmerobotics.roadrunner.geometry.Pose2d; @@ -33,6 +32,9 @@ import org.firstinspires.ftc.teamcode.API.HW.SmartMotor; import org.firstinspires.ftc.teamcode.API.Util.DashboardUtil; import org.firstinspires.ftc.teamcode.API.Util.LynxModuleUtil; +import org.firstinspires.ftc.teamcode.API.Util.MecanumDrive; +import org.firstinspires.ftc.teamcode.API.Util.MecanumKinematics; +import org.jetbrains.annotations.NotNull; import java.util.ArrayList; import java.util.Arrays; @@ -119,7 +121,7 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { } // TODO: adjust the names of the following hardware devices to match your configuration - imu = hardwareMap.get(BNO055IMU.class, "imu"); + imu = Sensor.getGyro(Naming.GYRO_0_NAME); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS; imu.initialize(parameters); @@ -151,9 +153,14 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); } + bl.setDirection(DcMotor.Direction.FORWARD); + br.setDirection(DcMotor.Direction.REVERSE); + fl.setDirection(DcMotor.Direction.FORWARD); + fr.setDirection(DcMotor.Direction.REVERSE); + // TODO: if desired, use setLocalizer() to change the localization method // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...)); - setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap)); + //setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap)); } public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { @@ -370,6 +377,14 @@ public List getWheelVelocities() { return wheelVelocities; } + @Override + public void setDrivePower(@NotNull Pose2d drivePower) { + List powers = MecanumKinematics.robotToWheelVelocities( + drivePower, 1.0, 1.0, 1); + Robot.movement.move4x4(powers.get(0), powers.get(3), powers.get(1), powers.get(2)); + + } + @Override public void setMotorPowers(double fl, double bl, double br, double fr) { Robot.movement.move4x4(fl, bl, br, fr); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java index dd95b79..2aa246b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java @@ -28,11 +28,11 @@ */ @Config public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { - public static double TICKS_PER_REV = 8192; + public static double TICKS_PER_REV = 4096; public static double WHEEL_RADIUS = 0.66; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - public static double LATERAL_DISTANCE = 14.3; // in; distance between the left and right wheels + public static double LATERAL_DISTANCE = 11.99; // in; distance between the left and right wheels public static double FORWARD_OFFSET = 8.3; // in; offset of the lateral wheel private static Encoder leftEncoder, rightEncoder, lateralEncoder; @@ -49,6 +49,9 @@ public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) { lateralEncoder = Sensor.getEncoder(Naming.ENCODER_LATERAL); // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) + leftEncoder.setDirection(Encoder.Direction.FORWARD); + rightEncoder.setDirection(Encoder.Direction.REVERSE); + lateralEncoder.setDirection(Encoder.Direction.FORWARD); } public static double encoderTicksToInches(double ticks) { @@ -73,9 +76,9 @@ public List getWheelVelocities() { // compensation method return Arrays.asList( - encoderTicksToInches(leftEncoder.getRawVelocity()), - encoderTicksToInches(rightEncoder.getRawVelocity()), - encoderTicksToInches(lateralEncoder.getRawVelocity()) + encoderTicksToInches(leftEncoder.getCorrectedVelocity()), + encoderTicksToInches(rightEncoder.getCorrectedVelocity()), + encoderTicksToInches(lateralEncoder.getCorrectedVelocity()) ); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Util/MecanumDrive.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Util/MecanumDrive.kt new file mode 100644 index 0000000..dc1a210 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Util/MecanumDrive.kt @@ -0,0 +1,123 @@ +package org.firstinspires.ftc.teamcode.API.Util + +import com.acmerobotics.roadrunner.drive.Drive +import com.acmerobotics.roadrunner.drive.DriveSignal +import com.acmerobotics.roadrunner.geometry.Pose2d +import com.acmerobotics.roadrunner.kinematics.Kinematics +import com.acmerobotics.roadrunner.localization.Localizer +import com.acmerobotics.roadrunner.util.Angle + +/** + * This class provides the basic functionality of a mecanum drive using [MecanumKinematics]. + * + * @param kV velocity feedforward + * @param kA acceleration feedforward + * @param kStatic additive constant feedforward + * @param trackWidth lateral distance between pairs of wheels on different sides of the robot + * @param wheelBase distance between pairs of wheels on the same side of the robot + * @param lateralMultiplier lateral multiplier + */ +abstract class MecanumDrive @JvmOverloads constructor( + private val kV: Double, + private val kA: Double, + private val kStatic: Double, + private val trackWidth: Double, + private val wheelBase: Double = trackWidth, + private val lateralMultiplier: Double = 1.0 +) : Drive() { + + /** + * Default localizer for mecanum drives based on the drive encoders and (optionally) a heading sensor. + * + * @param drive drive + * @param useExternalHeading use external heading provided by an external sensor (e.g., IMU, gyroscope) + */ + class MecanumLocalizer @JvmOverloads constructor( + private val drive: MecanumDrive, + private val useExternalHeading: Boolean = true + ) : Localizer { + private var _poseEstimate = Pose2d() + override var poseEstimate: Pose2d + get() = _poseEstimate + set(value) { + lastWheelPositions = emptyList() + lastExtHeading = Double.NaN + if (useExternalHeading) drive.externalHeading = value.heading + _poseEstimate = value + } + override var poseVelocity: Pose2d? = null + private set + private var lastWheelPositions = emptyList() + private var lastExtHeading = Double.NaN + + override fun update() { + val wheelPositions = drive.getWheelPositions() + val extHeading = if (useExternalHeading) drive.externalHeading else Double.NaN + if (lastWheelPositions.isNotEmpty()) { + val wheelDeltas = wheelPositions + .zip(lastWheelPositions) + .map { it.first - it.second } + val robotPoseDelta = MecanumKinematics.wheelToRobotVelocities( + wheelDeltas, drive.trackWidth, drive.wheelBase, drive.lateralMultiplier + ) + val finalHeadingDelta = if (useExternalHeading) + Angle.normDelta(extHeading - lastExtHeading) + else + robotPoseDelta.heading + _poseEstimate = Kinematics.relativeOdometryUpdate( + _poseEstimate, + Pose2d(robotPoseDelta.vec(), finalHeadingDelta) + ) + } + + val wheelVelocities = drive.getWheelVelocities() + val extHeadingVel = drive.getExternalHeadingVelocity() + if (wheelVelocities != null) { + poseVelocity = MecanumKinematics.wheelToRobotVelocities( + wheelVelocities, drive.trackWidth, drive.wheelBase, drive.lateralMultiplier + ) + if (useExternalHeading && extHeadingVel != null) { + poseVelocity = Pose2d(poseVelocity!!.vec(), extHeadingVel) + } + } + + lastWheelPositions = wheelPositions + lastExtHeading = extHeading + } + } + + override var localizer: Localizer = MecanumLocalizer(this) + + override fun setDriveSignal(driveSignal: DriveSignal) { + val velocities = MecanumKinematics.robotToWheelVelocities( + driveSignal.vel, trackWidth, wheelBase, lateralMultiplier) + val accelerations = MecanumKinematics.robotToWheelAccelerations( + driveSignal.accel, trackWidth, wheelBase, lateralMultiplier) + val powers = Kinematics.calculateMotorFeedforward(velocities, accelerations, kV, kA, kStatic) + setMotorPowers(powers[0], powers[1], powers[2], powers[3]) + } + + override fun setDrivePower(drivePower: Pose2d) { + val powers = MecanumKinematics.robotToWheelVelocities( + drivePower, 1.0, 1.0, lateralMultiplier) + setMotorPowers(powers[0], powers[1], powers[2], powers[3]) + } + + /** + * Sets the following motor powers (normalized voltages). All arguments are on the interval `[-1.0, 1.0]`. + */ + abstract fun setMotorPowers(frontLeft: Double, rearLeft: Double, rearRight: Double, frontRight: Double) + + /** + * Returns the positions of the wheels in linear distance units. Positions should exactly match the ordering in + * [setMotorPowers]. + */ + abstract fun getWheelPositions(): List + + /** + * Returns the velocities of the wheels in linear distance units. Positions should exactly match the ordering in + * [setMotorPowers]. + */ + open fun getWheelVelocities(): List? = null +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Util/MecanumKinematics.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Util/MecanumKinematics.kt new file mode 100644 index 0000000..80c0d90 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Util/MecanumKinematics.kt @@ -0,0 +1,95 @@ +package org.firstinspires.ftc.teamcode.API.Util + +import com.acmerobotics.roadrunner.geometry.Pose2d +import com.qualcomm.robotcore.util.Range + +/** + * Mecanum drive kinematic equations. All wheel positions and velocities are given starting with front left and + * proceeding counter-clockwise (i.e., front left, rear left, rear right, front right). Robot poses are specified in a + * coordinate system with positive x pointing forward, positive y pointing left, and positive heading measured + * counter-clockwise from the x-axis. + * + * [This paper](http://www.chiefdelphi.com/media/papers/download/2722) provides a motivated derivation. + */ +object MecanumKinematics { + + /** + * Computes the wheel velocities corresponding to [robotVel] given the provided [trackWidth] and + * [wheelBase]. + * + * @param robotVel velocity of the robot in its reference frame + * @param trackWidth lateral distance between pairs of wheels on different sides of the robot + * @param wheelBase distance between pairs of wheels on the same side of the robot + * @param lateralMultiplier multiplicative gain to adjust for systematic, proportional lateral error (gain greater + * than 1.0 corresponds to overcompensation). + */ + @JvmStatic + @JvmOverloads + fun robotToWheelVelocities( + robotVel: Pose2d, + trackWidth: Double, + wheelBase: Double = trackWidth, + lateralMultiplier: Double = 1.0 + ): List { + val k = (trackWidth + wheelBase) / 2.0 + return listOf( + Range.clip(( robotVel.x - lateralMultiplier * robotVel.y + k * robotVel.heading), -1.0, 1.0), // fl + Range.clip((-robotVel.x - lateralMultiplier * robotVel.y + k * robotVel.heading), -1.0, 1.0), // bl + Range.clip(( robotVel.x - lateralMultiplier * robotVel.y - k * robotVel.heading), -1.0, 1.0), // br + Range.clip((-robotVel.x - lateralMultiplier * robotVel.y - k * robotVel.heading), -1.0, 1.0) // fr + ) + } + + /** + * Computes the wheel accelerations corresponding to [robotAccel] given the provided [trackWidth] and + * [wheelBase]. + * + * @param robotAccel acceleration of the robot in its reference frame + * @param trackWidth lateral distance between pairs of wheels on different sides of the robot + * @param wheelBase distance between pairs of wheels on the same side of the robot + * @param lateralMultiplier multiplicative gain to adjust for systematic, proportional lateral error (gain greater + * than 1.0 corresponds to overcompensation). + */ + @JvmStatic + @JvmOverloads + // follows from linearity of the derivative + fun robotToWheelAccelerations( + robotAccel: Pose2d, + trackWidth: Double, + wheelBase: Double = trackWidth, + lateralMultiplier: Double = 1.0 + ) = + robotToWheelVelocities( + robotAccel, + trackWidth, + wheelBase, + lateralMultiplier + ) + + /** + * Computes the robot velocity corresponding to [wheelVelocities] and the given drive parameters. + * + * @param wheelVelocities wheel velocities (or wheel position deltas) + * @param trackWidth lateral distance between pairs of wheels on different sides of the robot + * @param wheelBase distance between pairs of wheels on the same side of the robot + * @param lateralMultiplier multiplicative gain to adjust for systematic, proportional lateral error (gain greater + * than 1.0 corresponds to overcompensation). + */ + @JvmStatic + @JvmOverloads + fun wheelToRobotVelocities( + wheelVelocities: List, + trackWidth: Double, + wheelBase: Double = trackWidth, + lateralMultiplier: Double = 1.0 + ): Pose2d { + val k = (trackWidth + wheelBase) / 2.0 + val (frontLeft, rearLeft, rearRight, frontRight) = wheelVelocities + return Pose2d( + wheelVelocities.sum(), + (rearLeft + frontRight - frontLeft - rearRight) / lateralMultiplier, + (rearRight + frontRight - frontLeft - rearLeft) / k + ) * 0.25 + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/BackAndForth.java index ead3520..36ea1e5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/BackAndForth.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/BackAndForth.java @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.API.InitRobot; import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; /* @@ -32,6 +33,7 @@ public class BackAndForth extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { + InitRobot.init(this); SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d()) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StraightTest.java index 303ed6b..2a51e9b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StraightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StraightTest.java @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.API.InitRobot; import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; /* @@ -18,6 +19,7 @@ public class StraightTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { + InitRobot.init(this); SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackWidthTuner.java index be5686d..ae82812 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackWidthTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackWidthTuner.java @@ -11,6 +11,7 @@ import org.firstinspires.ftc.robotcore.internal.system.Misc; import org.firstinspires.ftc.teamcode.API.Config.Constants; +import org.firstinspires.ftc.teamcode.API.InitRobot; import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; /* @@ -33,6 +34,8 @@ public class TrackWidthTuner extends LinearOpMode { public void runOpMode() throws InterruptedException { telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + InitRobot.init(this); + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); // TODO: if you haven't already, set the localizer to something that doesn't depend on // drive encoders for computing the heading diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java index 2244cc2..10a196f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java @@ -7,10 +7,14 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.RobotLog; +import org.firstinspires.ftc.teamcode.API.Config.Naming; import org.firstinspires.ftc.teamcode.API.InitRobot; +import org.firstinspires.ftc.teamcode.API.Movement; import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.API.StandardTrackingWheelLocalizer; +import java.util.Objects; + /** * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel @@ -112,6 +116,13 @@ public void runOpMode() throws InterruptedException { telemetry.clearAll(); telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator)); telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading)); + telemetry.addData("Heading", vel.getHeading()); + telemetry.addData("X", vel.getX()); + telemetry.addData("Y", vel.getY()); + telemetry.addData("Back Left", Objects.requireNonNull(Movement.motors.get(Naming.MOTOR_BL_NAME)).getVelocity()); + telemetry.addData("Back Right", Objects.requireNonNull(Movement.motors.get(Naming.MOTOR_BR_NAME)).getVelocity()); + telemetry.addData("Front Left", Objects.requireNonNull(Movement.motors.get(Naming.MOTOR_FL_NAME)).getVelocity()); + telemetry.addData("Front right", Objects.requireNonNull(Movement.motors.get(Naming.MOTOR_FR_NAME)).getVelocity()); telemetry.addLine(); telemetry.addLine("Press Y/△ to conclude routine"); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java index e75bb39..492ce32 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java @@ -24,6 +24,6 @@ public void runOpMode() throws InterruptedException { if (isStopRequested()) return; - drive.turn(Math.toRadians(ANGLE)); + drive.turn(ANGLE); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/Forward.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/Forward.java new file mode 100644 index 0000000..331c3ec --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/Forward.java @@ -0,0 +1,52 @@ +/* +Copyright 2020 FIRST Tech Challenge Team 11792 +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and +associated documentation files (the "Software"), to deal in the Software without restriction, +including without limitation the rights to use, copy, modify, merge, publish, distribute, +sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in all copies or substantial +portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT +NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +package org.firstinspires.ftc.teamcode.OpMode.TeleOp; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.API.InitRobot; +import org.firstinspires.ftc.teamcode.API.Robot; + +@TeleOp(name = "Forward!", group = "Linear OpMode") +public class Forward extends LinearOpMode { + private static final double MAX_SPEED = 0.8; + @Override + public void runOpMode() { + InitRobot.init(this); + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + telemetry.addData(">", "Press start"); + telemetry.update(); + + waitForStart(); + + telemetry.addData(">", "Press stop"); + telemetry.update(); + + waitForStart(); + while(opModeIsActive()) { + if (gamepad1.x) { + Robot.movement.move4x4(1, 1, 1, 1); + } else { + Robot.movement.move4x4(0,0,0,0); + } + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java index 607302f..91d08c5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java @@ -43,14 +43,14 @@ public void runOpMode() { waitForStart(); while(opModeIsActive()) { - double x1 = -gamepad1.left_stick_x; + double x1 = gamepad1.left_stick_x; double y1 = gamepad1.left_stick_y; - double rotation = -gamepad1.right_stick_x; + double rotation = gamepad1.right_stick_x; - double flPower = Range.clip((y1 + x1 + rotation), -MAX_SPEED, MAX_SPEED); - double frPower = Range.clip((y1 - x1 - rotation), -MAX_SPEED, MAX_SPEED); - double blPower = Range.clip((y1 - x1 + rotation), -MAX_SPEED, MAX_SPEED); - double brPower = Range.clip((y1 + x1 - rotation), -MAX_SPEED, MAX_SPEED); + double flPower = Range.clip(( x1 - y1 + rotation), -MAX_SPEED, MAX_SPEED); + double blPower = Range.clip((-x1 - y1 + rotation), -MAX_SPEED, MAX_SPEED); + double brPower = Range.clip(( x1 - y1 - rotation), -MAX_SPEED, MAX_SPEED); + double frPower = Range.clip((-x1 - y1 - rotation), -MAX_SPEED, MAX_SPEED); Robot.movement.move4x4(flPower, frPower, blPower, brPower); diff --git a/build.gradle b/build.gradle index 6057daa..8902ca4 100644 --- a/build.gradle +++ b/build.gradle @@ -5,8 +5,8 @@ */ buildscript { repositories { - google() jcenter() + google() } dependencies { classpath 'com.android.tools.build:gradle:4.1.1' @@ -17,7 +17,7 @@ buildscript { // google() repository beginning with version 3.2 of the Android Gradle Plugin allprojects { repositories { - google() jcenter() + google() } } From 00def5779ccae3c857e387e8871f4f90822cefd1 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Wed, 16 Dec 2020 20:06:15 -0500 Subject: [PATCH 10/29] Add white line autonomous --- .../ftc/teamcode/API/Sensor.java | 28 +++++++++----- .../teamcode/OpMode/Autonomous/WhiteLine.java | 38 +++++++++++++++++++ 2 files changed, 57 insertions(+), 9 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java index 7299db4..153c45f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java @@ -147,36 +147,46 @@ public class Sensor { public static HashMap webcams; // Initialize webcams public static HashMap encoders; // Special encoders + public enum Colors { + RED, + GREEN, + BLUE, + WHITE, + GRAY + } + /** * Gets the RGB value of the color sensor - * @return 0 if red, 1 if green, 2 if blue, 3 if none + * @return enum for color */ - public int getRGB(String id, double redThresh, double greenThresh, double blueThresh) { + public Colors getRGB(String id, double redThresh, double greenThresh, double blueThresh) { double red = getRed(id); double green = getGreen(id); double blue = getBlue(id); if (((Math.abs(red-green)/red) < THRESH_LIMIT) || ((Math.abs(red-green)/green) < THRESH_LIMIT) || ((Math.abs(red-blue)/red) < THRESH_LIMIT) || ((Math.abs(red-blue)/blue) < THRESH_LIMIT) || ((Math.abs(green-blue)/green) < THRESH_LIMIT) || ((Math.abs(green-blue)/blue) < THRESH_LIMIT)) { - return 3; + return Colors.GRAY; } if ((red>blue) && (red>green) && (red>redThresh)) { - return 0; + return Colors.RED; } else if ((green>red) && (green>blue) && (green>greenThresh)) { - return 1; + return Colors.GREEN; } else if ((blue>red) && (blue>green) && (blue>blueThresh)) { - return 2; + return Colors.BLUE; + } else if ((red>redThresh) && (green>greenThresh) && (blue>blueThresh)) { + return Colors.WHITE; } else { - return 3; + return Colors.GRAY; } } /** * Gets the RGB value of the color sensor - * @return 0 if red, 1 if green, 2 if blue, 3 if none + * @return enum for color */ - public int getRGB(String id) { + public Colors getRGB(String id) { return getRGB(id, RED_THRESH, GREEN_THRESH, BLUE_THRESH); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java new file mode 100644 index 0000000..26a28e4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.OpMode.Autonomous; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.API.InitRobot; +import org.firstinspires.ftc.teamcode.API.Robot; +import org.firstinspires.ftc.teamcode.API.Sensor; + +/* + * This is a simple program to reach the white line + */ +@Config +@Autonomous(group = "drive") +public class WhiteLine extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + InitRobot.init(this); + + telemetry.addLine(); + telemetry.addData(">", "Press start"); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + telemetry.addData(">", "Press stop"); + telemetry.update(); + if (Robot.sensor.getRGB("line", 1000, 1000, 1000) == Sensor.Colors.WHITE) { + Robot.movement.move4x4(0.6,0.6,0.6,0.6); + } else { + Robot.movement.move4x4(0,0,0,0); + requestOpModeStop(); + } + } + } +} \ No newline at end of file From 5cf83849b42233a900b1cda77a7cc65df99020b2 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Wed, 16 Dec 2020 20:22:00 -0500 Subject: [PATCH 11/29] Update whiteline halting, add localizer to mecanum --- .../firstinspires/ftc/teamcode/API/SampleMecanumDrive.java | 2 +- .../ftc/teamcode/OpMode/Autonomous/WhiteLine.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java index a894f9e..c05fd60 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java @@ -160,7 +160,7 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { // TODO: if desired, use setLocalizer() to change the localization method // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...)); - //setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap)); + setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap)); } public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java index 26a28e4..1c51362 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java @@ -12,7 +12,7 @@ * This is a simple program to reach the white line */ @Config -@Autonomous(group = "drive") +@Autonomous(name="White line", group = "drive") public class WhiteLine extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { @@ -24,7 +24,7 @@ public void runOpMode() throws InterruptedException { waitForStart(); - while (opModeIsActive()) { + while (!isStopRequested() && opModeIsActive()) { telemetry.addData(">", "Press stop"); telemetry.update(); if (Robot.sensor.getRGB("line", 1000, 1000, 1000) == Sensor.Colors.WHITE) { From 29de135b047f796be12f7b736b8f9ddca7911cca Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sat, 19 Dec 2020 21:42:04 -0500 Subject: [PATCH 12/29] Overhaul color sensor mechanism, fix whiteline auto --- .../ftc/teamcode/API/InitRobot.java | 7 +- .../ftc/teamcode/API/Sensor.java | 129 +++++++++++++----- .../teamcode/OpMode/Autonomous/WhiteLine.java | 12 +- .../OpMode/TeleOp/ColorSensorTelemetry.java | 53 +++++++ 4 files changed, 157 insertions(+), 44 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ColorSensorTelemetry.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index 38f90d8..b1ef85f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.teamcode.API.Config.Naming; @@ -100,13 +101,13 @@ public static void init(LinearOpMode l) { // Get color sensors //ColorSensor scissorDownLimit = l.hardwareMap.get(ColorSensor.class, Naming.COLOR_SENSOR_DOWN_LIMIT_NAME); //ColorSensor scissorUpLimit = l.hardwareMap.get(ColorSensor.class, Naming.COLOR_SENSOR_UP_LIMIT_NAME); - //ColorSensor parkSensor = l.hardwareMap.get(ColorSensor.class, Naming.COLOR_SENSOR_PARK); + NormalizedColorSensor parkSensor = (NormalizedColorSensor)l.hardwareMap.get(ColorSensor.class, Naming.COLOR_SENSOR_PARK); // Add color sensors into list - HashMap colorSensors = new HashMap<>(); + HashMap colorSensors = new HashMap<>(); //colorSensors.put(Naming.COLOR_SENSOR_DOWN_LIMIT_NAME, scissorDownLimit); //colorSensors.put(Naming.COLOR_SENSOR_UP_LIMIT_NAME, scissorUpLimit); - //colorSensors.put(Naming.COLOR_SENSOR_PARK, parkSensor); + colorSensors.put(Naming.COLOR_SENSOR_PARK, parkSensor); // Get webcams //WebcamName webcam1 = l.hardwareMap.get(WebcamName.class, Naming.WEBCAM_0_NAME); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java index 153c45f..47c83df 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java @@ -16,12 +16,15 @@ package org.firstinspires.ftc.teamcode.API; +import android.graphics.Color; + import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator; import com.qualcomm.robotcore.hardware.AnalogInput; -import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.DistanceSensor; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.util.Range; import com.qualcomm.robotcore.util.ReadWriteFile; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; @@ -66,9 +69,15 @@ public class Sensor { private static final float phoneZRotate = 9.5f; // Color sensor configuration - private static final double RED_THRESH = 500; - private static final double GREEN_THRESH = 700; - private static final double BLUE_THRESH = 600; + // TODO: implement + private static final double BLACK_THRESH = 0.2; + private static final double WHITE_THRESH = 0.9; + private static final double RED_THRESH = 600; + private static final double ORANGE_THRESH = 600; + private static final double YELLOW_THRESH = 600; + private static final double GREEN_THRESH = 600; + private static final double BLUE_THRESH = 600; + private static final double PURPLE_THRESH = 600; private static final double THRESH_LIMIT = 0.15; @@ -142,52 +151,65 @@ public class Sensor { public static HashMap gyros; // Initialize gyroscopes public static HashMap pots; // Initialize potentiometers public static HashMap buttons; // Initialize buttons - public static HashMap colorSensors; // Initialize color sensors + public static HashMap colorSensors; // Initialize color sensors public static HashMap distances; // Initialize distance sensors public static HashMap webcams; // Initialize webcams public static HashMap encoders; // Special encoders public enum Colors { RED, + ORANGE, + YELLOW, GREEN, BLUE, + PURPLE, WHITE, - GRAY + GRAY, + BLACK, + BROWN } /** * Gets the RGB value of the color sensor * @return enum for color */ - public Colors getRGB(String id, double redThresh, double greenThresh, double blueThresh) { - double red = getRed(id); - double green = getGreen(id); - double blue = getBlue(id); - - if (((Math.abs(red-green)/red) < THRESH_LIMIT) || ((Math.abs(red-green)/green) < THRESH_LIMIT) || ((Math.abs(red-blue)/red) < THRESH_LIMIT) || - ((Math.abs(red-blue)/blue) < THRESH_LIMIT) || ((Math.abs(green-blue)/green) < THRESH_LIMIT) || ((Math.abs(green-blue)/blue) < THRESH_LIMIT)) { - return Colors.GRAY; + public Colors getRGB(String id, double redFudge, double greenFudge, double blueFudge) { + float[] hsv = new float[3]; + Color.RGBToHSV( + (int) Range.clip(getRed(id) * 255 * redFudge, 0, 255), + (int) Range.clip(getGreen(id) * 255 * greenFudge, 0, 255), + (int) Range.clip(getBlue(id) * 255 * blueFudge, 0, 255), + hsv + ); + + if (hsv[1] < 0.2) { + // Greyscale (inner core in HSV cylinder) + if (hsv[2] > WHITE_THRESH) { + return Colors.WHITE; + } else if (hsv[2] < BLACK_THRESH) { + return Colors.BLACK; + } } - if ((red>blue) && (red>green) && (red>redThresh)) { - return Colors.RED; - } else if ((green>red) && (green>blue) && (green>greenThresh)) { - return Colors.GREEN; - } else if ((blue>red) && (blue>green) && (blue>blueThresh)) { - return Colors.BLUE; - } else if ((red>redThresh) && (green>greenThresh) && (blue>blueThresh)) { - return Colors.WHITE; + // If the value is too low, black + if (hsv[2] < BLACK_THRESH) { + return Colors.BLACK; } else { - return Colors.GRAY; + if ((hsv[0] > 320) || (hsv[0] <= 20)) { + return Colors.RED; + } else if ((hsv[0] > 20) && (hsv[0] <= 46)) { + return Colors.ORANGE; + } else if ((hsv[0] > 46) && (hsv[0] <= 64)) { + return Colors.YELLOW; + } else if ((hsv[0] > 146) && (hsv[0] <= 160)) { + return Colors.GREEN; + } else if ((hsv[0] > 160) && (hsv[0] <= 248)) { + return Colors.BLUE; + } else if ((hsv[0] > 248) && (hsv[0] <= 320)) { + return Colors.PURPLE; + } } - } - - /** - * Gets the RGB value of the color sensor - * @return enum for color - */ - public Colors getRGB(String id) { - return getRGB(id, RED_THRESH, GREEN_THRESH, BLUE_THRESH); + return Colors.GRAY; } public static BNO055IMU getGyro(String id) { @@ -199,8 +221,8 @@ public static BNO055IMU getGyro(String id) { * @param id ID of the color sensor * @return Boolean on if the sensor detects red or not */ - public int getRed(String id) { - return Objects.requireNonNull(colorSensors.get(id)).red(); + public double getRed(String id) { + return Objects.requireNonNull(colorSensors.get(id)).getNormalizedColors().red; } /** @@ -208,8 +230,8 @@ public int getRed(String id) { * @param id ID of the color sensor * @return Boolean on if the sensor detects green or not */ - public int getGreen(String id) { - return Objects.requireNonNull(colorSensors.get(id)).green(); + public double getGreen(String id) { + return Objects.requireNonNull(colorSensors.get(id)).getNormalizedColors().green; } /** @@ -217,8 +239,8 @@ public int getGreen(String id) { * @param id ID of the color sensor * @return Boolean on if the sensor detects blue or not */ - public int getBlue(String id) { - return Objects.requireNonNull(colorSensors.get(id)).blue(); + public double getBlue(String id) { + return Objects.requireNonNull(colorSensors.get(id)).getNormalizedColors().blue; } /** @@ -275,5 +297,38 @@ public void calibrateGyro(String id) { File file = AppUtil.getInstance().getSettingsFile(filename); ReadWriteFile.writeFile(file, calibrationData.serialize()); } + + public static double[] RGBtoHSV(double[] rgb) { + double[] hsv = new double[] {0, 0, 0}; + double max = Math.max(Math.max(rgb[0], rgb[1]), rgb[2]); + double min = Math.min(Math.min(rgb[0], rgb[1]), rgb[2]); + double delta = max - min; + + if (delta == 0) { + hsv[0] = 360; + hsv[1] = 0; + hsv[2] = max; + return hsv; + } + + if (max == rgb[0]) { + hsv[0] = (rgb[1] - rgb[2]) / delta % 6; + } else if (max == rgb[1]) { + hsv[0] = (rgb[2] - rgb[0]) / delta + 2; + } else { + hsv[0] = (rgb[0] - rgb[1]) / delta + 4; + } + hsv[0] *= 60; + + if (max == 0) { + hsv[1] = 0; + } else { + hsv[1] = delta / max; + } + + hsv[2] = max; + + return hsv; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java index 1c51362..8742bcb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.API.Config.Naming; import org.firstinspires.ftc.teamcode.API.InitRobot; import org.firstinspires.ftc.teamcode.API.Robot; import org.firstinspires.ftc.teamcode.API.Sensor; @@ -12,8 +13,11 @@ * This is a simple program to reach the white line */ @Config -@Autonomous(name="White line", group = "drive") +@Autonomous(name="White Line", group = "drive") public class WhiteLine extends LinearOpMode { + private static final double REDFUDGE = 25*30; + private static final double GREENFUDGE = 15*30; + private static final double BLUEFUDGE = 15*30; @Override public void runOpMode() throws InterruptedException { InitRobot.init(this); @@ -27,11 +31,11 @@ public void runOpMode() throws InterruptedException { while (!isStopRequested() && opModeIsActive()) { telemetry.addData(">", "Press stop"); telemetry.update(); - if (Robot.sensor.getRGB("line", 1000, 1000, 1000) == Sensor.Colors.WHITE) { - Robot.movement.move4x4(0.6,0.6,0.6,0.6); - } else { + if (Robot.sensor.getRGB(Naming.COLOR_SENSOR_PARK, REDFUDGE, GREENFUDGE, BLUEFUDGE) == Sensor.Colors.WHITE) { Robot.movement.move4x4(0,0,0,0); requestOpModeStop(); + } else { + Robot.movement.move4x4(0.3,0.3,0.3,0.3); // Only power back wheels because one of the front wheels isn't working } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ColorSensorTelemetry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ColorSensorTelemetry.java new file mode 100644 index 0000000..4570277 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ColorSensorTelemetry.java @@ -0,0 +1,53 @@ +package org.firstinspires.ftc.teamcode.OpMode.TeleOp; + +import android.graphics.Color; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.teamcode.API.Config.Naming; +import org.firstinspires.ftc.teamcode.API.InitRobot; +import org.firstinspires.ftc.teamcode.API.Robot; + +/* + * This is a simple program to reach the white line + */ +@Config +@TeleOp(name="Color Sensor Telemetry") +public class ColorSensorTelemetry extends LinearOpMode { + private static final double REDFUDGE = 25*30; + private static final double GREENFUDGE = 15*30; + private static final double BLUEFUDGE = 15*30; + @Override + public void runOpMode() throws InterruptedException { + InitRobot.init(this); + + telemetry.addLine(); + telemetry.addData(">", "Press start"); + telemetry.update(); + + waitForStart(); + + while (!isStopRequested() && opModeIsActive()) { + telemetry.addData(">", "Press stop"); + int red = (int) Range.clip(Robot.sensor.getRed(Naming.COLOR_SENSOR_PARK) * 255 * REDFUDGE, 0, 255); + int green = (int) Range.clip(Robot.sensor.getGreen(Naming.COLOR_SENSOR_PARK) * 255 * GREENFUDGE, 0, 255); + int blue = (int) Range.clip(Robot.sensor.getBlue(Naming.COLOR_SENSOR_PARK) * 255 * BLUEFUDGE, 0, 255); + + float[] hsv = new float[3]; + Color.RGBToHSV(red, green, blue, hsv); + + telemetry.addData("Red", red); + telemetry.addData("Green", green); + telemetry.addData("Blue", blue); + telemetry.addData("Hue", hsv[0]); + telemetry.addData("Saturation", hsv[1]); + telemetry.addData("Value", hsv[2]); + telemetry.addData("Detected", Robot.sensor.getRGB(Naming.COLOR_SENSOR_PARK, REDFUDGE, GREENFUDGE, BLUEFUDGE)); + + telemetry.update(); + } + } +} From 875283e0f92c513c9370e58a11a61d051ab104a5 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sun, 10 Jan 2021 15:13:52 -0500 Subject: [PATCH 13/29] Update whiteline code --- .../firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java index 8742bcb..96b2b81 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java @@ -35,7 +35,7 @@ public void runOpMode() throws InterruptedException { Robot.movement.move4x4(0,0,0,0); requestOpModeStop(); } else { - Robot.movement.move4x4(0.3,0.3,0.3,0.3); // Only power back wheels because one of the front wheels isn't working + Robot.movement.move4x4(0.3,0.3,0.3,0.3); } } } From 2dd21bfff8417f34c3fd3f24493ba18847bf3c62 Mon Sep 17 00:00:00 2001 From: VarunDamarla Date: Fri, 22 Jan 2021 19:03:40 -0500 Subject: [PATCH 14/29] Added flywheel and intake --- TeamCode/build.gradle | 2 +- .../ftc/teamcode/API/Config/Naming.java | 3 ++ .../ftc/teamcode/API/InitRobot.java | 6 ++- .../ftc/teamcode/API/Movement.java | 46 ++++--------------- .../teamcode/OpMode/TeleOp/TeleOpMain.java | 2 + build.gradle | 2 +- 6 files changed, 20 insertions(+), 41 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 498bf05..b3df367 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -32,7 +32,7 @@ buildscript { google() } dependencies { - classpath 'com.android.tools.build:gradle:3.0.0' + classpath 'com.android.tools.build:gradle:4.1.2' classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlinVersion" } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java index fd04a87..7c00d9f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java @@ -40,5 +40,8 @@ public class Naming { public static final String ENCODER_LATERAL = "lateralEncoder"; public static final String ENCODER_LEFT = "leftEncoder"; public static final String ENCODER_RIGHT = "rightEncoder"; + + public static final String MOTOR_FLYWHEEL = "flywheel"; + public static final String MOTOR_INTAKE = "intake"; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index b1ef85f..ab7a9f1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -23,7 +23,7 @@ */ public class InitRobot { public static final boolean MODE_4x4 = true; // True if you are using 4x4 drive - private static SmartMotor bl, br, fl, fr; + private static SmartMotor bl, br, fl, fr, flywheel, intake; private static Encoder leftEncoder, rightEncoder, lateralEncoder; // TODO: JavaDoc @@ -45,6 +45,8 @@ public static void init(LinearOpMode l) { fl = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FL_NAME)); fr = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FR_NAME)); } + flywheel = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FLYWHEEL)); + intake = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); HashMap motors = new HashMap<>(); motors.put(Naming.MOTOR_BL_NAME, bl); @@ -53,6 +55,8 @@ public static void init(LinearOpMode l) { motors.put(Naming.MOTOR_FL_NAME, fl); motors.put(Naming.MOTOR_FR_NAME, fr); } + motors.put(Naming.MOTOR_FLYWHEEL, flywheel); + motors.put(Naming.MOTOR_INTAKE, intake); // Get servos //SmartServo grabber = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_GRABBER_NAME)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java index 6edbdc7..dab2515 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java @@ -147,48 +147,18 @@ public void setServoSpeed(String id, double power) { } /** - * Opens the grabber based on a boolean assignment - * @param command true to open the grabber or false to close the grabber + * Set the movement of the flywheel + * @param wheelPower Power sent to flywheel */ - public void openGrabber(boolean command) { - SmartServo sg; // sg: Servo grabber - sg = servos.get(Naming.SERVO_GRABBER_NAME); - assert sg != null; - if (command) { - sg.setPosition(GRABBER_OPEN); // Opens the grabber - } else { - sg.setPosition(GRABBER_CLOSE); // Closes the grabber - } + public void moveFlywheel(double wheelPower) { + Objects.requireNonNull(motors.get(Naming.MOTOR_FLYWHEEL)).setPower(wheelPower); } /** - * Opens the swivel based on a boolean assignment - * @param command true to open the swivel or false to close the swivel + * Set the movement of the intake + * @param intakePower Power sent to intake */ - public void openSwivel(boolean command) { - SmartServo ss; // ss: Servo Swivel - ss = servos.get(Naming.SERVO_ROTATE_NAME); - assert ss != null; - if (command) { - ss.setPosition(SWIVEL_OPEN); // Opens the swivel - } else { - ss.setPosition(SWIVEL_CLOSE); // Closes the swivel - } - } - - public void grabFoundation(boolean command) { - SmartServo slfn, srfn; - slfn = servos.get(Naming.SERVO_FOUNDATION_LEFT_NEW_NAME); // sfln: Servo Left Foundation New - srfn = servos.get(Naming.SERVO_FOUNDATION_RIGHT_NEW_NAME); // sfrn: Servo Right Foundation New - assert slfn != null; - if (command) { // Grabs foundation - slfn.setPosition(FOUNDATION_CLOSE); - assert srfn != null; - srfn.setPosition(FOUNDATION_CLOSE); - } else { // Releases foundation - slfn.setPosition(FOUNDATION_OPEN); - assert srfn != null; - srfn.setPosition(FOUNDATION_OPEN); - } + public void moveIntake(double intakePower) { + Objects.requireNonNull(motors.get(Naming.MOTOR_INTAKE)).setPower(intakePower); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java index 91d08c5..9f56627 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java @@ -53,6 +53,8 @@ public void runOpMode() { double frPower = Range.clip((-x1 - y1 - rotation), -MAX_SPEED, MAX_SPEED); Robot.movement.move4x4(flPower, frPower, blPower, brPower); + Robot.movement.moveFlywheel(gamepad2.left_trigger); + Robot.movement.moveIntake(gamepad2.left_trigger); telemetry.addData("Back Left", blPower); telemetry.addData("Back Right", brPower); diff --git a/build.gradle b/build.gradle index 8902ca4..6c1bde0 100644 --- a/build.gradle +++ b/build.gradle @@ -9,7 +9,7 @@ buildscript { google() } dependencies { - classpath 'com.android.tools.build:gradle:4.1.1' + classpath 'com.android.tools.build:gradle:4.1.2' } } From 36d8d825f2d12e9caba90dff6761e552592bf338 Mon Sep 17 00:00:00 2001 From: Varun Damarla <57463486+damarlavarun2004@users.noreply.github.com> Date: Fri, 22 Jan 2021 20:53:26 -0500 Subject: [PATCH 15/29] Added wobbleGrabber servo, wobbleGrabber function, and ZeroServo. --- .../ftc/teamcode/API/Config/Naming.java | 1 + .../ftc/teamcode/API/InitRobot.java | 3 ++ .../ftc/teamcode/API/Movement.java | 28 ++++++++--- .../teamcode/OpMode/TeleOp/TeleOpMain.java | 7 +++ .../ftc/teamcode/OpMode/TeleOp/ZeroServo.java | 49 +++++++++++++++++++ 5 files changed, 81 insertions(+), 7 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java index 7c00d9f..60c110a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java @@ -16,6 +16,7 @@ public class Naming { public static final String SERVO_FOUNDATION_LEFT_NAME = "foundationLeft"; public static final String SERVO_FOUNDATION_RIGHT_NAME = "foundationRight"; public static final String SERVO_ROTATE_NAME = "rotate"; + public static final String SERVO_WOBBLE_GRABBER_NAME = "wobbleGrabber"; // Only for temporary purposes public static final String SERVO_FOUNDATION_LEFT_NEW_NAME = "testFoundationLeft"; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index ab7a9f1..ad2585e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -7,6 +7,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.teamcode.API.Config.Naming; @@ -59,6 +60,7 @@ public static void init(LinearOpMode l) { motors.put(Naming.MOTOR_INTAKE, intake); // Get servos + SmartServo wobble = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_GRABBER_NAME)); //SmartServo grabber = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_GRABBER_NAME)); //SmartServo rotate = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_ROTATE_NAME)); //SmartServo fLeftNew = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_FOUNDATION_LEFT_NEW_NAME)); @@ -69,6 +71,7 @@ public static void init(LinearOpMode l) { // Add servos into the list HashMap servos = new HashMap<>(); + servos.put(Naming.SERVO_WOBBLE_GRABBER_NAME, wobble); //servos.put(Naming.SERVO_GRABBER_NAME, grabber); //servos.put(Naming.SERVO_ROTATE_NAME, rotate); //servos.put(Naming.SERVO_FOUNDATION_LEFT_NEW_NAME, fLeftNew); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java index dab2515..7767dfb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java @@ -41,11 +41,13 @@ public class Movement { // Servo configuration private final static int SERVO_SLEEP = 10; // Milliseconds - private final static double SERVO_STEP = 0.01; // Degrees - private final static double GRABBER_OPEN = 0; // Degrees - private final static double GRABBER_CLOSE = 0.65; // Degrees - private final static double SWIVEL_OPEN = 0; // Degrees - private final static double SWIVEL_CLOSE = 1; // Degrees + private final static double SERVO_STEP = 0.01; // on a scale of 0-1 + private final static double GRABBER_OPEN = 0; // on a scale of 0-1 + private final static double GRABBER_CLOSE = 0.65; // on a scale of 0-1 + private final static double WOBBLE_IN = 1; // on a scale of 0-1 + private final static double WOBBLE_OUT = 0.3; // on a scale of 0-1 + private final static double SWIVEL_OPEN = 0; // on a scale of 0-1 + private final static double SWIVEL_CLOSE = 1; // on a scale of 0-1 private final static double FOUNDATION_OPEN = 0.3; private final static double FOUNDATION_CLOSE = 0.95; @@ -114,7 +116,7 @@ public void moveElevator(double speed) { /** * Sets the servo to a specific position. Useful if we do not want to slowly scan the servo to a position * @param id ID of the servo - * @param degrees Position (in degrees) to set the servo to. + * @param degrees Position (in on a scale of 0-1) to set the servo to. */ public void setServo(String id, double degrees) { Objects.requireNonNull(servos.get(id)).setPosition(degrees); @@ -123,7 +125,7 @@ public void setServo(String id, double degrees) { /** * Scan the servo (move the servo slowly) to a position. * @param id ID of servo - * @param degrees Position (in degrees) to scan the servo to. + * @param degrees Position (in on a scale of 0-1) to scan the servo to. */ public void scanServo(String id, double degrees, boolean clockwise) { while (Math.abs(Objects.requireNonNull(servos.get(id)).getPosition() - degrees) < 0.001) { @@ -161,4 +163,16 @@ public void moveFlywheel(double wheelPower) { public void moveIntake(double intakePower) { Objects.requireNonNull(motors.get(Naming.MOTOR_INTAKE)).setPower(intakePower); } + + /** + * Opens the grabber based on a boolean assignment + * @param command tue to open + */ + public void moveWobble(boolean command) { + if (command) { + Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_GRABBER_NAME)).setPosition(WOBBLE_IN); // Opens the grabber + } else { + Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_GRABBER_NAME)).setPosition(WOBBLE_OUT); // Closes the grabber + } + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java index 9f56627..cd35e8b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java @@ -56,6 +56,13 @@ public void runOpMode() { Robot.movement.moveFlywheel(gamepad2.left_trigger); Robot.movement.moveIntake(gamepad2.left_trigger); + // Moving the wobble + if (gamepad2.x) { + Robot.movement.moveWobble(true); + } else if (gamepad2.y) { + Robot.movement.moveWobble(false); + } + telemetry.addData("Back Left", blPower); telemetry.addData("Back Right", brPower); telemetry.addData("Front Left", flPower); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java new file mode 100644 index 0000000..a206873 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java @@ -0,0 +1,49 @@ +/* +Copyright 2020 FIRST Tech Challenge Team 11792 +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and +associated documentation files (the "Software"), to deal in the Software without restriction, +including without limitation the rights to use, copy, modify, merge, publish, distribute, +sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in all copies or substantial +portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT +NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +package org.firstinspires.ftc.teamcode.OpMode.TeleOp; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +@TeleOp(name = "Zero Servo", group = "Linear OpMode") +public class ZeroServo extends LinearOpMode { + @Override + public void runOpMode() { + Servo zeroServo = hardwareMap.get(Servo.class, "zeroServo"); + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + telemetry.addData(">", "Press start"); + telemetry.update(); + + waitForStart(); + + telemetry.addData(">", "Press stop"); + telemetry.update(); + + waitForStart(); + while(opModeIsActive()) { + if (gamepad1.x) { + zeroServo.setPosition(0.3); + } else if (gamepad1.y) { + zeroServo.setPosition(1); + } + } + } +} From 54842b89c5d674e5c121ffc63d7e94371594ef75 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sun, 24 Jan 2021 17:57:47 -0500 Subject: [PATCH 16/29] Add grabber arm, launcher. Fix telemetry, color --- .../ftc/teamcode/API/Config/Naming.java | 2 + .../ftc/teamcode/API/InitRobot.java | 10 +++- .../ftc/teamcode/API/Movement.java | 31 ++++++++++-- .../teamcode/OpMode/Autonomous/WhiteLine.java | 21 ++++++-- .../teamcode/OpMode/TeleOp/TeleOpMain.java | 50 ++++++++++++++++--- .../ftc/teamcode/OpMode/TeleOp/ZeroServo.java | 2 +- 6 files changed, 98 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java index 60c110a..cf67f4b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java @@ -17,6 +17,8 @@ public class Naming { public static final String SERVO_FOUNDATION_RIGHT_NAME = "foundationRight"; public static final String SERVO_ROTATE_NAME = "rotate"; public static final String SERVO_WOBBLE_GRABBER_NAME = "wobbleGrabber"; + public static final String SERVO_WOBBLE_ARM_NAME = "wobbleArm"; + public static final String SERVO_LAUNCHER = "launcher"; // Only for temporary purposes public static final String SERVO_FOUNDATION_LEFT_NEW_NAME = "testFoundationLeft"; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index ad2585e..389af21 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -48,6 +48,8 @@ public static void init(LinearOpMode l) { } flywheel = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FLYWHEEL)); intake = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); + flywheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); HashMap motors = new HashMap<>(); motors.put(Naming.MOTOR_BL_NAME, bl); @@ -60,7 +62,9 @@ public static void init(LinearOpMode l) { motors.put(Naming.MOTOR_INTAKE, intake); // Get servos - SmartServo wobble = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_GRABBER_NAME)); + SmartServo wobbleArm = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_ARM_NAME)); + SmartServo wobbleGrabber = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_GRABBER_NAME)); + SmartServo launcher = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_LAUNCHER)); //SmartServo grabber = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_GRABBER_NAME)); //SmartServo rotate = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_ROTATE_NAME)); //SmartServo fLeftNew = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_FOUNDATION_LEFT_NEW_NAME)); @@ -71,7 +75,9 @@ public static void init(LinearOpMode l) { // Add servos into the list HashMap servos = new HashMap<>(); - servos.put(Naming.SERVO_WOBBLE_GRABBER_NAME, wobble); + servos.put(Naming.SERVO_WOBBLE_ARM_NAME, wobbleArm); + servos.put(Naming.SERVO_WOBBLE_GRABBER_NAME, wobbleGrabber); + servos.put(Naming.SERVO_LAUNCHER, launcher); //servos.put(Naming.SERVO_GRABBER_NAME, grabber); //servos.put(Naming.SERVO_ROTATE_NAME, rotate); //servos.put(Naming.SERVO_FOUNDATION_LEFT_NEW_NAME, fLeftNew); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java index 7767dfb..79e0786 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java @@ -44,12 +44,16 @@ public class Movement { private final static double SERVO_STEP = 0.01; // on a scale of 0-1 private final static double GRABBER_OPEN = 0; // on a scale of 0-1 private final static double GRABBER_CLOSE = 0.65; // on a scale of 0-1 - private final static double WOBBLE_IN = 1; // on a scale of 0-1 + private final static double WOBBLE_IN = 0.75; // on a scale of 0-1 private final static double WOBBLE_OUT = 0.3; // on a scale of 0-1 + private final static double WOBBLE_GRAB = 1; + private final static double WOBBLE_RELEASE = 0; private final static double SWIVEL_OPEN = 0; // on a scale of 0-1 private final static double SWIVEL_CLOSE = 1; // on a scale of 0-1 private final static double FOUNDATION_OPEN = 0.3; private final static double FOUNDATION_CLOSE = 0.95; + private final static double LAUNCHER_OPEN = 0.8; + private final static double LAUNCHER_CLOSE = 1; public static HashMap motors; public static HashMap servos; @@ -170,9 +174,30 @@ public void moveIntake(double intakePower) { */ public void moveWobble(boolean command) { if (command) { - Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_GRABBER_NAME)).setPosition(WOBBLE_IN); // Opens the grabber + Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_ARM_NAME)).setPosition(WOBBLE_IN); // Opens the grabber } else { - Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_GRABBER_NAME)).setPosition(WOBBLE_OUT); // Closes the grabber + Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_ARM_NAME)).setPosition(WOBBLE_OUT); // Closes the grabber + } + } + + /** + * Opens the grabber based on a boolean assignment + * @param command tue to open + */ + public void grabWobble(boolean command) { + if (command) { + Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_GRABBER_NAME)).setPosition(WOBBLE_GRAB); // Opens the grabber + } else { + Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_GRABBER_NAME)).setPosition(WOBBLE_RELEASE); // Closes the grabber + } + } + + + public void launch(boolean command) { + if (command) { + Objects.requireNonNull(servos.get(Naming.SERVO_LAUNCHER)).setPosition(LAUNCHER_OPEN); + } else { + Objects.requireNonNull(servos.get(Naming.SERVO_LAUNCHER)).setPosition(LAUNCHER_CLOSE); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java index 96b2b81..6b049e5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java @@ -1,6 +1,8 @@ package org.firstinspires.ftc.teamcode.OpMode.Autonomous; +import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -15,13 +17,16 @@ @Config @Autonomous(name="White Line", group = "drive") public class WhiteLine extends LinearOpMode { - private static final double REDFUDGE = 25*30; - private static final double GREENFUDGE = 15*30; - private static final double BLUEFUDGE = 15*30; + public static double FUDGE = 10; + public static double REDFUDGE = 25*FUDGE; + public static double GREENFUDGE = 15*FUDGE; + public static double BLUEFUDGE = 15*FUDGE; @Override public void runOpMode() throws InterruptedException { InitRobot.init(this); + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetry.addLine(); telemetry.addData(">", "Press start"); telemetry.update(); @@ -30,13 +35,19 @@ public void runOpMode() throws InterruptedException { while (!isStopRequested() && opModeIsActive()) { telemetry.addData(">", "Press stop"); - telemetry.update(); - if (Robot.sensor.getRGB(Naming.COLOR_SENSOR_PARK, REDFUDGE, GREENFUDGE, BLUEFUDGE) == Sensor.Colors.WHITE) { + Sensor.Colors color = Robot.sensor.getRGB(Naming.COLOR_SENSOR_PARK, REDFUDGE, GREENFUDGE, BLUEFUDGE); + if (color == Sensor.Colors.WHITE) { Robot.movement.move4x4(0,0,0,0); requestOpModeStop(); } else { Robot.movement.move4x4(0.3,0.3,0.3,0.3); } + telemetry.addData(">", color); + telemetry.addData("Red", Robot.sensor.getRed(Naming.COLOR_SENSOR_PARK)); + telemetry.addData("Green", Robot.sensor.getRed(Naming.COLOR_SENSOR_PARK)); + telemetry.addData("Blue", Robot.sensor.getRed(Naming.COLOR_SENSOR_PARK)); + telemetry.update(); + } } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java index cd35e8b..510ec01 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java @@ -16,8 +16,11 @@ associated documentation files (the "Software"), to deal in the Software without package org.firstinspires.ftc.teamcode.OpMode.TeleOp; +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.teamcode.API.InitRobot; @@ -25,11 +28,14 @@ associated documentation files (the "Software"), to deal in the Software without @TeleOp(name = "TeleOp Main", group = "Linear OpMode") public class TeleOpMain extends LinearOpMode { - private static final double MAX_SPEED = 0.8; + private double maxspeed = 0.6; + private boolean switch = false; @Override public void runOpMode() { InitRobot.init(this); + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetry.addData("Status", "Initialized"); telemetry.update(); @@ -47,14 +53,35 @@ public void runOpMode() { double y1 = gamepad1.left_stick_y; double rotation = gamepad1.right_stick_x; - double flPower = Range.clip(( x1 - y1 + rotation), -MAX_SPEED, MAX_SPEED); - double blPower = Range.clip((-x1 - y1 + rotation), -MAX_SPEED, MAX_SPEED); - double brPower = Range.clip(( x1 - y1 - rotation), -MAX_SPEED, MAX_SPEED); - double frPower = Range.clip((-x1 - y1 - rotation), -MAX_SPEED, MAX_SPEED); + double flPower = Range.clip(( x1 - y1 + rotation), -maxspeed, maxspeed); + double blPower = Range.clip((-x1 - y1 + rotation), -maxspeed, maxspeed); + double brPower = Range.clip(( x1 - y1 - rotation), -maxspeed, maxspeed); + double frPower = Range.clip((-x1 - y1 - rotation), -maxspeed, maxspeed); Robot.movement.move4x4(flPower, frPower, blPower, brPower); Robot.movement.moveFlywheel(gamepad2.left_trigger); Robot.movement.moveIntake(gamepad2.left_trigger); + + if (gamepad1.left_bumper) { + if (switch) { + Robot.movement.getMotor("fl").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + Robot.movement.getMotor("fr").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + Robot.movement.getMotor("bl").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + Robot.movement.getMotor("br").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } else { + Robot.movement.getMotor("fl").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + Robot.movement.getMotor("fr").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + Robot.movement.getMotor("bl").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + Robot.movement.getMotor("br").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + switch = !switch; + } + + if (gamepad1.right_bumper) { + maxspeed = 0.8; + } else { + maxspeed = 0.6; + } // Moving the wobble if (gamepad2.x) { @@ -62,12 +89,21 @@ public void runOpMode() { } else if (gamepad2.y) { Robot.movement.moveWobble(false); } + + if (gamepad2.a) { + Robot.movement.grabWobble(true); + } else if (gamepad2.b) { + Robot.movement.grabWobble(false); + } + + // Launch async because we don't want the robot to hang while it does stuff + Robot.movement.launch(gamepad2.left_bumper); telemetry.addData("Back Left", blPower); telemetry.addData("Back Right", brPower); telemetry.addData("Front Left", flPower); - telemetry.addData("Front right", frPower); + telemetry.addData("Front Right", frPower); telemetry.update(); } } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java index a206873..95248b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java @@ -40,7 +40,7 @@ public void runOpMode() { waitForStart(); while(opModeIsActive()) { if (gamepad1.x) { - zeroServo.setPosition(0.3); + zeroServo.setPosition(0.8); } else if (gamepad1.y) { zeroServo.setPosition(1); } From 60914f0d024a79d9f738b3b0f5ad1182146aba6b Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sun, 24 Jan 2021 19:03:34 -0500 Subject: [PATCH 17/29] Squash bug --- .../ftc/teamcode/OpMode/TeleOp/TeleOpMain.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java index 510ec01..96febeb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java @@ -29,7 +29,7 @@ associated documentation files (the "Software"), to deal in the Software without @TeleOp(name = "TeleOp Main", group = "Linear OpMode") public class TeleOpMain extends LinearOpMode { private double maxspeed = 0.6; - private boolean switch = false; + private boolean brakeSwitch = false; @Override public void runOpMode() { InitRobot.init(this); @@ -63,7 +63,7 @@ public void runOpMode() { Robot.movement.moveIntake(gamepad2.left_trigger); if (gamepad1.left_bumper) { - if (switch) { + if (brakeSwitch) { Robot.movement.getMotor("fl").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); Robot.movement.getMotor("fr").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); Robot.movement.getMotor("bl").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -74,7 +74,7 @@ public void runOpMode() { Robot.movement.getMotor("bl").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); Robot.movement.getMotor("br").setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); } - switch = !switch; + brakeSwitch = !brakeSwitch; } if (gamepad1.right_bumper) { From 3ae1105c86888e7a0f0d2e2999c33eca3abfdd0b Mon Sep 17 00:00:00 2001 From: Varun Damarla <57463486+damarlavarun2004@users.noreply.github.com> Date: Thu, 4 Feb 2021 20:50:43 -0500 Subject: [PATCH 18/29] Attempt to fix RoadRunner --- .../ftc/teamcode/API/Config/Constants.java | 4 ++-- .../firstinspires/ftc/teamcode/API/InitRobot.java | 13 ++++++++++--- .../ftc/teamcode/API/SampleMecanumDrive.java | 13 ++++++++----- .../org/firstinspires/ftc/teamcode/API/Sensor.java | 2 +- .../API/StandardTrackingWheelLocalizer.java | 4 ++-- .../TrackingWheelLateralDistanceTuner.java | 2 +- 6 files changed, 24 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java index 3d80e5b..7664c1b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java @@ -7,7 +7,7 @@ public class Constants { /* * These are motor constants that should be listed online for your motors. */ - public static final double TICKS_PER_REV = 537.6; + public static final double TICKS_PER_REV = 4096; public static final double MAX_RPM = 340; /* @@ -31,7 +31,7 @@ public class Constants { */ public static double WHEEL_RADIUS = 2; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 2; // in + public static double TRACK_WIDTH = 1.5; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index 389af21..728208b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -47,9 +47,9 @@ public static void init(LinearOpMode l) { fr = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FR_NAME)); } flywheel = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FLYWHEEL)); - intake = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); + //intake = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); flywheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + //intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); HashMap motors = new HashMap<>(); motors.put(Naming.MOTOR_BL_NAME, bl); @@ -59,7 +59,7 @@ public static void init(LinearOpMode l) { motors.put(Naming.MOTOR_FR_NAME, fr); } motors.put(Naming.MOTOR_FLYWHEEL, flywheel); - motors.put(Naming.MOTOR_INTAKE, intake); + //motors.put(Naming.MOTOR_INTAKE, intake); // Get servos SmartServo wobbleArm = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_ARM_NAME)); @@ -108,6 +108,13 @@ public static void init(LinearOpMode l) { fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); } + bl.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + br.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + if (MODE_4x4) { + fl.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + fr.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + // Switch direction of servo //rotate.setDirection(Servo.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java index c05fd60..4d4c0b7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java @@ -28,8 +28,11 @@ import com.qualcomm.robotcore.hardware.VoltageSensor; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; import org.firstinspires.ftc.teamcode.API.Config.Naming; import org.firstinspires.ftc.teamcode.API.HW.SmartMotor; +import org.firstinspires.ftc.teamcode.API.Util.AxesSigns; +import org.firstinspires.ftc.teamcode.API.Util.BNO055IMUUtil; import org.firstinspires.ftc.teamcode.API.Util.DashboardUtil; import org.firstinspires.ftc.teamcode.API.Util.LynxModuleUtil; import org.firstinspires.ftc.teamcode.API.Util.MecanumDrive; @@ -57,11 +60,11 @@ public class SampleMecanumDrive extends MecanumDrive { public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0); public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); - public static double LATERAL_MULTIPLIER = 1; + public static double LATERAL_MULTIPLIER = 4; - public static double VX_WEIGHT = 1; - public static double VY_WEIGHT = 1; - public static double OMEGA_WEIGHT = 1; + public static double VX_WEIGHT = 4; + public static double VY_WEIGHT = 4; + public static double OMEGA_WEIGHT = 4; public static int POSE_HISTORY_LIMIT = 100; @@ -128,7 +131,7 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { // TODO: if your hub is mounted vertically, remap the IMU axes so that the z-axis points // upward (normal to the floor) using a command like the following: - // BNO055IMUUtil.remapAxes(imu, AxesOrder.XYZ, AxesSigns.NPN); + BNO055IMUUtil.remapAxes(imu, AxesOrder.XYZ, AxesSigns.NPN); fl = Robot.movement.getMotor(Naming.MOTOR_FL_NAME); bl = Robot.movement.getMotor(Naming.MOTOR_BL_NAME); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java index 47c83df..2697ba3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Sensor.java @@ -271,7 +271,7 @@ public static Encoder getEncoder(String id) { */ public void initGyro(String id) { BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); - parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; + parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS; parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; parameters.calibrationDataFile = String.format(Locale.ENGLISH, "BNO055IMUCalibration%s.json", id); parameters.loggingEnabled = true; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java index 2aa246b..bba85a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/StandardTrackingWheelLocalizer.java @@ -29,10 +29,10 @@ @Config public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { public static double TICKS_PER_REV = 4096; - public static double WHEEL_RADIUS = 0.66; // in + public static double WHEEL_RADIUS = 2; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - public static double LATERAL_DISTANCE = 11.99; // in; distance between the left and right wheels + public static double LATERAL_DISTANCE = 4; // in; distance between the left and right wheels public static double FORWARD_OFFSET = 8.3; // in; offset of the lateral wheel private static Encoder leftEncoder, rightEncoder, lateralEncoder; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java index 10a196f..d5aedfe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java @@ -42,7 +42,7 @@ * * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns. * - * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators + * 7. Once the bot has finished spinning 10 times, press Y to finishing the routine. The indicators * on the bot and on the ground you created earlier should be lined up. * * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your From a02f7d771e4b960db2e2035d9756eae276c52b55 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sun, 7 Feb 2021 10:34:54 -0500 Subject: [PATCH 19/29] Update naming scheme, clean up code --- .../ftc/teamcode/API/Config/Naming.java | 39 ++++-------- .../ftc/teamcode/API/InitRobot.java | 62 +++++++------------ .../ftc/teamcode/API/Movement.java | 31 +++++----- .../ftc/teamcode/API/SampleMecanumDrive.java | 10 +-- .../TrackingWheelLateralDistanceTuner.java | 8 +-- 5 files changed, 60 insertions(+), 90 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java index cf67f4b..631223c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java @@ -6,39 +6,24 @@ */ public class Naming { // Name configuration - public static final String MOTOR_LIFT_NAME = "scissorLift"; - public static final String MOTOR_FL_NAME = "fl"; - public static final String MOTOR_BL_NAME = "bl"; - public static final String MOTOR_FR_NAME = "fr"; - public static final String MOTOR_BR_NAME = "br"; - - public static final String SERVO_GRABBER_NAME = "grabber"; - public static final String SERVO_FOUNDATION_LEFT_NAME = "foundationLeft"; - public static final String SERVO_FOUNDATION_RIGHT_NAME = "foundationRight"; - public static final String SERVO_ROTATE_NAME = "rotate"; - public static final String SERVO_WOBBLE_GRABBER_NAME = "wobbleGrabber"; - public static final String SERVO_WOBBLE_ARM_NAME = "wobbleArm"; + public static final String MOTOR_LIFT = "scissorLift"; + public static final String MOTOR_FL = "fl"; + public static final String MOTOR_BL = "bl"; + public static final String MOTOR_FR = "fr"; + public static final String MOTOR_BR = "br"; + + public static final String SERVO_WOBBLE_GRABBER = "wobbleGrabber"; + public static final String SERVO_WOBBLE_ARM = "wobbleArm"; public static final String SERVO_LAUNCHER = "launcher"; - // Only for temporary purposes - public static final String SERVO_FOUNDATION_LEFT_NEW_NAME = "testFoundationLeft"; - public static final String SERVO_FOUNDATION_RIGHT_NEW_NAME = "testFoundationRight"; - - public static final String CRSERVO_EXTEND_NAME = "extender"; - - public static final String COLOR_SENSOR_DOWN_LIMIT_NAME = "scissorDownLimit"; - public static final String COLOR_SENSOR_UP_LIMIT_NAME = "scissorUpLimit"; + public static final String CRSERVO_INTAKE = "intake2"; public static final String COLOR_SENSOR_PARK = "parkSensor"; - public static final String GYRO_0_NAME = "imu"; - public static final String GYRO_1_NAME = "imu 1"; - - public static final String WEBCAM_0_NAME = "Webcam 1"; - - public static final int SIDE_RED = -1; - public static final int SIDE_BLUE = 1; + public static final String GYRO_0 = "imu"; + public static final String GYRO_1 = "imu 1"; + public static final String WEBCAM_0 = "Webcam 1"; public static final String ENCODER_LATERAL = "lateralEncoder"; public static final String ENCODER_LEFT = "leftEncoder"; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index 728208b..a34d8ab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -40,55 +40,44 @@ public static void init(LinearOpMode l) { */ // Get motors - bl = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_BL_NAME)); - br = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_BR_NAME)); + bl = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_BL)); + br = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_BR)); if (MODE_4x4) { - fl = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FL_NAME)); - fr = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FR_NAME)); + fl = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FL)); + fr = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FR)); } flywheel = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FLYWHEEL)); - //intake = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); + intake = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); flywheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - //intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); HashMap motors = new HashMap<>(); - motors.put(Naming.MOTOR_BL_NAME, bl); - motors.put(Naming.MOTOR_BR_NAME, br); + motors.put(Naming.MOTOR_BL, bl); + motors.put(Naming.MOTOR_BR, br); if (MODE_4x4) { - motors.put(Naming.MOTOR_FL_NAME, fl); - motors.put(Naming.MOTOR_FR_NAME, fr); + motors.put(Naming.MOTOR_FL, fl); + motors.put(Naming.MOTOR_FR, fr); } motors.put(Naming.MOTOR_FLYWHEEL, flywheel); - //motors.put(Naming.MOTOR_INTAKE, intake); + motors.put(Naming.MOTOR_INTAKE, intake); // Get servos - SmartServo wobbleArm = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_ARM_NAME)); - SmartServo wobbleGrabber = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_GRABBER_NAME)); + SmartServo wobbleArm = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_ARM)); + SmartServo wobbleGrabber = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_GRABBER)); SmartServo launcher = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_LAUNCHER)); - //SmartServo grabber = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_GRABBER_NAME)); - //SmartServo rotate = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_ROTATE_NAME)); - //SmartServo fLeftNew = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_FOUNDATION_LEFT_NEW_NAME)); - //SmartServo fRightNew = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_FOUNDATION_RIGHT_NEW_NAME)); - - //fLeftNew.setDirection(Servo.Direction.REVERSE); - //fRightNew.setDirection(Servo.Direction.FORWARD); // Add servos into the list HashMap servos = new HashMap<>(); - servos.put(Naming.SERVO_WOBBLE_ARM_NAME, wobbleArm); - servos.put(Naming.SERVO_WOBBLE_GRABBER_NAME, wobbleGrabber); + servos.put(Naming.SERVO_WOBBLE_ARM, wobbleArm); + servos.put(Naming.SERVO_WOBBLE_GRABBER, wobbleGrabber); servos.put(Naming.SERVO_LAUNCHER, launcher); - //servos.put(Naming.SERVO_GRABBER_NAME, grabber); - //servos.put(Naming.SERVO_ROTATE_NAME, rotate); - //servos.put(Naming.SERVO_FOUNDATION_LEFT_NEW_NAME, fLeftNew); - //servos.put(Naming.SERVO_FOUNDATION_RIGHT_NEW_NAME, fRightNew); // Get CRServos - //CRServo armExtend = l.hardwareMap.get(CRServo.class, Naming.CRSERVO_EXTEND_NAME); + CRServo intake2 = l.hardwareMap.get(CRServo.class, Naming.CRSERVO_INTAKE); // Add CRServos into the list HashMap crServos = new HashMap<>(); - //crServos.put(Naming.CRSERVO_EXTEND_NAME, armExtend); + crServos.put(Naming.CRSERVO_INTAKE, intake2); // Most robots need the motor on one side to be reversed to drive forward // Reverse the motor that runs backwards when connected directly to the battery @@ -100,7 +89,6 @@ public static void init(LinearOpMode l) { } // Set motors to spin in the correct direction - //sc.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); if (MODE_4x4) { @@ -119,31 +107,27 @@ public static void init(LinearOpMode l) { //rotate.setDirection(Servo.Direction.REVERSE); // Get color sensors - //ColorSensor scissorDownLimit = l.hardwareMap.get(ColorSensor.class, Naming.COLOR_SENSOR_DOWN_LIMIT_NAME); - //ColorSensor scissorUpLimit = l.hardwareMap.get(ColorSensor.class, Naming.COLOR_SENSOR_UP_LIMIT_NAME); NormalizedColorSensor parkSensor = (NormalizedColorSensor)l.hardwareMap.get(ColorSensor.class, Naming.COLOR_SENSOR_PARK); // Add color sensors into list HashMap colorSensors = new HashMap<>(); - //colorSensors.put(Naming.COLOR_SENSOR_DOWN_LIMIT_NAME, scissorDownLimit); - //colorSensors.put(Naming.COLOR_SENSOR_UP_LIMIT_NAME, scissorUpLimit); colorSensors.put(Naming.COLOR_SENSOR_PARK, parkSensor); // Get webcams - //WebcamName webcam1 = l.hardwareMap.get(WebcamName.class, Naming.WEBCAM_0_NAME); + WebcamName webcam1 = l.hardwareMap.get(WebcamName.class, Naming.WEBCAM_0); // Add webcams to list HashMap webcams = new HashMap<>(); - //webcams.put(Naming.WEBCAM_0_NAME, webcam1); + webcams.put(Naming.WEBCAM_0, webcam1); // Get gyros - BNO055IMU gyro0 = l.hardwareMap.get(BNO055IMU.class, Naming.GYRO_0_NAME); - BNO055IMU gyro1 = l.hardwareMap.get(BNO055IMU.class, Naming.GYRO_1_NAME); + BNO055IMU gyro0 = l.hardwareMap.get(BNO055IMU.class, Naming.GYRO_0); + BNO055IMU gyro1 = l.hardwareMap.get(BNO055IMU.class, Naming.GYRO_1); // Add gyros to list HashMap gyros = new HashMap<>(); - gyros.put(Naming.GYRO_0_NAME, gyro0); - gyros.put(Naming.GYRO_1_NAME, gyro1); + gyros.put(Naming.GYRO_0, gyro0); + gyros.put(Naming.GYRO_1, gyro1); // Get dead wheel encoders leftEncoder = new Encoder(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java index 79e0786..7dbafa6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java @@ -81,10 +81,10 @@ public CRServo getCRServo(String id) { * @param brPower Power to the back right wheel */ public void move4x4(double flPower, double frPower, double blPower, double brPower) { - Objects.requireNonNull(motors.get(Naming.MOTOR_FL_NAME)).setPower(flPower); - Objects.requireNonNull(motors.get(Naming.MOTOR_FR_NAME)).setPower(frPower); - Objects.requireNonNull(motors.get(Naming.MOTOR_BL_NAME)).setPower(blPower); - Objects.requireNonNull(motors.get(Naming.MOTOR_BR_NAME)).setPower(brPower); + Objects.requireNonNull(motors.get(Naming.MOTOR_FL)).setPower(flPower); + Objects.requireNonNull(motors.get(Naming.MOTOR_FR)).setPower(frPower); + Objects.requireNonNull(motors.get(Naming.MOTOR_BL)).setPower(blPower); + Objects.requireNonNull(motors.get(Naming.MOTOR_BR)).setPower(brPower); } /** @@ -93,10 +93,10 @@ public void move4x4(double flPower, double frPower, double blPower, double brPow * @param rPower Power to the right side */ public void move2x4(double lPower, double rPower) { - Objects.requireNonNull(motors.get(Naming.MOTOR_FL_NAME)).setPower(lPower); - Objects.requireNonNull(motors.get(Naming.MOTOR_FR_NAME)).setPower(rPower); - Objects.requireNonNull(motors.get(Naming.MOTOR_BL_NAME)).setPower(lPower); - Objects.requireNonNull(motors.get(Naming.MOTOR_BR_NAME)).setPower(rPower); + Objects.requireNonNull(motors.get(Naming.MOTOR_FL)).setPower(lPower); + Objects.requireNonNull(motors.get(Naming.MOTOR_FR)).setPower(rPower); + Objects.requireNonNull(motors.get(Naming.MOTOR_BL)).setPower(lPower); + Objects.requireNonNull(motors.get(Naming.MOTOR_BR)).setPower(rPower); } /** @@ -105,8 +105,8 @@ public void move2x4(double lPower, double rPower) { * @param rPower Power sent to back right motor */ public void move2x2(double lPower, double rPower) { - Objects.requireNonNull(motors.get(Naming.MOTOR_BL_NAME)).setPower(lPower); - Objects.requireNonNull(motors.get(Naming.MOTOR_BR_NAME)).setPower(rPower); + Objects.requireNonNull(motors.get(Naming.MOTOR_BL)).setPower(lPower); + Objects.requireNonNull(motors.get(Naming.MOTOR_BR)).setPower(rPower); } /** @@ -114,7 +114,7 @@ public void move2x2(double lPower, double rPower) { * @param speed Speed of the elevator */ public void moveElevator(double speed) { - Objects.requireNonNull(motors.get(Naming.MOTOR_LIFT_NAME)).setPower(speed*ELEVATOR_POWER); + Objects.requireNonNull(motors.get(Naming.MOTOR_LIFT)).setPower(speed*ELEVATOR_POWER); } /** @@ -166,6 +166,7 @@ public void moveFlywheel(double wheelPower) { */ public void moveIntake(double intakePower) { Objects.requireNonNull(motors.get(Naming.MOTOR_INTAKE)).setPower(intakePower); + Objects.requireNonNull(crServos.get(Naming.CRSERVO_INTAKE)).setPower(intakePower); } /** @@ -174,9 +175,9 @@ public void moveIntake(double intakePower) { */ public void moveWobble(boolean command) { if (command) { - Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_ARM_NAME)).setPosition(WOBBLE_IN); // Opens the grabber + Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_ARM)).setPosition(WOBBLE_IN); // Opens the grabber } else { - Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_ARM_NAME)).setPosition(WOBBLE_OUT); // Closes the grabber + Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_ARM)).setPosition(WOBBLE_OUT); // Closes the grabber } } @@ -186,9 +187,9 @@ public void moveWobble(boolean command) { */ public void grabWobble(boolean command) { if (command) { - Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_GRABBER_NAME)).setPosition(WOBBLE_GRAB); // Opens the grabber + Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_GRABBER)).setPosition(WOBBLE_GRAB); // Opens the grabber } else { - Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_GRABBER_NAME)).setPosition(WOBBLE_RELEASE); // Closes the grabber + Objects.requireNonNull(servos.get(Naming.SERVO_WOBBLE_GRABBER)).setPosition(WOBBLE_RELEASE); // Closes the grabber } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java index 4d4c0b7..275caaf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java @@ -124,7 +124,7 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { } // TODO: adjust the names of the following hardware devices to match your configuration - imu = Sensor.getGyro(Naming.GYRO_0_NAME); + imu = Sensor.getGyro(Naming.GYRO_0); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS; imu.initialize(parameters); @@ -133,10 +133,10 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { // upward (normal to the floor) using a command like the following: BNO055IMUUtil.remapAxes(imu, AxesOrder.XYZ, AxesSigns.NPN); - fl = Robot.movement.getMotor(Naming.MOTOR_FL_NAME); - bl = Robot.movement.getMotor(Naming.MOTOR_BL_NAME); - br = Robot.movement.getMotor(Naming.MOTOR_BR_NAME); - fr = Robot.movement.getMotor(Naming.MOTOR_FR_NAME); + fl = Robot.movement.getMotor(Naming.MOTOR_FL); + bl = Robot.movement.getMotor(Naming.MOTOR_BL); + br = Robot.movement.getMotor(Naming.MOTOR_BR); + fr = Robot.movement.getMotor(Naming.MOTOR_FR); motors = Arrays.asList(fl, bl, br, fr); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java index d5aedfe..1a93436 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java @@ -119,10 +119,10 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Heading", vel.getHeading()); telemetry.addData("X", vel.getX()); telemetry.addData("Y", vel.getY()); - telemetry.addData("Back Left", Objects.requireNonNull(Movement.motors.get(Naming.MOTOR_BL_NAME)).getVelocity()); - telemetry.addData("Back Right", Objects.requireNonNull(Movement.motors.get(Naming.MOTOR_BR_NAME)).getVelocity()); - telemetry.addData("Front Left", Objects.requireNonNull(Movement.motors.get(Naming.MOTOR_FL_NAME)).getVelocity()); - telemetry.addData("Front right", Objects.requireNonNull(Movement.motors.get(Naming.MOTOR_FR_NAME)).getVelocity()); + telemetry.addData("Back Left", Objects.requireNonNull(Movement.motors.get(Naming.MOTOR_BL)).getVelocity()); + telemetry.addData("Back Right", Objects.requireNonNull(Movement.motors.get(Naming.MOTOR_BR)).getVelocity()); + telemetry.addData("Front Left", Objects.requireNonNull(Movement.motors.get(Naming.MOTOR_FL)).getVelocity()); + telemetry.addData("Front right", Objects.requireNonNull(Movement.motors.get(Naming.MOTOR_FR)).getVelocity()); telemetry.addLine(); telemetry.addLine("Press Y/△ to conclude routine"); telemetry.update(); From 18531c87dc0eb7c91f5d63e4007db3c1225757f0 Mon Sep 17 00:00:00 2001 From: Varun Damarla <57463486+damarlavarun2004@users.noreply.github.com> Date: Thu, 11 Feb 2021 14:47:42 -0500 Subject: [PATCH 20/29] Modified intake and shooter controls and shooter servo --- .../java/org/firstinspires/ftc/teamcode/API/Movement.java | 4 ++-- .../ftc/teamcode/OpMode/TeleOp/TeleOpMain.java | 2 +- .../ftc/teamcode/OpMode/TeleOp/ZeroServo.java | 8 ++++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java index 7dbafa6..6fc4f2b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java @@ -52,7 +52,7 @@ public class Movement { private final static double SWIVEL_CLOSE = 1; // on a scale of 0-1 private final static double FOUNDATION_OPEN = 0.3; private final static double FOUNDATION_CLOSE = 0.95; - private final static double LAUNCHER_OPEN = 0.8; + private final static double LAUNCHER_OPEN = 0.7; private final static double LAUNCHER_CLOSE = 1; public static HashMap motors; @@ -166,7 +166,7 @@ public void moveFlywheel(double wheelPower) { */ public void moveIntake(double intakePower) { Objects.requireNonNull(motors.get(Naming.MOTOR_INTAKE)).setPower(intakePower); - Objects.requireNonNull(crServos.get(Naming.CRSERVO_INTAKE)).setPower(intakePower); + Objects.requireNonNull(crServos.get(Naming.CRSERVO_INTAKE)).setPower(-intakePower); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java index 96febeb..cb21c22 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java @@ -60,7 +60,7 @@ public void runOpMode() { Robot.movement.move4x4(flPower, frPower, blPower, brPower); Robot.movement.moveFlywheel(gamepad2.left_trigger); - Robot.movement.moveIntake(gamepad2.left_trigger); + Robot.movement.moveIntake(gamepad2.right_trigger); if (gamepad1.left_bumper) { if (brakeSwitch) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java index 95248b8..8bde9c9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/ZeroServo.java @@ -24,7 +24,7 @@ associated documentation files (the "Software"), to deal in the Software without public class ZeroServo extends LinearOpMode { @Override public void runOpMode() { - Servo zeroServo = hardwareMap.get(Servo.class, "zeroServo"); + Servo zeroServo = hardwareMap.get(Servo.class, "launcher"); telemetry.addData("Status", "Initialized"); telemetry.update(); @@ -39,9 +39,9 @@ public void runOpMode() { waitForStart(); while(opModeIsActive()) { - if (gamepad1.x) { - zeroServo.setPosition(0.8); - } else if (gamepad1.y) { + if (gamepad2.left_bumper) { + zeroServo.setPosition(0.7); + } else { zeroServo.setPosition(1); } } From 4f93a782ed2659f063a1efed3e87766bea889a3c Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Thu, 11 Feb 2021 20:29:10 -0500 Subject: [PATCH 21/29] Intake, flywheel curve, and some tuning --- .../ftc/teamcode/API/Config/Naming.java | 3 +- .../ftc/teamcode/API/InitRobot.java | 28 +++++++++++-------- .../ftc/teamcode/API/Movement.java | 4 +-- .../teamcode/OpMode/TeleOp/TeleOpMain.java | 4 ++- 4 files changed, 23 insertions(+), 16 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java index 631223c..d9f3e3e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Naming.java @@ -16,8 +16,6 @@ public class Naming { public static final String SERVO_WOBBLE_ARM = "wobbleArm"; public static final String SERVO_LAUNCHER = "launcher"; - public static final String CRSERVO_INTAKE = "intake2"; - public static final String COLOR_SENSOR_PARK = "parkSensor"; public static final String GYRO_0 = "imu"; @@ -31,5 +29,6 @@ public class Naming { public static final String MOTOR_FLYWHEEL = "flywheel"; public static final String MOTOR_INTAKE = "intake"; + public static final String MOTOR_INTAKE2 = "rightEncoder"; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java index a34d8ab..7420d2c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/InitRobot.java @@ -24,8 +24,8 @@ */ public class InitRobot { public static final boolean MODE_4x4 = true; // True if you are using 4x4 drive - private static SmartMotor bl, br, fl, fr, flywheel, intake; - private static Encoder leftEncoder, rightEncoder, lateralEncoder; + private static SmartMotor fl; + private static SmartMotor fr; // TODO: JavaDoc public static void init(LinearOpMode l) { @@ -40,16 +40,18 @@ public static void init(LinearOpMode l) { */ // Get motors - bl = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_BL)); - br = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_BR)); + SmartMotor bl = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_BL)); + SmartMotor br = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_BR)); if (MODE_4x4) { fl = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FL)); fr = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FR)); } - flywheel = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FLYWHEEL)); - intake = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); + SmartMotor flywheel = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_FLYWHEEL)); + SmartMotor intake = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); + SmartMotor intake2 = new SmartMotor(l.hardwareMap.get(DcMotorEx.class, Naming.MOTOR_INTAKE2)); flywheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + intake2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); HashMap motors = new HashMap<>(); motors.put(Naming.MOTOR_BL, bl); @@ -60,6 +62,7 @@ public static void init(LinearOpMode l) { } motors.put(Naming.MOTOR_FLYWHEEL, flywheel); motors.put(Naming.MOTOR_INTAKE, intake); + motors.put(Naming.MOTOR_INTAKE2, intake2); // Get servos SmartServo wobbleArm = new SmartServo(l.hardwareMap.get(Servo.class, Naming.SERVO_WOBBLE_ARM)); @@ -73,11 +76,12 @@ public static void init(LinearOpMode l) { servos.put(Naming.SERVO_LAUNCHER, launcher); // Get CRServos - CRServo intake2 = l.hardwareMap.get(CRServo.class, Naming.CRSERVO_INTAKE); + + // Set direction of CRServos + //intake2.setDirection(CRServo.Direction.REVERSE); // Add CRServos into the list HashMap crServos = new HashMap<>(); - crServos.put(Naming.CRSERVO_INTAKE, intake2); // Most robots need the motor on one side to be reversed to drive forward // Reverse the motor that runs backwards when connected directly to the battery @@ -87,6 +91,8 @@ public static void init(LinearOpMode l) { fl.setDirection(DcMotor.Direction.FORWARD); fr.setDirection(DcMotor.Direction.REVERSE); } + + intake2.setDirection(DcMotor.Direction.REVERSE); // Set motors to spin in the correct direction bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -130,9 +136,9 @@ public static void init(LinearOpMode l) { gyros.put(Naming.GYRO_1, gyro1); // Get dead wheel encoders - leftEncoder = new Encoder(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); - rightEncoder = new Encoder(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_RIGHT)); - lateralEncoder = new Encoder(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LATERAL)); + Encoder leftEncoder = new Encoder(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LEFT)); + Encoder rightEncoder = new Encoder(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_RIGHT)); + Encoder lateralEncoder = new Encoder(l.hardwareMap.get(DcMotorEx.class, Naming.ENCODER_LATERAL)); // Add encoders to list HashMap encoders = new HashMap<>(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java index 6fc4f2b..6dd84d0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java @@ -52,7 +52,7 @@ public class Movement { private final static double SWIVEL_CLOSE = 1; // on a scale of 0-1 private final static double FOUNDATION_OPEN = 0.3; private final static double FOUNDATION_CLOSE = 0.95; - private final static double LAUNCHER_OPEN = 0.7; + private final static double LAUNCHER_OPEN = 0.6; private final static double LAUNCHER_CLOSE = 1; public static HashMap motors; @@ -166,7 +166,7 @@ public void moveFlywheel(double wheelPower) { */ public void moveIntake(double intakePower) { Objects.requireNonNull(motors.get(Naming.MOTOR_INTAKE)).setPower(intakePower); - Objects.requireNonNull(crServos.get(Naming.CRSERVO_INTAKE)).setPower(-intakePower); + Objects.requireNonNull(motors.get(Naming.MOTOR_INTAKE2)).setPower(intakePower); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java index cb21c22..229d699 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java @@ -23,6 +23,7 @@ associated documentation files (the "Software"), to deal in the Software without import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.teamcode.API.Config.Naming; import org.firstinspires.ftc.teamcode.API.InitRobot; import org.firstinspires.ftc.teamcode.API.Robot; @@ -59,7 +60,7 @@ public void runOpMode() { double frPower = Range.clip((-x1 - y1 - rotation), -maxspeed, maxspeed); Robot.movement.move4x4(flPower, frPower, blPower, brPower); - Robot.movement.moveFlywheel(gamepad2.left_trigger); + Robot.movement.moveFlywheel(Range.clip(Math.log10(gamepad2.left_trigger*10+1), 0, 1)); Robot.movement.moveIntake(gamepad2.right_trigger); if (gamepad1.left_bumper) { @@ -103,6 +104,7 @@ public void runOpMode() { telemetry.addData("Back Right", brPower); telemetry.addData("Front Left", flPower); telemetry.addData("Front Right", frPower); + telemetry.addData("Flywheel", Robot.movement.getMotor(Naming.MOTOR_FLYWHEEL).getPower()); telemetry.update(); } } From e2527efe5e478b265b5ede9f0aaed31531698f5e Mon Sep 17 00:00:00 2001 From: Varun Damarla <57463486+damarlavarun2004@users.noreply.github.com> Date: Fri, 12 Feb 2021 18:39:41 -0500 Subject: [PATCH 22/29] RR to 0.5.3; Constants and SampleMecanumDrive modified as per update --- TeamCode/build.release.gradle | 2 +- .../ftc/teamcode/API/Config/Constants.java | 11 +++---- .../ftc/teamcode/API/SampleMecanumDrive.java | 33 ++++++++++++------- 3 files changed, 28 insertions(+), 18 deletions(-) diff --git a/TeamCode/build.release.gradle b/TeamCode/build.release.gradle index ce141c0..42c0159 100644 --- a/TeamCode/build.release.gradle +++ b/TeamCode/build.release.gradle @@ -8,7 +8,7 @@ dependencies { implementation 'org.firstinspires.ftc:FtcCommon:6.0.1' implementation (name: 'tfod-release', ext:'aar') implementation (name: 'tensorflow-lite-0.0.0-nightly', ext:'aar') - implementation 'com.acmerobotics.roadrunner:core:0.5.2' + implementation 'com.acmerobotics.roadrunner:core:0.5.3' implementation 'com.acmerobotics.dashboard:dashboard:0.3.10' implementation 'org.apache.commons:commons-math3:3.6.1' } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java index 7664c1b..77a5098 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Config/Constants.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode.API.Config; -import com.acmerobotics.roadrunner.trajectory.constraints.DriveConstraints; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.PIDFCoefficients; public class Constants { @@ -51,11 +51,10 @@ public class Constants { * acceleration values are required, and the jerk values are optional (setting a jerk of 0.0 * forces acceleration-limited profiling). All distance units are inches. */ - public static DriveConstraints BASE_CONSTRAINTS = new DriveConstraints( - 30.0, 30.0, 0.0, - Math.toRadians(180.0), Math.toRadians(180.0), 0.0 - ); - + public static double MAX_VEL = 30.0; + public static double MAX_ACCEL = 30.0; + public static double MAX_ANG_VEL = Math.toRadians(180.0); + public static double MAX_ANG_ACCEL = Math.toRadians(180.0); public static double encoderTicksToInches(double ticks) { return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java index 275caaf..75ab9fc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/SampleMecanumDrive.java @@ -17,8 +17,12 @@ import com.acmerobotics.roadrunner.profile.MotionState; import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; -import com.acmerobotics.roadrunner.trajectory.constraints.DriveConstraints; -import com.acmerobotics.roadrunner.trajectory.constraints.MecanumConstraints; +import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; import com.acmerobotics.roadrunner.util.NanoClock; import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.lynx.LynxModule; @@ -44,7 +48,10 @@ import java.util.LinkedList; import java.util.List; -import static org.firstinspires.ftc.teamcode.API.Config.Constants.BASE_CONSTRAINTS; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.MAX_ANG_ACCEL; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.MAX_ANG_VEL; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.MAX_VEL; import static org.firstinspires.ftc.teamcode.API.Config.Constants.MOTOR_VELO_PID; import static org.firstinspires.ftc.teamcode.API.Config.Constants.RUN_USING_ENCODER; import static org.firstinspires.ftc.teamcode.API.Config.Constants.TRACK_WIDTH; @@ -83,7 +90,8 @@ public enum Mode { private MotionProfile turnProfile; private double turnStart; - private DriveConstraints constraints; + private TrajectoryVelocityConstraint velConstraint; + private TrajectoryAccelerationConstraint accelConstraint; private TrajectoryFollower follower; private LinkedList poseHistory; @@ -109,7 +117,11 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { turnController = new PIDFController(HEADING_PID); turnController.setInputBounds(0, 2 * Math.PI); - constraints = new MecanumConstraints(BASE_CONSTRAINTS, TRACK_WIDTH); + velConstraint = new MinVelocityConstraint(Arrays.asList( + new AngularVelocityConstraint(MAX_ANG_VEL), + new MecanumVelocityConstraint(MAX_VEL, TRACK_WIDTH) + )); + accelConstraint = new ProfileAccelerationConstraint(MAX_ACCEL); follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID, new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5); @@ -167,15 +179,15 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { } public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { - return new TrajectoryBuilder(startPose, constraints); + return new TrajectoryBuilder(startPose, velConstraint, accelConstraint); } public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) { - return new TrajectoryBuilder(startPose, reversed, constraints); + return new TrajectoryBuilder(startPose, reversed, velConstraint, accelConstraint); } public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) { - return new TrajectoryBuilder(startPose, startHeading, constraints); + return new TrajectoryBuilder(startPose, startHeading, velConstraint, accelConstraint); } public void turnAsync(double angle) { @@ -186,9 +198,8 @@ public void turnAsync(double angle) { turnProfile = MotionProfileGenerator.generateSimpleMotionProfile( new MotionState(heading, 0, 0, 0), new MotionState(heading + angle, 0, 0, 0), - constraints.maxAngVel, - constraints.maxAngAccel, - constraints.maxAngJerk + MAX_ANG_VEL, + MAX_ANG_ACCEL ); turnStart = clock.seconds(); From beeddf2c1d6dd776266959708836d67f75df8e4c Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Fri, 12 Feb 2021 19:16:10 -0500 Subject: [PATCH 23/29] update opmode testers to latest roadrunner --- .../Autonomous/DriveVelocityPIDTuner.java | 34 +++++++------------ .../Autonomous/ManualFeedforwardTuner.java | 16 ++++----- 2 files changed, 20 insertions(+), 30 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/DriveVelocityPIDTuner.java index 726c034..47bc9f7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/DriveVelocityPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/DriveVelocityPIDTuner.java @@ -13,11 +13,12 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.RobotLog; -import org.firstinspires.ftc.teamcode.API.Config.Constants; import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; import java.util.List; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.MAX_VEL; import static org.firstinspires.ftc.teamcode.API.Config.Constants.MOTOR_VELO_PID; import static org.firstinspires.ftc.teamcode.API.Config.Constants.RUN_USING_ENCODER; import static org.firstinspires.ftc.teamcode.API.Config.Constants.kV; @@ -32,7 +33,7 @@ * connected, start the program, and your robot will begin moving forward and backward according to * a motion profile. Your job is to graph the velocity errors over time and adjust the PID * coefficients (note: the tuning variable will not appear until the op mode finishes initializing). - * Once you've found a satisfactory set of gains, add them to the Constants.java file under the + * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the * MOTOR_VELO_PID field. * * Recommended tuning process: @@ -53,29 +54,15 @@ public class DriveVelocityPIDTuner extends LinearOpMode { public static double DISTANCE = 72; // in - private FtcDashboard dashboard = FtcDashboard.getInstance(); - - private SampleMecanumDrive drive; - enum Mode { DRIVER_MODE, TUNING_MODE } - private Mode mode; - - private double lastKp = MOTOR_VELO_PID.p; - private double lastKi = MOTOR_VELO_PID.i; - private double lastKd = MOTOR_VELO_PID.d; - private double lastKf = MOTOR_VELO_PID.f; - private static MotionProfile generateProfile(boolean movingForward) { MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); - return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, - Constants.BASE_CONSTRAINTS.maxVel, - Constants.BASE_CONSTRAINTS.maxAccel, - Constants.BASE_CONSTRAINTS.maxJerk); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); } @Override @@ -85,11 +72,16 @@ public void runOpMode() { "PID is not in use", getClass().getSimpleName()); } - telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry()); + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - drive = new SampleMecanumDrive(hardwareMap); + Mode mode = Mode.TUNING_MODE; - mode = Mode.TUNING_MODE; + double lastKp = MOTOR_VELO_PID.p; + double lastKi = MOTOR_VELO_PID.i; + double lastKd = MOTOR_VELO_PID.d; + double lastKf = MOTOR_VELO_PID.f; drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); @@ -137,7 +129,7 @@ public void runOpMode() { // update telemetry telemetry.addData("targetVelocity", motionState.getV()); for (int i = 0; i < velocities.size(); i++) { - telemetry.addData("velocity" + i, velocities.get(i)); + telemetry.addData("measuredVelocity" + i, velocities.get(i)); telemetry.addData( "error" + i, motionState.getV() - velocities.get(i) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/ManualFeedforwardTuner.java index d6f5021..a503336 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/ManualFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/ManualFeedforwardTuner.java @@ -13,11 +13,12 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.util.RobotLog; -import org.firstinspires.ftc.teamcode.API.Config.Constants; import org.firstinspires.ftc.teamcode.API.SampleMecanumDrive; import java.util.Objects; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.API.Config.Constants.MAX_VEL; import static org.firstinspires.ftc.teamcode.API.Config.Constants.RUN_USING_ENCODER; import static org.firstinspires.ftc.teamcode.API.Config.Constants.kA; import static org.firstinspires.ftc.teamcode.API.Config.Constants.kStatic; @@ -32,7 +33,7 @@ * you are using the Control Hub. Once you've successfully connected, start the program, and your * robot will begin moving forward and backward according to a motion profile. Your job is to graph * the velocity errors over time and adjust the feedforward coefficients. Once you've found a - * satisfactory set of gains, add them to the appropriate fields in the Constants.java file. + * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file. * * Pressing X (on the Xbox and Logitech F310 gamepads, square on the PS4 Dualshock gamepad) will * pause the tuning process and enter driver override, allowing the user to reset the position of @@ -59,10 +60,7 @@ enum Mode { private static MotionProfile generateProfile(boolean movingForward) { MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); - return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, - Constants.BASE_CONSTRAINTS.maxVel, - Constants.BASE_CONSTRAINTS.maxAccel, - Constants.BASE_CONSTRAINTS.maxJerk); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); } @Override @@ -123,8 +121,8 @@ public void runOpMode() { // update telemetry telemetry.addData("targetVelocity", motionState.getV()); - telemetry.addData("poseVelocity", currentVelo); - telemetry.addData("error", currentVelo - motionState.getV()); + telemetry.addData("measuredVelocity", currentVelo); + telemetry.addData("error", motionState.getV() - currentVelo); break; case DRIVER_MODE: if (gamepad1.a) { @@ -147,4 +145,4 @@ public void runOpMode() { telemetry.update(); } } -} \ No newline at end of file +} From d64fe97142e688252b7c5a47b955c5915831742f Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Fri, 12 Feb 2021 20:48:47 -0500 Subject: [PATCH 24/29] Intake2 speed limit, backwards in case ring sticks --- .../java/org/firstinspires/ftc/teamcode/API/Movement.java | 5 ++++- .../firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java | 1 + 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java index 6dd84d0..42d444d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Movement.java @@ -38,6 +38,9 @@ public class Movement { // Elevator configuration private final static double ELEVATOR_POWER = 1.00; + + // Motor configuration + private final static double INTAKE2_THRESH = 0.8; // Servo configuration private final static int SERVO_SLEEP = 10; // Milliseconds @@ -166,7 +169,7 @@ public void moveFlywheel(double wheelPower) { */ public void moveIntake(double intakePower) { Objects.requireNonNull(motors.get(Naming.MOTOR_INTAKE)).setPower(intakePower); - Objects.requireNonNull(motors.get(Naming.MOTOR_INTAKE2)).setPower(intakePower); + Objects.requireNonNull(motors.get(Naming.MOTOR_INTAKE2)).setPower(intakePower*INTAKE2_THRESH); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java index 229d699..0cc3f0e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java @@ -62,6 +62,7 @@ public void runOpMode() { Robot.movement.move4x4(flPower, frPower, blPower, brPower); Robot.movement.moveFlywheel(Range.clip(Math.log10(gamepad2.left_trigger*10+1), 0, 1)); Robot.movement.moveIntake(gamepad2.right_trigger); + Robot.movement.moveIntake((gamepad2.right_bumper ? -1 : 0)); if (gamepad1.left_bumper) { if (brakeSwitch) { From eb296ac94c75e8d57501aad02fd9f9a8ec06de19 Mon Sep 17 00:00:00 2001 From: Sambhav Saggi <17993169+sambhavsaggi@users.noreply.github.com> Date: Sat, 13 Feb 2021 20:28:01 -0500 Subject: [PATCH 25/29] Update app from 6.0.1 -> 6.1.1 --- .github/CONTRIBUTING.md | 68 +++++++++ .github/PULL_REQUEST_TEMPLATE.md | 1 + .gitignore | 5 - .idea/codeStyles/Project.xml | 134 ------------------ .idea/compiler.xml | 9 ++ .idea/jarRepositories.xml | 5 - .idea/misc.xml | 2 +- .idea/runConfigurations.xml | 12 -- .travis.yml | 21 --- FtcRobotController/build.gradle | 8 +- FtcRobotController/build.release.gradle | 16 +-- .../src/main/AndroidManifest.xml | 7 +- .../src/main/assets/trajectory/_group.yaml | 8 -- .../ConceptTensorFlowObjectDetection.java | 6 +- ...rFlowObjectDetectionSwitchableCameras.java | 6 +- ...onceptTensorFlowObjectDetectionWebcam.java | 6 +- .../internal/FtcRobotControllerActivity.java | 11 +- LICENSE.md | 7 - TeamCode/build.gradle | 2 +- TeamCode/build.release.gradle | 11 +- .../src/main/assets/trajectory/Test1.yaml | 15 -- .../src/main/assets/trajectory/_group.yaml | 8 -- .../ftc/teamcode/API/Config/Constants.java | 1 - .../ftc/teamcode/API/SampleMecanumDrive.java | 22 +-- .../ftc/teamcode/API/Util/Encoder.java | 93 ++++++++++++ .../ftc/teamcode/API/Util/LynxModuleUtil.java | 5 +- .../ftc/teamcode/API/Util/MecanumDrive.kt | 58 +++++--- .../ftc/teamcode/API/Util/RegressionUtil.java | 4 +- .../src/main/res/raw/init_config.properties | 7 - build.common.gradle | 5 +- build.gradle | 4 +- gradle/wrapper/gradle-wrapper.properties | 4 +- gradlew | 0 33 files changed, 258 insertions(+), 313 deletions(-) create mode 100644 .github/CONTRIBUTING.md create mode 100644 .github/PULL_REQUEST_TEMPLATE.md delete mode 100644 .idea/codeStyles/Project.xml create mode 100644 .idea/compiler.xml delete mode 100644 .idea/runConfigurations.xml delete mode 100644 .travis.yml delete mode 100644 FtcRobotController/src/main/assets/trajectory/_group.yaml delete mode 100644 LICENSE.md delete mode 100644 TeamCode/src/main/assets/trajectory/Test1.yaml delete mode 100644 TeamCode/src/main/assets/trajectory/_group.yaml create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API/Util/Encoder.java delete mode 100644 TeamCode/src/main/res/raw/init_config.properties mode change 100755 => 100644 gradlew diff --git a/.github/CONTRIBUTING.md b/.github/CONTRIBUTING.md new file mode 100644 index 0000000..bfd93bc --- /dev/null +++ b/.github/CONTRIBUTING.md @@ -0,0 +1,68 @@ +# Contributing to the FTC SDK + +The following is a set of guidelines for contributing the FIRST FTC SDK. The FTC Technology Team welcomes suggestions for improvements to core software, ideas for new features, requests for built-in support of new sensors, and well written bug reports. + +## How can I contribute? + +### Pull requests + +__STOP!__ If you are new to git, do not understand the mechanics of forks, branches, and pulls, if what you just read is confusing, __do not__ push this button. Most likely it won't do what you think it will. + +![Pull Button](../doc/media/PullRequest.PNG) + +If you are looking at this button then you've pushed some changes to your team's fork of ftctechnh/ftc_app. Congratulations! You are almost certainly finished. + +The vast majority of pull requests seen on the ftctechnh/ftc_app repository are not intended to be merged into the official SDK. Team software is just that, your team's. It's specific to the tasks you are trying to accomplish, the testing you are doing, and goals your team has. You don't want that pushed into the official SDK. + +If what you've read so far makes little sense, there are some very good git learning resources online. +[Git Book](https://git-scm.com/book/en/v2) +[Interactive Git Tutorial](https://try.github.io) + +##### Guidlines for experienced GIT users. + +If you are absolutely certain that you want to push the big green button above, read on. Otherwise back _slowly away from keyboard_. + +The real intent for advanced users is often to issue a pull request from the [branch](https://www.atlassian.com/git/tutorials/using-branches/git-branch) on a local fork back to master on either the same local fork or a child of the team fork and not on the parent ftctechnh/ftc_app. See [Creating a Pull Request](https://help.github.com/articles/creating-a-pull-request-from-a-fork/). + +If that is indeed the intent, then you can merge your [topic branch](https://git-scm.com/book/en/v2/Git-Branching-Branching-Workflows#Topic-Branches) into master locally by hand before pushing it up to github, or if you want a pull request for pulls between branches on the same repository because, say, you want team members to look at your software before merging into master, you can select the base fork from the dropdown on the "Open a pull request" page and select your team repo instead of ftctechnh's. + +Alternatively, if you have a team repository forked from ftctechnh/ftc_app, and then team members individually fork from your team repository, then pull requests from the individual team member's forks will have the main team repository automatically selected as the base fork for the pull. And you won't inadvertently request to pull your team software into ftctechnh's repository. + +The latter would be the "best" way to manage software among a large team. But as with all things git there are many options. + +Pull requests that do not fall into the category above are evaluated by the FTC Technology Team on a case-by-case basis. Please note however that the deployment model of the SDK does not support direct pulls into ftctechnh/ftc_app. + +### Report bugs + +This section guides you through filing a bug report. The better the report the more likely it is to be root caused and fixed. Please refrain from feature requests or software enhancements when opening new issues. See Suggesting Enhancements below. + +#### Before submitting a bug report + +- Check the [forums](http://ftcforum.usfirst.org/forum.php) to see if someone else has run into the problem and whether there is an official solution that doesn't require a new SDK. + +- Perform a search of current [issues](https://github.com/ftctechnh/ftc_app/issues) to see if the problem has already been reported. If so, add a comment to the existing issue instead of creating a new one. + +#### How Do I Submit A (Good) Bug Report? + +Bugs are tracked as GitHub issues. Create an issue on ftctechnh/ftc_app and provide the following information. +Explain the problem and include additional details to help maintainers reproduce the problem: + +- Use a clear and descriptive title for the issue to identify the problem. + +- Describe the exact steps which reproduce the problem in as many details as possible. + +- Provide specific examples to demonstrate the steps. + +- Describe the behavior you observed after following the steps and point out what exactly is the problem with that behavior. Explain which behavior you expected to see instead and why. If applicable, include screenshots which show you following the described steps and clearly demonstrate the problem. + +- If you're reporting that the RobotController crashed, include the logfile with a stack trace of the crash. [Example of good bug report with stack trace](https://github.com/ftctechnh/ftc_app/issues/224) + +- If the problem wasn't triggered by a specific action, describe what you were doing before the problem happened and share more information using the guidelines below. + +### Suggesting Enhancements + +FIRST volunteers are awesome. You all have great ideas and we want to hear them. + +Enhancements should be broadly applicable to a large majority of teams, should not force teams to change their workflow, and should provide real value to the mission of FIRST as it relates to engaging youth in engineering activities. + +The best way to get momentum behind new features is to post a description of your idea in the forums. Build community support for it. The FTC Technology Team monitors the forums. We'll hear you and if there's a large enough call for the feature it's very likely to get put on the list for a future release. diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md new file mode 100644 index 0000000..665369b --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -0,0 +1 @@ +Before issuing a pull request, please see the contributing page. diff --git a/.gitignore b/.gitignore index 4800981..674138c 100644 --- a/.gitignore +++ b/.gitignore @@ -86,9 +86,4 @@ lint/outputs/ lint/tmp/ # lint/reports/ -.DS_Store - -.idea -*ASUS_PC_01* -debug.log VuForiaKey.java diff --git a/.idea/codeStyles/Project.xml b/.idea/codeStyles/Project.xml deleted file mode 100644 index 0d15693..0000000 --- a/.idea/codeStyles/Project.xml +++ /dev/null @@ -1,134 +0,0 @@ - - - - - - - - - - - -

- - - - xmlns:android - - ^$ - - - -
-
- - - - xmlns:.* - - ^$ - - - BY_NAME - -
-
- - - - .*:id - - http://schemas.android.com/apk/res/android - - - -
-
- - - - .*:name - - http://schemas.android.com/apk/res/android - - - -
-
- - - - name - - ^$ - - - -
-
- - - - style - - ^$ - - - -
-
- - - - .* - - ^$ - - - BY_NAME - -
-
- - - - .* - - http://schemas.android.com/apk/res/android - - - ANDROID_ATTRIBUTE_ORDER - -
-
- - - - .* - - .* - - - BY_NAME - -
- - - - - \ No newline at end of file diff --git a/.idea/compiler.xml b/.idea/compiler.xml new file mode 100644 index 0000000..3058433 --- /dev/null +++ b/.idea/compiler.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml index 1286b79..f92b736 100644 --- a/.idea/jarRepositories.xml +++ b/.idea/jarRepositories.xml @@ -26,10 +26,5 @@