+ * 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..8c938a7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StrafeTest.java @@ -0,0 +1,43 @@ +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.InitRobot; +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 { + InitRobot.init(this); + 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..2a51e9b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/StraightTest.java @@ -0,0 +1,43 @@ +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.InitRobot; +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 { + InitRobot.init(this); + 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..ae82812 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackWidthTuner.java @@ -0,0 +1,90 @@ +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.InitRobot; +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()); + + 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 + + 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..1a93436 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TrackingWheelLateralDistanceTuner.java @@ -0,0 +1,143 @@ +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.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 + * 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 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 + * 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 { + InitRobot.init(this); + 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(gamepad1.left_stick_x, gamepad1.left_stick_y, 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.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)).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(); + + 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..5e9dd7b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/TurnTest.java @@ -0,0 +1,29 @@ +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.SampleMecanumDrive; + +/* + * This is a simple routine to test turning capabilities. + */ +@Config +@Autonomous(group = "drive") +public class TurnTest extends LinearOpMode { + public static double ANGLE = Math.PI; // rad + + @Override + public void runOpMode() throws InterruptedException { + InitRobot.init(this); + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + if (isStopRequested()) return; + + drive.turn(ANGLE); + } +} 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..6b049e5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/Autonomous/WhiteLine.java @@ -0,0 +1,53 @@ +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; + +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; + +/* + * This is a simple program to reach the white line + */ +@Config +@Autonomous(name="White Line", group = "drive") +public class WhiteLine extends LinearOpMode { + 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(); + + waitForStart(); + + while (!isStopRequested() && opModeIsActive()) { + telemetry.addData(">", "Press stop"); + 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/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(); + } + } +} 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/Forward.java similarity index 65% 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/Forward.java index 2f49a80..331c3ec 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/Forward.java @@ -18,13 +18,12 @@ associated documentation files (the "Software"), to deal in the Software without 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.InitRobot; import org.firstinspires.ftc.teamcode.API.Robot; -@TeleOp(name = "Mecanum Drive", group = "Linear OpMode") -public class MecanumDrive extends LinearOpMode { +@TeleOp(name = "Forward!", group = "Linear OpMode") +public class Forward extends LinearOpMode { private static final double MAX_SPEED = 0.8; @Override public void runOpMode() { @@ -43,22 +42,11 @@ public void runOpMode() { waitForStart(); while(opModeIsActive()) { - double x1 = -gamepad1.left_stick_x; - double y1 = gamepad1.left_stick_y; - 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); - - Robot.movement.move4x4(flPower, frPower, blPower, brPower); - - telemetry.addData("Back Left", blPower); - telemetry.addData("Back Right", brPower); - telemetry.addData("Front Left", flPower); - telemetry.addData("Front right", frPower); - telemetry.update(); + if (gamepad1.x) { + Robot.movement.move4x4(1, 1, 1, 1); + } else { + Robot.movement.move4x4(0,0,0,0); + } } } -} \ 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 new file mode 100644 index 0000000..0cc3f0e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpMode/TeleOp/TeleOpMain.java @@ -0,0 +1,112 @@ +/* +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.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.Config.Naming; +import org.firstinspires.ftc.teamcode.API.InitRobot; +import org.firstinspires.ftc.teamcode.API.Robot; + +@TeleOp(name = "TeleOp Main", group = "Linear OpMode") +public class TeleOpMain extends LinearOpMode { + private double maxspeed = 0.6; + private boolean brakeSwitch = false; + @Override + public void runOpMode() { + InitRobot.init(this); + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + telemetry.addData(">", "Press start"); + telemetry.update(); + + waitForStart(); + + telemetry.addData(">", "Press stop"); + telemetry.update(); + + waitForStart(); + while(opModeIsActive()) { + double x1 = gamepad1.left_stick_x; + double y1 = gamepad1.left_stick_y; + double rotation = gamepad1.right_stick_x; + + 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(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) { + 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); + } + brakeSwitch = !brakeSwitch; + } + + if (gamepad1.right_bumper) { + maxspeed = 0.8; + } else { + maxspeed = 0.6; + } + + // Moving the wobble + if (gamepad2.x) { + Robot.movement.moveWobble(true); + } 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("Flywheel", Robot.movement.getMotor(Naming.MOTOR_FLYWHEEL).getPower()); + telemetry.update(); + } + } +} 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..8bde9c9 --- /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, "launcher"); + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + telemetry.addData(">", "Press start"); + telemetry.update(); + + waitForStart(); + + telemetry.addData(">", "Press stop"); + telemetry.update(); + + waitForStart(); + while(opModeIsActive()) { + if (gamepad2.left_bumper) { + zeroServo.setPosition(0.7); + } else { + zeroServo.setPosition(1); + } + } + } +} diff --git a/TeamCode/src/main/res/raw/init_config.properties b/TeamCode/src/main/res/raw/init_config.properties deleted file mode 100644 index f231770..0000000 --- a/TeamCode/src/main/res/raw/init_config.properties +++ /dev/null @@ -1,7 +0,0 @@ -# Motors (fl, bl, br, br). If four_wheel_drive is set to anything but "true" (without quotes), then -# you only have to set "bl" and "br". -four_wheel_drive=true -fl=fl -bl=bl -fr=fr -br=br \ No newline at end of file diff --git a/build.common.gradle b/build.common.gradle index f2562d4..0873ade 100644 --- a/build.common.gradle +++ b/build.common.gradle @@ -21,7 +21,7 @@ apply plugin: 'com.android.application' android { - compileSdkVersion 28 + compileSdkVersion 29 signingConfigs { debug { @@ -39,7 +39,7 @@ android { defaultConfig { signingConfig signingConfigs.debug applicationId 'com.qualcomm.ftcrobotcontroller' - minSdkVersion 25 + minSdkVersion 23 targetSdkVersion 28 /** @@ -69,7 +69,6 @@ android { // versionCode vCode versionName vName - multiDexEnabled true } // Advanced user code might just want to use Vuforia directly, so we set up the libs as needed @@ -109,3 +108,4 @@ repositories { } } apply from: 'build.release.gradle' +apply plugin: 'kotlin-android' diff --git a/build.gradle b/build.gradle index 6057daa..9075dde 100644 --- a/build.gradle +++ b/build.gradle @@ -9,7 +9,7 @@ buildscript { jcenter() } dependencies { - classpath 'com.android.tools.build:gradle:4.1.1' + classpath 'com.android.tools.build:gradle:4.1.2' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index c4c3513..2a1e74b 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ -#Thu Oct 01 20:12:15 EDT 2020 +#Fri Jul 24 14:30:03 PDT 2020 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-6.6.1-all.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-6.8.2-all.zip