diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicTeleOp.java deleted file mode 100644 index 8e3052b1..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicTeleOp.java +++ /dev/null @@ -1,132 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.robot.PowerPlayBotV2; -import org.firstinspires.ftc.teamcode.robot.Robot; -import org.firstinspires.ftc.teamcode.robot.hardware.Arm; -import org.firstinspires.ftc.teamcode.util.Debouncer; -import org.firstinspires.ftc.teamcode.util.Logger; -import org.firstinspires.ftc.teamcode.util.Toggle; - -@TeleOp(name = "TeleOp") -public class BasicTeleOp extends LinearOpMode { - Logger logger = new Logger(telemetry); - - Toggle manualOverride = new Toggle(); - Toggle liftHold = new Toggle(); - int liftHoldValue = -500; - Debouncer aButton = new Debouncer(); - Debouncer bButton = new Debouncer(); - - @Override - public void runOpMode() { - PowerPlayBotV2 robot = new PowerPlayBotV2(hardwareMap, logger); - robot.initHardware(); - - robot.arm.lift.setPreset(Arm.Lift.Preset.DRIVING); - robot.arm.reacher.setTargetPosition(0); - - waitForStart(); - - while (!isStopRequested()) { - // BEGIN DRIVE - if (gamepad1.dpad_up || gamepad1.dpad_down || gamepad1.dpad_left || gamepad1.dpad_right) { - if (gamepad1.dpad_up) { - robot.drives.get(Robot.DrivePos.FRONT_LEFT).setPower(.1); - robot.drives.get(Robot.DrivePos.FRONT_RIGHT).setPower(.1); - robot.drives.get(Robot.DrivePos.BACK_LEFT).setPower(.1); - robot.drives.get(Robot.DrivePos.BACK_RIGHT).setPower(.1); - } - if (gamepad1.dpad_down) { - robot.drives.get(Robot.DrivePos.FRONT_LEFT).setPower(-.1); - robot.drives.get(Robot.DrivePos.FRONT_RIGHT).setPower(-.1); - robot.drives.get(Robot.DrivePos.BACK_LEFT).setPower(-.1); - robot.drives.get(Robot.DrivePos.BACK_RIGHT).setPower(-.1); - } - if (gamepad1.dpad_left) { - robot.drives.get(Robot.DrivePos.FRONT_LEFT).setPower(-.1); - robot.drives.get(Robot.DrivePos.FRONT_RIGHT).setPower(.1); - robot.drives.get(Robot.DrivePos.BACK_LEFT).setPower(.1); - robot.drives.get(Robot.DrivePos.BACK_RIGHT).setPower(-.1); - } - if (gamepad1.dpad_right) { - robot.drives.get(Robot.DrivePos.FRONT_LEFT).setPower(.1); - robot.drives.get(Robot.DrivePos.FRONT_RIGHT).setPower(-.1); - robot.drives.get(Robot.DrivePos.BACK_LEFT).setPower(-.1); - robot.drives.get(Robot.DrivePos.BACK_RIGHT).setPower(.1); - } - } else { - robot.drives.get(Robot.DrivePos.FRONT_LEFT).setPower((gamepad1.right_trigger - gamepad1.left_trigger + (Math.signum(gamepad1.left_stick_x) * Math.pow(Math.abs(gamepad1.left_stick_x), 1.5)) + gamepad1.right_stick_x)); - robot.drives.get(Robot.DrivePos.FRONT_RIGHT).setPower((gamepad1.right_trigger - gamepad1.left_trigger - (Math.signum(gamepad1.left_stick_x) * Math.pow(Math.abs(gamepad1.left_stick_x), 1.5)) - gamepad1.right_stick_x)); - robot.drives.get(Robot.DrivePos.BACK_LEFT).setPower((gamepad1.right_trigger - gamepad1.left_trigger + (Math.signum(gamepad1.left_stick_x) * Math.pow(Math.abs(gamepad1.left_stick_x), 1.5)) - gamepad1.right_stick_x)); - robot.drives.get(Robot.DrivePos.BACK_RIGHT).setPower((gamepad1.right_trigger - gamepad1.left_trigger - (Math.signum(gamepad1.left_stick_x) * Math.pow(Math.abs(gamepad1.left_stick_x), 1.5)) + gamepad1.right_stick_x)); - } - // END DRIVE - - // BEGIN ARM - - - // Begin Reacher - telemetry.addData("Reach", robot.arm.reacher.getCurrentPosition()); - - if (gamepad2.x) { - robot.arm.reacher.setTargetPosition(0); - } else if (gamepad2.y) { - robot.arm.reacher.setTargetPosition(2058); - } else if (gamepad2.dpad_left) { - robot.arm.reacher.setPower(-.1); - } else if (gamepad2.dpad_right) { - robot.arm.reacher.setPower(.1); - } else { - robot.arm.reacher.setPower(0); - } - // End Reacher - - // Begin Lift - telemetry.addData("Lift", robot.arm.lift.getCurrentPosition()); - - if (bButton.update(gamepad2.b)) { - robot.arm.lift.setTargetPosition(robot.arm.lift.getTargetPosition() + 100); - } - - if (!(gamepad2.right_trigger > 0.5)) { - if (gamepad2.a) { - robot.arm.lift.setPower(0); - } else if (gamepad1.right_bumper) { - robot.arm.lift.setPreset(Arm.Lift.Preset.DRIVING); - } else if (aButton.update(gamepad1.a)) { - robot.arm.pincher.contract(); - robot.arm.lift.setPreset(Arm.Lift.Preset.GRAB); - while (robot.arm.lift.isBusy()) { - telemetry.addData("Lift", robot.arm.lift.getCurrentPosition()); - telemetry.update(); - } - robot.arm.pincher.expand(); - sleep(100); - robot.arm.lift.setPreset(Arm.Lift.Preset.DRIVING); - } else if (gamepad1.x) { - robot.arm.lift.setPreset(Arm.Lift.Preset.LOW_POLE); - } else if (gamepad1.y) { - robot.arm.lift.setPreset(Arm.Lift.Preset.MEDIUM_POLE); - } else if (gamepad1.b) { - robot.arm.lift.setPreset(Arm.Lift.Preset.HIGH_POLE); - } - } - - // Begin Pincher - if (gamepad2.left_bumper || gamepad1.left_bumper) { - robot.arm.pincher.contract(); - } else if (gamepad2.right_bumper) { - robot.arm.pincher.expand(); - } - // End Pincher - // END ARM - - telemetry.update(); // Update telemetry - - } - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Shared/Navigator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Shared/Navigator.java deleted file mode 100644 index 8d6960a2..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Shared/Navigator.java +++ /dev/null @@ -1,640 +0,0 @@ -package org.firstinspires.ftc.teamcode.Shared; - -import android.util.Log; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.util.ElapsedTime; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; -import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; -import org.firstinspires.ftc.robotcore.external.navigation.Orientation; -import org.firstinspires.ftc.teamcode.robot.Robot; -import org.firstinspires.ftc.teamcode.util.Logger; - -import java.util.HashMap; - -@Config -public class Navigator { - // Used to when calling Android's Log class - String TAG = "Drive"; - - /// These are fetched from the Robot - // TODO: Use the Robot class directly (maybe refactor to be more concise) - public final org.firstinspires.ftc.teamcode.robot.hardware.Drive leftFrontDrive, rightFrontDrive, leftBackDrive, rightBackDrive; - - /// This is only used in order to call isStopRequested() during the navigationMonitorTicks loop, and for the telemetry - LinearOpMode opMode; - // TODO: This should replaced with a Logger - Telemetry telemetry; - Logger logger = new Logger(); - - // TODO: Replace this with a local timer variable - private final ElapsedTime runtime = new ElapsedTime(); - - // These values should be replaced in the constructor - final double COUNTS_PER_MOTOR_REV; /// Ticks output by the motor encoder for each revolution of the motor shaft - final double DRIVE_GEAR_REDUCTION; /// Modifier, less than 1 if geared UP - final double WHEEL_DIAMETER_INCHES; /// Self-explanatory - final double COUNTS_PER_INCH; /// Translates encoder ticks to inches - - /// These multipliers are used to scale the power of the motors based on the overall power - HashMap motorPowerFactors; - - /// The robot class, contains all the hardware - Robot robot; - - // This is used to stop the robot from a different thread, set in ceaseMotion and used in the navigationMonitorTicks loop - boolean mIsStopped = false; - - final double SPEEDSCALE = 50;//Speed in inches/second at speed=1 - - public Navigator(Robot robot, LinearOpMode opMode){ - this.robot = robot; - this.opMode = opMode; - this.telemetry = opMode.telemetry; - this.logger = new Logger(opMode.telemetry); - - this.leftFrontDrive = robot.drives.get(Robot.DrivePos.FRONT_LEFT); - this.rightFrontDrive = robot.drives.get(Robot.DrivePos.FRONT_RIGHT); - this.leftBackDrive = robot.drives.get(Robot.DrivePos.BACK_LEFT); - this.rightBackDrive = robot.drives.get(Robot.DrivePos.BACK_RIGHT); - - // Replace default values with actual values - COUNTS_PER_MOTOR_REV = robot.REV_COUNTS; - DRIVE_GEAR_REDUCTION = robot.GEAR_REDUCTION; - WHEEL_DIAMETER_INCHES = robot.WHEEL_DIAMETER; - COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * Math.PI); - - motorPowerFactors = new HashMap<>(); - setTargetAngle(0); - } - - /**navigationByPhi - * - * @param targetSpeed Desired Speed - * @param targetTheta Desired Orientation of Robot - * - * Utilizes orientation away from the desired location(phi) in order to alter power power facotrs on the motors. - */ - public void navigationByPhi(double targetSpeed, double targetTheta) { - double rightFrontPowerFactor, leftFrontPowerFactor, rightBackPowerFactor, leftBackPowerFactor; - double pi = Math.PI; - - // Send telemetry message to indicate successful Encoder reset - telemetry.addData("Path", "Starting at %7d : %7d", - leftFrontDrive.getCurrentPosition(), - rightFrontDrive.getCurrentPosition()); - telemetry.update(); - - // reset the timeout time and start motion. - runtime.reset(); - - if(targetTheta > 0 && targetTheta < pi/2){ - rightFrontPowerFactor = -Math.cos(2 * targetTheta); - }else if(targetTheta >= -pi && targetTheta < -pi/2){ - rightFrontPowerFactor = Math.cos(2 * targetTheta); - }else if(targetTheta >= pi/2 && targetTheta <= pi){ - rightFrontPowerFactor = 1; - }else{ - rightFrontPowerFactor = -1; - } - - if(targetTheta > 0 && targetTheta < pi/2) { - leftBackPowerFactor = -Math.cos(2 * targetTheta); - }else if(targetTheta >= -pi && targetTheta < -pi/2){ - leftBackPowerFactor = Math.cos(2 * targetTheta); - }else if(targetTheta >= pi/2 && targetTheta <= pi){ - leftBackPowerFactor = 1; - }else{ - leftBackPowerFactor = -1; - } - - if(targetTheta > -pi/2 && targetTheta < 0) { - rightBackPowerFactor = Math.cos(2 * targetTheta); - }else if(targetTheta > pi/2 && targetTheta < pi){ - rightBackPowerFactor = -Math.cos(2 * targetTheta); - }else if(targetTheta >= 0 && targetTheta <= pi/2){ - rightBackPowerFactor = 1; - }else{ - rightBackPowerFactor = -1; - } - - if(targetTheta > -pi/2 && targetTheta < 0) { - leftFrontPowerFactor = Math.cos(2 * targetTheta); - }else if(targetTheta > pi/2 && targetTheta < pi){ - leftFrontPowerFactor = -Math.cos(2 * targetTheta); - }else if(targetTheta >= 0 && targetTheta <= pi/2){ - leftFrontPowerFactor = 1; - }else{ - leftFrontPowerFactor = -1; - } - - motorPowerFactors.put(leftFrontDrive, leftFrontPowerFactor); - motorPowerFactors.put(leftBackDrive, leftBackPowerFactor); - motorPowerFactors.put(rightFrontDrive, rightFrontPowerFactor); - motorPowerFactors.put(rightBackDrive, rightBackPowerFactor); - - SpeedsPhi speedsPhi = getPhiSpeeds(targetSpeed, motorPowerFactors); - setMotorPowersPhi(speedsPhi); - } - - public void navigationMonitorTicksAndCeaseMotion(double speed, double xInches, double yInches, double timeout) { - navigationMonitorTicks(speed, xInches, yInches, timeout); - ceaseMotion(); - } - - - /** - * This is the original navigation function, without anything fancy. - * @param speed inches per second - * @param xInches inches left/right - * @param yInches inches forward/backwards - * @param timeout seconds - */ - public void navigationMonitorTicks(double speed, double xInches, double yInches, double timeout) { - navigationMonitorTicksPhi(speed, xInches, yInches, 0, timeout); - } - - /** - * This is a slightly different version that adds phi rotation to the mix. - * Please don't try to move x/y *and* rotate just yet... - * @param speed inches per second - * @param xInches inches left/right - * @param yInches inches forward/backwards - * @param phi degrees heading (relative) - * @param timout seconds - */ - public void navigationMonitorTicksPhi(double speed, double xInches, double yInches, double phi, double timout) { - internalNavigationLoop(speed, xInches, yInches, phi, timout, false, true); - } - - public static int ROT_TIMEOUT = 2; - - public void navigationMonitorTurn(double phi) { - // We only want to turn, but due to a bug in the navigation loop, it will not work if both x/y are 0. - // So we'll set x to 1, and the speed to 0 so that it doesn't actually move. - // monitorTotalTravel is set to false so that the loop is not blocked until the correct number of ticks are reached (which will never happen) - internalNavigationLoop(0, 999, 0, phi, ROT_TIMEOUT, false, true); - } - - public void navigationMonitorExternal(double inchesPerSecond, double xInches, double yInches, double phi, double timeoutSec, boolean isMonitorAcceleration) { - internalNavigationLoop(inchesPerSecond, xInches, yInches, phi, timeoutSec, isMonitorAcceleration, true); - } - - /** - * INTERNAL USE ONLY. Takes care of the navigation loop, lots of parameters that should be engaged with other functions. - */ - private void internalNavigationLoop(double inchesPerSecond, double xInches, double yInches, double phi, double timeoutSec, boolean isMonitorAcceleration, boolean monitorTotalTravel) { - //Borrowed Holonomic robot navigation ideas from https://www.bridgefusion.com/blog/2019/4/10/robot-localization-dead-reckoning-in-f irst-tech-challenge-ftc - // Robot Localization -- Dead Reckoning in First Tech Challenge (FTC) - Log.i("start", "#$#$#$#$#$#$#$#$#$"); - double theta = Math.atan2(yInches, xInches); - double magnitude = Math.hypot(xInches, yInches); - int tickCountPriorLeftFront = leftFrontDrive.getCurrentPosition(), tickCountPriorLeftBack = leftBackDrive.getCurrentPosition(); - int tickCountPriorRightFront = rightFrontDrive.getCurrentPosition(), tickCountPriorRightBack = rightBackDrive.getCurrentPosition(); - int ticksTraveledLeftFront = 0, ticksTraveledLeftBack = 0, ticksTraveledRightFront = 0, ticksTraveledRightBack = 0; - double inchesTraveledX = 0, inchesTraveledY = 0, inchesTraveledTotal = 0, rotationInchesTotal = 0; - long cycleMillisNow, cycleMillisPrior = System.currentTimeMillis(), cycleMillisDelta, startMillis = System.currentTimeMillis(); - - // Get the time it is right now, so we can start the timer - //long start = System.currentTimeMillis(); - - //vroom_vroom(speed, theta, speed, theta); - mIsStopped = false; - mTargetAngleErrorSum = 0; - getImuAngle(); - double speed = inchesPerSecond/SPEEDSCALE; - navigationByPhi(speed, theta); - adjustThetaInit(); - //setTargetAngle(mImuCalibrationAngle); - - // We fudge the angle a little, because something is wrong... - // TODO: Investigate this fudge - double angle_fudged = phi - (phi * (1.5 / 90.0)); - setTargetAngle(angle_fudged); - - int settleCounter = 0; - - FtcDashboard dashboard = FtcDashboard.getInstance(); - - // Stay in the main loop while: - while (opMode.opModeIsActive() // The opmode is still active - && System.currentTimeMillis() < startMillis + (1000 * timeoutSec) // We haven't timed out - && (monitorTotalTravel && (inchesTraveledTotal < magnitude)) // We are monitoring total travel, and we haven't reached the target yet - //|| (settleCounter < 10) // We are waiting for the robot to settle - && !mIsStopped // We haven't been told to stop (by another thread) - && !shouldStopIfApplicable(isMonitorAcceleration, startMillis) // We haven't been told to stop (by the acceleration monitor) - ) { - /*TelemetryPacket packet = new TelemetryPacket(); - packet.put("Target", mTargetAngle); - packet.put("IMU Angle", getImuAngle()); - packet.put("Error", mTargetAngle - getImuAngle()); - packet.put("Euler Error", getEulerAngleDegrees(mTargetAngle - getImuAngle())); - dashboard.sendTelemetryPacket(packet); - - if (Math.abs(getEulerAngleDegrees(mTargetAngle - getAdjustedAngle())) < 1) { - // If we're really close to the target, increase the settle counter - settleCounter++; - } else { - // If we're not close, reset the settle counter - settleCounter = 0; - }*/ - - - - double TotalMotorCurrent = leftFrontDrive.getCurrent(); - TotalMotorCurrent += leftBackDrive.getCurrent(); - TotalMotorCurrent += rightFrontDrive.getCurrent(); - TotalMotorCurrent += rightBackDrive.getCurrent(); - Log.i(TAG,"navigationMonitorTicks: Total Motor Current= "+ TotalMotorCurrent); - - - int tickCountNowLeftFront = leftFrontDrive.getCurrentPosition(); - int tickCountNowLeftBack = leftBackDrive.getCurrentPosition(); - int tickCountNowRightFront = rightFrontDrive.getCurrentPosition(); - int tickCountNowRightBack = rightBackDrive.getCurrentPosition(); - int deltaTicksLeftFront = tickCountNowLeftFront - tickCountPriorLeftFront; - int deltaTicksLeftBack = tickCountNowLeftBack - tickCountPriorLeftBack; - int deltaTicksRightFront = tickCountNowRightFront - tickCountPriorRightFront; - int deltaTicksRightBack = tickCountNowRightBack - tickCountPriorRightBack; - ticksTraveledLeftFront += deltaTicksLeftFront; - ticksTraveledLeftBack += deltaTicksLeftBack; - ticksTraveledRightFront += deltaTicksRightFront; - ticksTraveledRightBack += deltaTicksRightBack; - double leftFrontInchesDelta = deltaTicksLeftFront / COUNTS_PER_INCH; - double rightFrontInchesDelta = -deltaTicksRightFront / COUNTS_PER_INCH; //Minus sign converts to holonomic drive perspective - double rightBackInchesDelta = -deltaTicksRightBack / COUNTS_PER_INCH; //Minus sign converts to holonomic drive perspective - double leftBackInchesDelta = deltaTicksLeftBack / COUNTS_PER_INCH; - double rotationAvgInchesDelta = (leftFrontInchesDelta + rightFrontInchesDelta + rightBackInchesDelta + leftBackInchesDelta)/4; - rotationInchesTotal += rotationAvgInchesDelta; - double leftFrontRobotInchesDelta = leftFrontInchesDelta - rotationAvgInchesDelta; - double rightFrontRobotInchesDelta = rightFrontInchesDelta - rotationAvgInchesDelta; - double rightBackRobotInchesDelta = rightBackInchesDelta - rotationAvgInchesDelta; - double leftBackRobotInchesDelta = leftBackInchesDelta - rotationAvgInchesDelta; - double deltaInchesRobotX = (leftFrontRobotInchesDelta + rightFrontRobotInchesDelta - rightBackRobotInchesDelta - leftBackRobotInchesDelta) / (2 * Math.sqrt(2)); - double deltaInchesRobotY = (leftFrontRobotInchesDelta - rightFrontRobotInchesDelta - rightBackRobotInchesDelta + leftBackRobotInchesDelta) / (2 * Math.sqrt(2)); - double deltaInchesRobot = Math.hypot(deltaInchesRobotX, deltaInchesRobotY); - double FUDGE_FACTOR = ((36/51.0)*(38/32.0)); - inchesTraveledX += deltaInchesRobotX * FUDGE_FACTOR; - inchesTraveledY += deltaInchesRobotY * FUDGE_FACTOR; - inchesTraveledTotal += Math.hypot(deltaInchesRobotX * FUDGE_FACTOR, deltaInchesRobotY * FUDGE_FACTOR); - - cycleMillisNow = System.currentTimeMillis(); - cycleMillisDelta = cycleMillisNow - cycleMillisPrior; - cycleMillisPrior = cycleMillisNow; - - -//Working on Changing Speed to be consistent - - if(runtime.seconds()>.05 && inchesTraveledTotal <= magnitude) { - // timePassed = opMode.getRuntime() - initialTime;//Length of time of the loop - double actualSpeedX = 1000 * (deltaInchesRobotX / cycleMillisDelta);//gets the actual speed in inches/second in x direction - double actualSpeedY = 1000 * (deltaInchesRobotY / cycleMillisDelta);//gets the actual speed in inches/second in y direction - //SPEEDSCALE = 24.0;//Speed in inches/second at speed=1 - //speedScaled = speed * SPEEDSCALE;//Speed in terms of inches/second instead of 0 to 1 - - - double actualSpeed = Math.sqrt((actualSpeedX * actualSpeedX) + (actualSpeedY * actualSpeedY));//Take hypotenuse of speed in x and y - Log.i("Speed", "........................................................................................."); - Log.i("Speed", String.format("InchesDeltaX, %.3f, InchesDeltaY, %.3f", deltaInchesRobotX, deltaInchesRobotY)); - - Log.i("Speed", String.format("Actual Speed in/s:, %.2f", actualSpeed));//log actual speed - - Log.i("Speed", String.format("SpeedScale:, %.1f", SPEEDSCALE)); - Log.i("Speed", String.format("Target Speed in/s:, %.1f", inchesPerSecond)); - // Log.i("Speed", String.format("SpeedScaled in/s:, %.2f", speedScaled));//log desired speed scaled should be input speed*12 - - double speedError = (inchesPerSecond - actualSpeed);//error in speed in/s - Log.i("Speed", String.format("SpeedError in/s:, %.2f", speedError)); - double speedGain = 0.005*.1; - if (speedError >= 1.25 * inchesPerSecond) { - speedError = 1.25 * inchesPerSecond; - } else if (speedError <= -1.25 * inchesPerSecond) { - speedError = -1.25 * inchesPerSecond; - } - speed = speed + speedGain * speedError; - //Log.i("Speed", String.format("speedFactor in/s:, %.2f", speedFactor)); - // speedScaled *= (1 + speedFactor);//corrects adjusted speed with percent error - - //Log.i("Speed", String.format("New Speed in/s:, %.2f", speed));//log actual speed - // speed = speedScaled / SPEEDSCALE;//converts adjusted speed to 0 to 1 scale - Log.i("Speed", String.format("New Speed from 0 to 1:, %.2f", speed));//log actual speed - Log.i("Speed", "........................................................................................."); - //Provide feedback to keep robot moving in right direction based on encoder ticks - - - adjustTheta(xInches, yInches, speed, inchesTraveledX, inchesTraveledY); - } - - //Must call adjustThetaInit() before a loop with adjustTheta() - - - - telemetry.addData("Ticks Traveled (lf, lb)", "%7d, %7d", ticksTraveledLeftFront, ticksTraveledLeftBack); - telemetry.addData("Ticks Traveled (rf, rb)", "%7d, %7d", ticksTraveledRightFront, ticksTraveledRightBack); - telemetry.addData("In Traveled (X, Y)", "X: %.1f, Y: %.1f", inchesTraveledX, inchesTraveledY); - telemetry.addData("In Traveled (Tot, Rot)", "%.1f, %.1f", inchesTraveledTotal,rotationInchesTotal); - telemetry.addData("Cycle Millis:", "%d", cycleMillisDelta); - telemetry.update(); - Log.i("Drive", String.format("Ticks Traveled (lf, lb): %7d, %7d", ticksTraveledLeftFront, ticksTraveledLeftBack)); - Log.i("Drive", String.format("Ticks Traveled (rf, rb): %7d, %7d", ticksTraveledRightFront, ticksTraveledRightBack)); - Log.i("Drive", String.format("Ticks Delta (lf, lb): %7d, %7d", deltaTicksLeftFront, deltaTicksLeftBack)); - Log.i("Drive", String.format("Ticks Delta (rf, rb): %7d, %7d", deltaTicksRightFront, deltaTicksRightBack)); - Log.i("Drive", String.format("In Traveled (X, Y): X: %.2f, Y: %.2f", inchesTraveledX, inchesTraveledY)); - Log.i("Drive", String.format("In Traveled (Tot, Rot): %.2f, %.2f", inchesTraveledTotal,rotationInchesTotal)); - Log.i("Drive", String.format("Incremental Speed (in/sec): %.2f", deltaInchesRobot/cycleMillisDelta * 1000)); - Log.i("Drive", String.format("Cycle Millis: %d, Total Seconds: %d", cycleMillisDelta, (System.currentTimeMillis() - startMillis)/1000)); - - tickCountPriorLeftFront = tickCountNowLeftFront; - tickCountPriorLeftBack = tickCountNowLeftBack; - tickCountPriorRightFront = tickCountNowRightFront; - tickCountPriorRightBack = tickCountNowRightBack; - } - } - - /**ceaseMotion - *Stop all motion; - */ - public void ceaseMotion(){ - leftFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - mIsStopped = true; - opMode.sleep(30); - leftFrontDrive.setPower(0); - leftBackDrive.setPower(0); - rightFrontDrive.setPower(0); - rightBackDrive.setPower(0); - } - - /**setMotorPowersPhi - *Set power factors of all drive motors using speedsPhi - */ - public void setMotorPowersPhi(SpeedsPhi speedsPhi){ - leftFrontDrive.setPower(speedsPhi.leftFrontSpeed); - rightFrontDrive.setPower(speedsPhi.rightFrontSpeed); - leftBackDrive.setPower(speedsPhi.leftBackSpeed); - rightBackDrive.setPower(speedsPhi.rightBackSpeed); - } - - private double thetaErrorSum; - public void adjustThetaInit() { thetaErrorSum = 0; } - public void adjustTheta(double targetX, double targetY, double targetSpeed, double nowX, double nowY){ - // TODO: Why is this testing the target angle - if(nowX == 0 && nowY == 0 && mTargetAngle == 0){ - Log.i("Drive", "adjustTheta: nowX and nowY are both still zero so not computing an adjustment factor yet"); - return; - } - //double GAIN = 0.6, THETA_ERROR_SUM_MAX = Math.PI/4/GAIN; //Max error sum is +/- 45 degrees - double targetTheta = Math.atan2(targetY, targetX); - double nowTheta = Math.atan2(nowY, nowX); -// if((targetTheta - nowTheta > 0 && thetaErrorSum < THETA_ERROR_SUM_MAX) || (targetTheta - nowTheta < 0 && thetaErrorSum > -THETA_ERROR_SUM_MAX)) -// thetaErrorSum += Math.min(Math.max(targetTheta - nowTheta, -THETA_ERROR_SUM_MAX), THETA_ERROR_SUM_MAX); //Max allowed increment is max value -// double adjustedTargetTheta = targetTheta + GAIN * thetaErrorSum; - - //The cosine function acts as the gain so that as the error angle approaches 180 degrees, the error factor goes to zero, - // since the motors will already be pulling in the opposite direction. - double angleDifference = calculateAngleDifference(targetTheta, nowTheta); - double adjustedTargetTheta = getEulerAngle(targetTheta + Math.cos(angleDifference/2) * angleDifference); - //Speeds speeds = getSpeeds(targetSpeed, nowTheta); -// vroom_vroom(targetSpeed, adjustedTargetTheta, targetSpeed, adjustedTargetTheta); - //vroom_vroom(speeds.rightSpeed, adjustedTargetTheta, speeds.leftSpeed, adjustedTargetTheta); - navigationByPhi(targetSpeed, adjustedTargetTheta); -// vroom_vroom(targetSpeed, targetTheta, targetSpeed, targetTheta); - Log.i("Drive", String.format("IMU angle: %.2f, adj angle: %.2f", mCurrentImuAngle, mAdjustedAngle)); - Log.i("Drive", String.format("adjustTheta: (degrees) target: %.4f, now: %.4f, adjusted: %.4f, error: %.4f", - targetTheta/Math.PI*180, nowTheta/Math.PI*180, adjustedTargetTheta/Math.PI*180, thetaErrorSum/Math.PI*180)); - } - - double mCurrentImuAngle; - double mPriorImuAngle; - double mTargetAngle; - double mAdjustedAngle; - double mPriorAdjustedAngle; - double mTargetAngleErrorSum; - - public void setTargetAngle(double targetAngle){ - mPriorImuAngle = mTargetAngle = targetAngle; - } - - /** - *We experimentally determined the Z axis is the axis we want to use for heading angle. - *We have to process the angle because the imu works in euler angles so the Z axis is - *returned as 0 to +180 or 0 to -180 rolling back to -179 or +179 when rotation passes - *180 degrees. We detect this transition and track the total cumulative angle of rotation. - * @return 0 - */ - public double getImuAngle(){ - Orientation angles = robot.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZXY, AngleUnit.DEGREES); - telemetry.addData("IMU Angles (X/Y/Z)", "%.1f / %.1f / %.1f", angles.secondAngle, angles.thirdAngle, angles.firstAngle); - telemetry.update(); - Log.i(TAG, String.format("getImuAngle: IMU Angles (X/Y/Z): %.1f / %.1f / %.1f", angles.secondAngle, angles.thirdAngle, angles.firstAngle)); - return mCurrentImuAngle = angles.firstAngle; - - } - - /** - * - * @return New adjusted angle used to create adjusted power levels - */ - public double getAdjustedAngle(){ - return mAdjustedAngle = getEulerAngleDegrees(getImuAngle()); - } - - /** - * Convert an angle such that -pi <= angle <= pi - */ - private double getEulerAngle(double angle){ - double modAngle = angle%(2*Math.PI); - if(modAngle < -Math.PI) return modAngle + 2 * Math.PI; - else if (modAngle > Math.PI) return modAngle - 2 * Math.PI; - else return modAngle; - //if(angle < -Math.PI) return angle%(2*Math.PI) + 2 * Math.PI; - //else if (angle > Math.PI) return angle%(2*Math.PI) - 2 * Math.PI; - //else return angle; - } - - /** - * Convert an angle such that -180 <= angle <= 180 - */ - private double getEulerAngleDegrees(double angle){ - double modAngle = angle%360; - if(modAngle < -180) return modAngle + 360; - else if (modAngle > 180) return modAngle - 360; - else return modAngle; - } - - /** - * See if we are moving in a straight line and if not return a power correction value. - * @return Power adjustment, + is adjust left - is adjust right. - */ - private double getPowerCorrection() - { - // The gain value determines how sensitive the correction is to direction changes. - // You will have to experiment with your robot to get small smooth direction changes - // to stay on a straight line. - double angleError, powerCorrection, angle, pGain, iGain; - - angle = getAdjustedAngle(); //IMU angle converted to Euler angle (IMU may already deliver Euler angles) - angleError = getEulerAngleDegrees(mTargetAngle - angle); // reverse sign of angle for correction. - mTargetAngleErrorSum += angleError; - int MAX_ERROR_ANGLE_SUM = 3; - if(mTargetAngleErrorSum > MAX_ERROR_ANGLE_SUM) - mTargetAngleErrorSum = MAX_ERROR_ANGLE_SUM; - else if(mTargetAngleErrorSum < -MAX_ERROR_ANGLE_SUM) - mTargetAngleErrorSum = -MAX_ERROR_ANGLE_SUM; - Log.i(TAG, String.format("getPowerCorrection: targetAngle: %.2f, imuAngle: %.2f, diffAngle: %.2f, diffAngleSum: %.2f", mTargetAngle, angle, angleError, mTargetAngleErrorSum)); - - //gain = Math.max(-0.05*Math.abs(angleError) + 0.1, .05); //Varies from .2 around zero to .05 for errors above 10 degrees - //pGain = Math.max(-0.05*Math.abs(angleError) + 0.1, .05)/3; //Varies from .2 around zero to .05 for errors above 10 degrees - pGain = 0.06 / 8.0; - iGain = 0.04 / 8.0; - - // If the angleError is really big, go open-loop for a bit to speed up the turn - //if (Math.abs(angleError) > 20) { - // pGain += 1000; - //} - - powerCorrection = angleError * pGain + mTargetAngleErrorSum * iGain; - - android.util.Log.d("angleError", String.valueOf(angleError)); - android.util.Log.d("mTargetAngleErrorSum", String.valueOf(mTargetAngleErrorSum)); - android.util.Log.d("mTargetAngle", String.valueOf(mTargetAngle)); - - /*telemetry.addData("angleError", angleError); - telemetry.addData("mTargetAngleErrorSum", mTargetAngleErrorSum); - telemetry.addData("mTargetAngle", mTargetAngle);*/ - - //logger.debug("power correction: " + powerCorrection); - - return powerCorrection; - } - - private static class SpeedsPhi{ - public double rightFrontSpeed; - public double leftFrontSpeed; - public double rightBackSpeed; - public double leftBackSpeed; - public SpeedsPhi(double leftFront, double leftBack, double rightFront, double rightBack){ - leftFrontSpeed = leftFront; - leftBackSpeed = leftBack; - rightFrontSpeed = rightFront; - rightBackSpeed = rightBack; - } - } - - /** - * Get the adjusted speeds for each side of the robot to allow it to turn enough to stay on a straight line. Only call once per cycle. - */ - private SpeedsPhi getPhiSpeeds(double targetSpeed, HashMap motorPowerFactors){ - double powerCorrection = getPowerCorrection(); - double adjustedLeftFrontSpeed, adjustedLeftBackSpeed, adjustedRightFrontSpeed, adjustedRightBackSpeed; - - if(motorPowerFactors.get(leftFrontDrive)*targetSpeed - powerCorrection > 1){ - adjustedLeftFrontSpeed = 1; - }else if(motorPowerFactors.get(leftFrontDrive)*targetSpeed - powerCorrection < -1) { - adjustedLeftFrontSpeed = -1; - }else{ - adjustedLeftFrontSpeed = motorPowerFactors.get(leftFrontDrive) * targetSpeed - powerCorrection; - } - - if(motorPowerFactors.get(leftBackDrive)*targetSpeed - powerCorrection > 1){ - adjustedLeftBackSpeed = 1; - }else if(motorPowerFactors.get(leftBackDrive)*targetSpeed - powerCorrection < -1) { - adjustedLeftBackSpeed = -1; - }else{ - adjustedLeftBackSpeed = motorPowerFactors.get(leftBackDrive) * targetSpeed - powerCorrection; - } - - if(motorPowerFactors.get(rightFrontDrive)*targetSpeed + powerCorrection > 1){ - adjustedRightFrontSpeed = 1; - }else if(motorPowerFactors.get(rightFrontDrive)*targetSpeed + powerCorrection < -1) { - adjustedRightFrontSpeed = -1; - }else{ - adjustedRightFrontSpeed = motorPowerFactors.get(rightFrontDrive) * targetSpeed + powerCorrection; - } - - if(motorPowerFactors.get(rightBackDrive)*targetSpeed + powerCorrection > 1){ - adjustedRightBackSpeed = 1; - }else if(motorPowerFactors.get(rightBackDrive)*targetSpeed + powerCorrection < -1) { - adjustedRightBackSpeed = -1; - }else{ - adjustedRightBackSpeed = motorPowerFactors.get(rightBackDrive) * targetSpeed + powerCorrection; - } - - mPriorImuAngle = mCurrentImuAngle; - mPriorAdjustedAngle = mAdjustedAngle; - Log.i(TAG, String.format("getSpeedsPhi: targetSpeed: %.3f, powerCorrection: %.3f", targetSpeed, powerCorrection)); - Log.i(TAG, String.format("getSpeedsPhi: adjustedLeftFrontSpeed: %.3f, adjustedLeftBackSpeed: %.3f, adjustedRightFrontSpeed: %.3f, adjustedRightBackSpeed: %.3f", - adjustedLeftFrontSpeed, adjustedLeftBackSpeed, adjustedRightFrontSpeed, adjustedRightBackSpeed)); - Log.i(TAG, String.format("getSpeedsPhi: leftFrontMotorPowerFactor: %.3f, leftBackMotorPowerFactor: %.3f, rightFrontMotorPowerFactor: %.3f, rightBackMotorPowerFactor: %.3f", - motorPowerFactors.get(leftFrontDrive), motorPowerFactors.get(leftBackDrive), motorPowerFactors.get(rightFrontDrive), motorPowerFactors.get(rightBackDrive))); - return new SpeedsPhi(adjustedLeftFrontSpeed, adjustedLeftBackSpeed, adjustedRightFrontSpeed, adjustedRightBackSpeed); - } - - - /** - * @return Difference in target angle and nowAngle - */ - private double calculateAngleDifference(double targetAngle, double nowAngle){ - double angle1, angle2, angleDiff180 = 0, angleDiff0 = 0, angleDifference; - if(targetAngle >= 0 && nowAngle <= 0){ - angle1 = Math.PI - targetAngle; - angle2 = nowAngle + Math.PI; - angleDiff180 = angle1 + angle2; - angleDiff0 = targetAngle - nowAngle; - angleDifference = Math.min(angleDiff180, angleDiff0); - if(angleDiff180 < angleDiff0) - angleDifference *= -1; - }else if(targetAngle <= 0 && nowAngle >= 0){ - angle1 = Math.PI - nowAngle; - angle2 = targetAngle + Math.PI; - angleDiff180 = angle1 + angle2; - angleDiff0 = nowAngle - targetAngle; - angleDifference = Math.min(angleDiff180, angleDiff0); - if(angleDiff0 < angleDiff180) - angleDifference *= -1; - }else{ - // for all cases that target & now have the same sign - angleDifference = targetAngle - nowAngle; - } - Log.i(TAG, String.format("calculateAngleDifference: angleDiff0: %.2f, angleDiff180: %.2f, angleDifference: %.2f", - angleDiff0, angleDiff180, angleDifference)); - return angleDifference; - } - - private boolean shouldStopIfApplicable(boolean shouldMonitorAcceleration, long startTimeMillis){ - if (shouldMonitorAcceleration) { - //android.util.Log.w("#%#%", "WE GOT HERE"); - - if ((System.currentTimeMillis() - startTimeMillis) < 400) - return false; //Wait 1 sec to allow for initial acceleration - double a = robot.imu.getLinearAcceleration().yAccel; - double current = 0; - for (org.firstinspires.ftc.teamcode.robot.hardware.Drive drive : robot.drives.values()) { - //double c = drive.getCurrent(); - //android.util.Log.w("current", "" + c); - current += drive.getCurrent(); - } - android.util.Log.w("isHighAcceleration", "Y Accel: " + a + " Current: " + current); - /*if (a > 0.5) { - android.util.Log.w("isHighAcceleration", "High acceleration detected... Crashed into carousel?"); - return true; - }*/ - if (current > 8) { - android.util.Log.w("isHighAcceleration", "High current detected... Crashed into carousel?"); - return true; - } - //if(robot.imu.getLinearAcceleration().xAccel > 1.0) - // return true; - } - return false; - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestPipeline.java deleted file mode 100644 index 7ec1c68e..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestPipeline.java +++ /dev/null @@ -1,101 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.Shared.Navigator; -import org.firstinspires.ftc.teamcode.robot.PowerPlayBotV2; -import org.firstinspires.ftc.teamcode.util.Logger; -import org.firstinspires.ftc.teamcode.vision.pole.DoubleThresholdPipeline; - -@TeleOp(name = "Test Pipeline") -public class TestPipeline extends LinearOpMode { - - @Override - public void runOpMode() { - Logger logger = new Logger(telemetry); - PowerPlayBotV2 robot = new PowerPlayBotV2(hardwareMap); - robot.initHardware(); - - Navigator navigator = new Navigator(robot, this); - - DoubleThresholdPipeline pipeline = new DoubleThresholdPipeline(telemetry); - - robot.camera.setPipeline(pipeline); - - robot.camera.open(); - - logger.info("Camera opened!"); - - //robot.arm.lift.setPreset(Arm.Lift.Preset.LOW_POLE); - // Tune for red cones - pipeline.minHue = 174.0; - pipeline.maxHue = 8.0; - pipeline.minBlue = 147.3; - pipeline.maxBlue = 184.2; - - waitForStart(); - - while (opModeIsActive()) { - // Tune for red cones - pipeline.minHue = 174.0; - pipeline.maxHue = 8.0; - pipeline.minBlue = 147.3; - pipeline.maxBlue = 184.2; - - /*while (pipeline.necessaryCorrection() != 0 && opModeIsActive()) { - // Turn until centered - logger.debug("Correction: " + pipeline.necessaryCorrection()); - logger.debug("Current Angle: " + navigator.getAdjustedAngle()); - navigator.navigationMonitorTurn(navigator.getAdjustedAngle() - (0.2 * pipeline.necessaryCorrection())); - //drive.getAdjustedAngle(); - }*/ - } - - - // Turn until centered - - - /*while (opModeInInit()) { - - if (pipeline.necessaryCorrection() > 5) { - // Right - robot.drives.get(Robot.DrivePos.FRONT_LEFT).setPower(.1); - robot.drives.get(Robot.DrivePos.FRONT_RIGHT).setPower(-.1); - robot.drives.get(Robot.DrivePos.BACK_LEFT).setPower(-.1); - robot.drives.get(Robot.DrivePos.BACK_RIGHT).setPower(.1); - //drive.navigationMonitorTicksPhi(5, 1, 0, drive.getAdjustedAngle(), 1); - } else if (pipeline.necessaryCorrection() < -5) { - // Left - robot.drives.get(Robot.DrivePos.FRONT_LEFT).setPower(-.1); - robot.drives.get(Robot.DrivePos.FRONT_RIGHT).setPower(.1); - robot.drives.get(Robot.DrivePos.BACK_LEFT).setPower(.1); - robot.drives.get(Robot.DrivePos.BACK_RIGHT).setPower(-.1); - //drive.navigationMonitorTicksPhi(5, -1, 0, drive.getAdjustedAngle(), 1); - } else { - robot.drives.get(Robot.DrivePos.FRONT_LEFT).setPower(0); - robot.drives.get(Robot.DrivePos.FRONT_RIGHT).setPower(0); - robot.drives.get(Robot.DrivePos.BACK_LEFT).setPower(0); - robot.drives.get(Robot.DrivePos.BACK_RIGHT).setPower(0); - //drive.navigationMonitorTicks(0, 9999, 99999, 1); - //drive.ceaseMotion(); - } - }*/ - /*waitForStart(); - - - while (opModeIsActive()) { - if (pipeline.necessaryCorrection() > 1) { - drive.navigationMonitorTicks(10, -1, 0,1); - } else if (pipeline.necessaryCorrection() < 1) { - drive.navigationMonitorTicks(10, 1, 0, 1); - } else { - drive.ceaseMotion(); - } - telemetry.addData("Correction", pipeline.necessaryCorrection()); - telemetry.update(); - // Wait for stop to be pressed - }*/ - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AllAutomovement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AllAutomovement.java deleted file mode 100644 index 30e052a7..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AllAutomovement.java +++ /dev/null @@ -1,446 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -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.Range; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.Shared.Navigator; -import org.firstinspires.ftc.teamcode.robot.PowerPlayBotV2; -import org.firstinspires.ftc.teamcode.robot.Robot; -import org.firstinspires.ftc.teamcode.robot.hardware.Arm; -import org.firstinspires.ftc.teamcode.robot.hardware.Drive; -import org.firstinspires.ftc.teamcode.util.Logger; -//import org.firstinspires.ftc.teamcode.vision.TensorflowStandardSleeve; -import org.firstinspires.ftc.teamcode.util.PIController; -import org.firstinspires.ftc.teamcode.util.Timeout; -import org.firstinspires.ftc.teamcode.vision.AprilTagPipeline; -import org.firstinspires.ftc.teamcode.vision.pole.DoubleThresholdPipeline; - -@Config -//@Autonomous(name = "Autonomous", preselectTeleOp = "TeleOp") -public class AllAutomovement { - private LinearOpMode opMode; - private Logger logger = new Logger(); - - public AllAutomovement(LinearOpMode opMode) { - this.opMode = opMode; - this.logger = new Logger(opMode.telemetry, true); - - - } - //private final Logger logger = new Logger(opMode.telemetry, true); - - private PowerPlayBotV2 robot; - private Navigator navigator; - - private AprilTagPipeline initializeAprilTag() { - AprilTagPipeline pipeline = new AprilTagPipeline(); - robot.camera.setPipeline(pipeline); - if (!robot.camera.opened) { - robot.camera.open(new Timeout(10)); - } else { - robot.camera.resume(); - } - return pipeline; - } - - private DoubleThresholdPipeline initializePoleDetection() { - DoubleThresholdPipeline pipeline = new DoubleThresholdPipeline(opMode.telemetry); - - // Tune the pipeline for yellow poles - pipeline.minHue = 16.4; - pipeline.maxHue = 36.6; - pipeline.minBlue = 167.4; - pipeline.maxBlue = 208.4; - // Offset for center of robot - pipeline.targetX = 160; - - return pipeline; - } - - private int detectTag(AprilTagPipeline pipeline) { - logger.debug("Detecting tag..."); - // Wait for the camera to see a tag - Timeout detectionTimeout = new Timeout(5); - while (pipeline.getIds() == null && opMode.opModeIsActive() && !detectionTimeout.expired()) { Thread.yield(); } - // null check to prevent crash - int[] ids = pipeline.getIds(); - if (ids == null) { - logger.warning("Could not find an AprilTag, falling back!"); - return 0; - } - return ids[0]; - } - - @Config - public static class RotationPID { - public static double PGAIN = 0.000675; - public static double IGAIN = 0.0; - public static double IWINDUPLIMIT = 100.0; - public static int SETTLE_PX = 8; - public static int TARGET = 420; - public static int FRAME_SLEEP = 80; - } - - @SuppressWarnings("ConstantConditions") - private void localizeOnPole(DoubleThresholdPipeline pipeline) { - PIController poleLocalizationController = new PIController(RotationPID.PGAIN, RotationPID.IGAIN, RotationPID.IWINDUPLIMIT); - FtcDashboard dashboard = FtcDashboard.getInstance(); - - int settleCounter = 0; - while (settleCounter < 3 && (opMode.opModeIsActive()) || opMode.opModeInInit()) { - TelemetryPacket packet = new TelemetryPacket(); - double x = pipeline.getPoleX(); - packet.put("x", x); - if (x == -1.0) { - // NO POLES: STOP RIGHT NOW - for (Drive drive : robot.drives.values()) { - drive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - drive.setPower(0); - } - dashboard.sendTelemetryPacket(packet); - opMode.sleep(RotationPID.FRAME_SLEEP); - continue; - } - double correction = RotationPID.TARGET - x; - packet.put("correction", correction); - //logger.debug("px Correction: " + correctionPx); - - if (Math.abs(correction) < RotationPID.SETTLE_PX) { - // If we're really close to the target, increase the settle counter - settleCounter++; - packet.put("settle", settleCounter); - } else { - // If we're not close, reset the settle counter - settleCounter = 0; - } - - double correctionPower = poleLocalizationController.update(correction); - packet.put("power", correctionPower); - - dashboard.sendTelemetryPacket(packet); - - //logger.debug("Power Correction: " + correctionPower); - - robot.drives.get(Robot.DrivePos.FRONT_LEFT).setPower(-correctionPower); - robot.drives.get(Robot.DrivePos.BACK_LEFT).setPower(-correctionPower); - robot.drives.get(Robot.DrivePos.FRONT_RIGHT).setPower(correctionPower); - robot.drives.get(Robot.DrivePos.BACK_RIGHT).setPower(correctionPower); - - opMode.sleep(RotationPID.FRAME_SLEEP); - } - - // Stop the wheels when we're done (just in case) - for (Drive drive : robot.drives.values()) { - drive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - drive.setPower(0); - } - } - - @Config - public static class DistancePID { - public static double PGAIN = 0.01; - public static double IGAIN = 0; - public static double IWINDUP_LIMIT = 100.0; - public static double SETTLE_LIMIT = 0.01; - public static int TARGET = 56; - public static int FRAME_SLEEP = 80; - public static double SAFETY_MAX = 1.0; - public static double MIN_WIDTH = 15.0; - public static double CUTOFF = 300; - } - - @SuppressWarnings("ConstantConditions") - private void localizeDistance(DoubleThresholdPipeline pipeline) { - PIController distanceController = new PIController(DistancePID.PGAIN, DistancePID.IGAIN, DistancePID.IWINDUP_LIMIT); - FtcDashboard dashboard = FtcDashboard.getInstance(); - - int settleCounter = 0; - while (settleCounter < 3 && (opMode.opModeIsActive() || opMode.opModeInInit())) { - TelemetryPacket packet = new TelemetryPacket(); - double width = pipeline.getPoleWidth(); - packet.put("width", width); - if (width == -1.0 || width < DistancePID.MIN_WIDTH) { - // NO POLES: STOP RIGHT NOW - for (Drive drive : robot.drives.values()) { - drive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - drive.setPower(0); - } - dashboard.sendTelemetryPacket(packet); - opMode.sleep(DistancePID.FRAME_SLEEP); - continue; - } - if (width > DistancePID.CUTOFF) { - // This isn't realistic, we're getting a bad reading - width = DistancePID.CUTOFF; - } - double correction = DistancePID.TARGET - width; - packet.put("correction", correction); - double widthPowerCorrection = distanceController.update(correction); - packet.put("power", widthPowerCorrection); - - dashboard.sendTelemetryPacket(packet); - - - if (Math.abs(widthPowerCorrection) < DistancePID.SETTLE_LIMIT) { - // If we're really close to the target, increase the settle counter - settleCounter++; - //logger.debug("Settle Counter: " + settleCounter); - } else { - // If we're not close, reset the settle counter - settleCounter = 0; - } - //logger.debug("Width Power Correction: " + widthPowerCorrection); - - widthPowerCorrection = Range.clip(widthPowerCorrection, -DistancePID.SAFETY_MAX, DistancePID.SAFETY_MAX); - robot.drives.get(Robot.DrivePos.FRONT_LEFT).setPower(widthPowerCorrection); - robot.drives.get(Robot.DrivePos.BACK_LEFT).setPower(widthPowerCorrection); - robot.drives.get(Robot.DrivePos.FRONT_RIGHT).setPower(widthPowerCorrection); - robot.drives.get(Robot.DrivePos.BACK_RIGHT).setPower(widthPowerCorrection); - - opMode.sleep(DistancePID.FRAME_SLEEP); - } - - // Stop the wheels when we're done (just in case) - for (Drive drive : robot.drives.values()) { - drive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - drive.setPower(0); - } - } - - public static double AUTO_SPEED = 20.0; - public static int REACH_LENGTH = 1800; - public static double NEXT_DEG = 90.0; - public static double TURN_DEG = 57.0; - - public static boolean DO_AUTO_PATH = true; - public static boolean DO_DISTANCE = true; - public static boolean DO_ROTATION = true; - public static boolean DO_PARKING = true; - public static boolean LOOP_ROTATION = false; - public static boolean LOOP_DISTANCE = false; - - // public static double X_MODIFIER = 0.0; - //public static double Y_PARK = 0.0; - - public static int LOOP_SLEEP = 5000; - - public static double MAGIC_CONVERSION = .04; - - public static double FORWARD = 17; - - public static boolean LEFT_SIDE_MIRROR = false; - - public static int OVERRIDE_PARK_ID = 14; - - // @Override - public void runOpMode() throws InterruptedException { - robot = new PowerPlayBotV2(opMode.hardwareMap, logger); - robot.initHardware(); - navigator = new Navigator(robot, opMode); - FtcDashboard dashboard = FtcDashboard.getInstance(); - - AprilTagPipeline aprilTagPipeline = new AprilTagPipeline(); - //SwitchablePipeline pipeline = new SwitchablePipeline(aprilTagPipeline); //TODO: Remove no longer necessary, use setPipeline - robot.camera.setPipeline(aprilTagPipeline); - if (!robot.camera.opened) { - robot.camera.open(new Timeout(10)); - } else { - robot.camera.resume(); - } - - // Wait for the start button to be pressed - opMode.waitForStart(); - - int tagId = 0; - if (DO_AUTO_PATH) { - // Detect the tag - tagId = detectTag(aprilTagPipeline); - logger.info("AprilTag Detected: " + tagId); - } - if (tagId == 0) { - tagId = OVERRIDE_PARK_ID; - } - - DoubleThresholdPipeline poleDetectionPipeline = null; - if (DO_DISTANCE || DO_ROTATION || LOOP_DISTANCE || LOOP_ROTATION) { - poleDetectionPipeline = initializePoleDetection(); - robot.camera.setPipeline(poleDetectionPipeline); - } - - if (opMode.isStopRequested()) return; - - if (DO_AUTO_PATH) { - - // Pause the pipeline to save resources - // robot.camera.pause(); - - // Get the cone in the pincher and get ready to drive - robot.arm.pincher.expand(); - - // Move forward so we don't scrape against the wall - navigator.navigationMonitorTicksAndCeaseMotion(AUTO_SPEED, 0, 1, 10); - - // Lift the preloaded cone up to drive height - robot.arm.lift.setPreset(Arm.Lift.Preset.DRIVING); - - // Wait for it to arrive at drive height - while (robot.arm.lift.isBusy() && !opMode.isStopRequested()) { - Thread.yield(); - } - - if (LEFT_SIDE_MIRROR) { - // Move left past the ground station - navigator.navigationMonitorTicksAndCeaseMotion(AUTO_SPEED, -15, 0, 10); - } else { - // Move right past the ground station - navigator.navigationMonitorTicksAndCeaseMotion(AUTO_SPEED, 15, 0, 10); - } - - // Move forward - navigator.navigationMonitorTicksAndCeaseMotion(AUTO_SPEED, 0, FORWARD, 10); - - if (LEFT_SIDE_MIRROR) { - // Turn to face our target direction - navigator.navigationMonitorTurn(-TURN_DEG); - // Move up to our special spot - navigator.navigationMonitorTicksPhi(AUTO_SPEED, 0, 5.5, -TURN_DEG, 10); - navigator.ceaseMotion(); - } else { - // Turn to face our target direction - navigator.navigationMonitorTurn(TURN_DEG); - // Move up to our special spot - navigator.navigationMonitorTicksPhi(AUTO_SPEED, 0, 5.5, TURN_DEG, 10); - navigator.ceaseMotion(); - } - - if (LEFT_SIDE_MIRROR) { - // Now it's time to drop the preloaded cone! - // Raise up to med height so that our camera is not obstructed - robot.arm.lift.setPreset(Arm.Lift.Preset.MEDIUM_POLE); - } - - // Wait for it to arrive at the height - while (robot.arm.lift.isBusy() && !opMode.isStopRequested()) { - Thread.yield(); - } - } - - if (LEFT_SIDE_MIRROR) { - if (DO_ROTATION) { - localizeOnPole(poleDetectionPipeline); - } - - if (DO_DISTANCE) { - localizeDistance(poleDetectionPipeline); - } - if (DO_ROTATION) { - localizeOnPole(poleDetectionPipeline); - } - - while (LOOP_DISTANCE && opMode.opModeIsActive()) { - localizeDistance(poleDetectionPipeline); - opMode.sleep(LOOP_SLEEP); - } - while (LOOP_ROTATION && opMode.opModeIsActive()) { - localizeOnPole(poleDetectionPipeline); - opMode.sleep(LOOP_SLEEP); - } - } - - if (DO_AUTO_PATH && !opMode.isStopRequested()) { - if (LEFT_SIDE_MIRROR) { - // Raise it up to high height - robot.arm.lift.setPreset(Arm.Lift.Preset.HIGH_POLE); - - // Wait for it to arrive at the height - while (robot.arm.lift.isBusy() && !opMode.isStopRequested()) { - Thread.yield(); - } - - robot.arm.reacher.setTargetPosition(REACH_LENGTH); - - while (robot.arm.reacher.isBusy() && !opMode.isStopRequested()) { - } - - if (opMode.isStopRequested()) { - robot.arm.reacher.setTargetPosition(0); - return; - } - - robot.arm.pincher.contract(); - - opMode.sleep(500); - } - //sleep(5000); - - robot.arm.reacher.setTargetPosition(0); - - //navigator.navigationMonitorTurn(NEXT_DEG); - // navigator.ceaseMotion(); - - while (robot.arm.reacher.isBusy() && !opMode.isStopRequested()) {} - - //robot.arm.lift.setPreset(Arm.Lift.Preset.MEDIUM_POLE); - - } - - if (DO_PARKING && !opMode.isStopRequested()) { - TelemetryPacket packet = new TelemetryPacket(); - double angle = navigator.getImuAngle(); - packet.put("IMU Angle", angle); - - // Invert constants for left side - double next_deg = NEXT_DEG; - double magic_conversion = MAGIC_CONVERSION; - //if (LEFT_SIDE_MIRROR) next_deg = -NEXT_DEG; - next_deg = 0; - if (LEFT_SIDE_MIRROR) magic_conversion = -MAGIC_CONVERSION; - - navigator.navigationMonitorTurn(next_deg); - navigator.ceaseMotion(); - if (LEFT_SIDE_MIRROR) { - robot.arm.lift.setPower(0); - } - - if (LEFT_SIDE_MIRROR) { - navigator.navigationMonitorTicksPhi(AUTO_SPEED / 2, 0, -1, 0, 10); - navigator.ceaseMotion(); - } - - double error = -(12 - (26 * Math.cos(angle))); - packet.put("Error", error); - packet.put("Converted", error * magic_conversion); - dashboard.sendTelemetryPacket(packet); - - double y_park = 0; - if (LEFT_SIDE_MIRROR) { - if (tagId == 16) { - y_park = -16; // left - } else if (tagId == 15) { - y_park = -4; // mid - } else if (tagId == 14) { - y_park = 6; // right - } - } else { - if (tagId == 14) { - y_park = 16; // left - } else if (tagId == 15) { - y_park = 6; // mid - } else if (tagId == 16) { - y_park = -4; // right - } - } - - navigator.navigationMonitorTicksPhi(AUTO_SPEED, -y_park, 0, next_deg, 10); - navigator.ceaseMotion(); - } - while (!opMode.isStopRequested()) {} - } -} - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Left.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Left.java deleted file mode 100644 index 6f6b1a7d..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Left.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -@Autonomous(name = "Left", preselectTeleOp = "TeleOp") -public class Left extends LinearOpMode { - - @Override - public void runOpMode() throws InterruptedException { - AllAutomovement.LEFT_SIDE_MIRROR = true; - AllAutomovement auto = new AllAutomovement(this); - auto.runOpMode(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Right.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Right.java deleted file mode 100644 index 0e7d6e70..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Right.java +++ /dev/null @@ -1,14 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -@Autonomous(name = "Right", preselectTeleOp = "TeleOp") -public class Right extends LinearOpMode { - @Override - public void runOpMode() throws InterruptedException { - AllAutomovement.LEFT_SIDE_MIRROR = false; - AllAutomovement auto = new AllAutomovement(this); - auto.runOpMode(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/RoadRunnerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/RoadRunnerTest.java deleted file mode 100644 index e8e092aa..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/RoadRunnerTest.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.firstinspires.ftc.teamcode.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.drive.SampleMecanumDrive; -@Autonomous -public class RoadRunnerTest extends LinearOpMode { - @Override - - public void runOpMode() { - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - // We want to start the bot at x: 10, y: -8, heading: 90 degrees - //Pose2d startPose = new Pose2d(10, -8, Math.toRadians(90)); - - 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(0)) - .build() - ); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Rotation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Rotation.java deleted file mode 100644 index d4decd1a..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Rotation.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.Shared.Navigator; -import org.firstinspires.ftc.teamcode.robot.PowerPlayBotV2; -import org.firstinspires.ftc.teamcode.util.Logger; - -@Autonomous -public class Rotation extends LinearOpMode { - - Logger logger = new Logger(telemetry); - @Override - public void runOpMode() throws InterruptedException { - PowerPlayBotV2 robot = new PowerPlayBotV2(hardwareMap, logger); - robot.initHardware(); - Navigator navigator = new Navigator(robot, this); - - waitForStart(); - - //drive.navigationMonitorTicksPhi(0, 1, 0, 90, 10); - navigator.navigationMonitorTurn(90); - navigator.ceaseMotion(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java deleted file mode 100644 index 55aeb3a5..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java +++ /dev/null @@ -1,305 +0,0 @@ -package org.firstinspires.ftc.teamcode.drive; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.control.PIDCoefficients; -import com.acmerobotics.roadrunner.drive.DriveSignal; -import com.acmerobotics.roadrunner.drive.TankDrive; -import com.acmerobotics.roadrunner.followers.TankPIDVAFollower; -import com.acmerobotics.roadrunner.followers.TrajectoryFollower; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.trajectory.Trajectory; -import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; -import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint; -import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; -import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; -import com.acmerobotics.roadrunner.trajectory.constraints.TankVelocityConstraint; -import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; -import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; -import com.qualcomm.hardware.lynx.LynxModule; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; -import com.qualcomm.robotcore.hardware.PIDFCoefficients; -import com.qualcomm.robotcore.hardware.VoltageSensor; -import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner; -import org.firstinspires.ftc.teamcode.util.LynxModuleUtil; - -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; - -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; - -/* - * Simple tank drive hardware implementation for REV hardware. - */ -@Config -public class SampleTankDrive extends TankDrive { - public static PIDCoefficients AXIAL_PID = new PIDCoefficients(0, 0, 0); - public static PIDCoefficients CROSS_TRACK_PID = new PIDCoefficients(0, 0, 0); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); - - public static double VX_WEIGHT = 1; - public static double OMEGA_WEIGHT = 1; - - private TrajectorySequenceRunner trajectorySequenceRunner; - - private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH); - private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(MAX_ACCEL); - - private TrajectoryFollower follower; - - private List motors, leftMotors, rightMotors; - private IMU imu; - - private VoltageSensor batteryVoltageSensor; - - public SampleTankDrive(HardwareMap hardwareMap) { - super(kV, kA, kStatic, TRACK_WIDTH); - - follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_PID, - new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5); - - LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap); - - batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); - - for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { - module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); - } - - // TODO: adjust the names of the following hardware devices to match your configuration - imu = hardwareMap.get(IMU.class, "imu"); - IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR)); - imu.initialize(parameters); - - // add/remove motors depending on your robot (e.g., 6WD) - DcMotorEx leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - DcMotorEx leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - DcMotorEx rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - DcMotorEx rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); - - motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); - leftMotors = Arrays.asList(leftFront, leftRear); - rightMotors = Arrays.asList(rightFront, rightRear); - - for (DcMotorEx motor : motors) { - MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); - motorConfigurationType.setAchieveableMaxRPMFraction(1.0); - motor.setMotorType(motorConfigurationType); - } - - if (RUN_USING_ENCODER) { - setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } - - setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) { - 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(...)); - - trajectorySequenceRunner = new TrajectorySequenceRunner( - follower, HEADING_PID, batteryVoltageSensor, - new ArrayList<>(), new ArrayList<>(), new ArrayList<>(), new ArrayList<>() - ); - } - - public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { - return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, accelConstraint); - } - - public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) { - return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, accelConstraint); - } - - public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) { - return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, accelConstraint); - } - - public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) { - return new TrajectorySequenceBuilder( - startPose, - VEL_CONSTRAINT, accelConstraint, - MAX_ANG_VEL, MAX_ANG_ACCEL - ); - } - - public void turnAsync(double angle) { - trajectorySequenceRunner.followTrajectorySequenceAsync( - trajectorySequenceBuilder(getPoseEstimate()) - .turn(angle) - .build() - ); - } - - public void turn(double angle) { - turnAsync(angle); - waitForIdle(); - } - - public void followTrajectoryAsync(Trajectory trajectory) { - trajectorySequenceRunner.followTrajectorySequenceAsync( - trajectorySequenceBuilder(trajectory.start()) - .addTrajectory(trajectory) - .build() - ); - } - - public void followTrajectory(Trajectory trajectory) { - followTrajectoryAsync(trajectory); - waitForIdle(); - } - - public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) { - trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence); - } - - public void followTrajectorySequence(TrajectorySequence trajectorySequence) { - followTrajectorySequenceAsync(trajectorySequence); - waitForIdle(); - } - - public Pose2d getLastError() { - return trajectorySequenceRunner.getLastPoseError(); - } - - - public void update() { - updatePoseEstimate(); - DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity()); - if (signal != null) setDriveSignal(signal); - } - - public void waitForIdle() { - while (!Thread.currentThread().isInterrupted() && isBusy()) - update(); - } - - public boolean isBusy() { - return trajectorySequenceRunner.isBusy(); - } - - public void setMode(DcMotor.RunMode runMode) { - for (DcMotorEx motor : motors) { - motor.setMode(runMode); - } - } - - public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) { - for (DcMotorEx motor : motors) { - motor.setZeroPowerBehavior(zeroPowerBehavior); - } - } - - public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) { - PIDFCoefficients compensatedCoefficients = new PIDFCoefficients( - coefficients.p, coefficients.i, coefficients.d, - coefficients.f * 12 / batteryVoltageSensor.getVoltage() - ); - for (DcMotorEx motor : motors) { - motor.setPIDFCoefficients(runMode, compensatedCoefficients); - } - } - - public void setWeightedDrivePower(Pose2d drivePower) { - Pose2d vel = drivePower; - - if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getHeading()) > 1) { - // re-normalize the powers according to the weights - double denom = VX_WEIGHT * Math.abs(drivePower.getX()) - + OMEGA_WEIGHT * Math.abs(drivePower.getHeading()); - - vel = new Pose2d( - VX_WEIGHT * drivePower.getX(), - 0, - OMEGA_WEIGHT * drivePower.getHeading() - ).div(denom); - } else { - // Ensure the y axis is zeroed out. - vel = new Pose2d(drivePower.getX(), 0, drivePower.getHeading()); - } - - setDrivePower(vel); - } - - @NonNull - @Override - public List getWheelPositions() { - double leftSum = 0, rightSum = 0; - for (DcMotorEx leftMotor : leftMotors) { - leftSum += encoderTicksToInches(leftMotor.getCurrentPosition()); - } - for (DcMotorEx rightMotor : rightMotors) { - rightSum += encoderTicksToInches(rightMotor.getCurrentPosition()); - } - return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size()); - } - - public List getWheelVelocities() { - double leftSum = 0, rightSum = 0; - for (DcMotorEx leftMotor : leftMotors) { - leftSum += encoderTicksToInches(leftMotor.getVelocity()); - } - for (DcMotorEx rightMotor : rightMotors) { - rightSum += encoderTicksToInches(rightMotor.getVelocity()); - } - return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size()); - } - - @Override - public void setMotorPowers(double v, double v1) { - for (DcMotorEx leftMotor : leftMotors) { - leftMotor.setPower(v); - } - for (DcMotorEx rightMotor : rightMotors) { - rightMotor.setPower(v1); - } - } - - @Override - public double getRawExternalHeading() { - return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); - } - - @Override - public Double getExternalHeadingVelocity() { - return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate; - } - - public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) { - return new MinVelocityConstraint(Arrays.asList( - new AngularVelocityConstraint(maxAngularVel), - new TankVelocityConstraint(maxVel, trackWidth) - )); - } - - public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) { - return new ProfileAccelerationConstraint(maxAccel); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java index 8e0c6214..7aa096d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java @@ -10,6 +10,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.util.RobotLog; @@ -34,6 +35,7 @@ */ @Config @Autonomous(group = "drive") +@Disabled public class AutomaticFeedforwardTuner extends LinearOpMode { public static double MAX_POWER = 0.7; public static double DISTANCE = 100; // in diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java index a78ad379..2cb93a21 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java @@ -4,6 +4,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; @@ -26,6 +27,7 @@ */ @Config @Autonomous(group = "drive") +@Disabled public class BackAndForth extends LinearOpMode { public static double DISTANCE = 50; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java index 931b7420..d406a4f3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java @@ -15,6 +15,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.RobotLog; @@ -50,6 +51,7 @@ */ @Config @Autonomous(group = "drive") +@Disabled public class DriveVelocityPIDTuner extends LinearOpMode { public static double DISTANCE = 72; // in diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java index 63f577e1..144f575d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java @@ -3,6 +3,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; @@ -23,6 +24,7 @@ */ @Config @Autonomous(group = "drive") +@Disabled public class FollowerPIDTuner extends LinearOpMode { public static double DISTANCE = 48; // in diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java index 8411792b..78ac8d92 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.drive.opmode; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -15,6 +16,7 @@ * encoder localizer heading may be significantly off if the track width has not been tuned). */ @TeleOp(group = "drive") +@Disabled public class LocalizationTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java index 0d01bcb9..001c38d5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java @@ -10,7 +10,6 @@ import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.drive.DriveSignal; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.kinematics.Kinematics; import com.acmerobotics.roadrunner.profile.MotionProfile; @@ -18,12 +17,12 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.VoltageSensor; import com.qualcomm.robotcore.util.RobotLog; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.drive.DriveConstants; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; import java.util.Objects; @@ -45,6 +44,7 @@ */ @Config @Autonomous(group = "drive") +@Disabled public class ManualFeedforwardTuner extends LinearOpMode { public static double DISTANCE = 72; // in diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java index 05d82657..f07da4ae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java @@ -5,6 +5,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.ElapsedTime; @@ -24,6 +25,7 @@ @Config @Autonomous(group = "drive") +@Disabled public class MaxAngularVeloTuner extends LinearOpMode { public static double RUNTIME = 4.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java index ddca6cd1..53a0d778 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java @@ -5,6 +5,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.VoltageSensor; @@ -26,6 +27,7 @@ */ @Config @Autonomous(group = "drive") +@Disabled public class MaxVelocityTuner extends LinearOpMode { public static double RUNTIME = 2.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java index 8015d650..42214b6c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java @@ -4,6 +4,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; @@ -12,6 +13,7 @@ * This is an example of a more complex path to really test the tuning. */ @Autonomous(group = "drive") +@Disabled public class SplineTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java index 992d03ae..136295cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java @@ -6,6 +6,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -16,6 +17,7 @@ */ @Config @Autonomous(group = "drive") +@Disabled public class StrafeTest extends LinearOpMode { public static double DISTANCE = 60; // in diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java index e1b27e1c..9c346111 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java @@ -6,6 +6,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -16,6 +17,7 @@ */ @Config @Autonomous(group = "drive") +@Disabled public class StraightTest extends LinearOpMode { public static double DISTANCE = 60; // in diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java index ffce0233..3a33d5a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java @@ -6,6 +6,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.util.MovingStatistics; @@ -25,6 +26,7 @@ */ @Config @Autonomous(group = "drive") +@Disabled public class TrackWidthTuner extends LinearOpMode { public static double ANGLE = 180; // deg public static int NUM_TRIALS = 5; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java index ebe97f28..646df91b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java @@ -6,6 +6,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.util.MovingStatistics; import com.qualcomm.robotcore.util.RobotLog; @@ -36,6 +37,7 @@ */ @Config @Autonomous(group="drive") +@Disabled public class TrackingWheelForwardOffsetTuner extends LinearOpMode { public static double ANGLE = 180; // deg public static int NUM_TRIALS = 5; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java index a11059c4..bd792ce2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java @@ -3,6 +3,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.RobotLog; @@ -63,6 +64,7 @@ */ @Config @TeleOp(group = "drive") +@Disabled public class TrackingWheelLateralDistanceTuner extends LinearOpMode { public static int NUM_TURNS = 10; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java index 0637d19e..08a1eb72 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java @@ -2,6 +2,7 @@ import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; @@ -11,6 +12,7 @@ */ @Config @Autonomous(group = "drive") +@Disabled public class TurnTest extends LinearOpMode { public static double ANGLE = 90; // deg diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/FreightFrenzyRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/FreightFrenzyRobot.java deleted file mode 100644 index 0575363a..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/FreightFrenzyRobot.java +++ /dev/null @@ -1,131 +0,0 @@ -package org.firstinspires.ftc.teamcode.robot; - -import com.qualcomm.hardware.bosch.BNO055IMU; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.teamcode.robot.hardware.Arm; -import org.firstinspires.ftc.teamcode.robot.hardware.Drive; -import org.firstinspires.ftc.teamcode.util.HardwareNotFoundException; -import org.firstinspires.ftc.teamcode.util.Logger; -import org.firstinspires.ftc.teamcode.util.Timeout; - -import java.util.EnumMap; - -public class FreightFrenzyRobot extends Robot { - - // Hardware definitions - static final private class Hardware { - static final double revCounts = 28; - static final double gearReduction = 40;//40 before - static final double wheelDiameter = 6;//3 for test robot - - //Competition Robot -// static final String frontLeftMotorName = "FL"; // CHM2 -// static final String frontRightMotorName = "FR"; // CHM3 -// static final String backLeftMotorName = "BL"; // CHM0 -// static final String backRightMotorName = "BR"; // CHM1 -// static final String imuName = "imu"; - - static final String frontLeftMotorName = "FL"; // CHM2 //left_front_drive - static final String frontRightMotorName = "FR"; // CHM3 //right_front_drive - static final String backLeftMotorName = "BL"; // CHM0 //left_back_drive - static final String backRightMotorName = "BR"; // CHM1 //right_back_drive - static final String imuName = "imu"; - } - - private HardwareMap hardwareMap; - - public enum DrivePos { - FRONT_LEFT, - FRONT_RIGHT, - BACK_LEFT, - BACK_RIGHT - } - - public EnumMap Drives = new EnumMap(DrivePos.class); - - public BNO055IMU imu; - - public Arm arm; - - private final Logger logger; - - public FreightFrenzyRobot(HardwareMap hardwareMap, Logger logger) { - super(hardwareMap); - this.hardwareMap = hardwareMap; - this.logger = logger; - } - - public FreightFrenzyRobot(HardwareMap hardwareMap) { - super(hardwareMap); - this.hardwareMap = hardwareMap; - this.logger = new Logger(); - } - - public void initHardware() throws IllegalArgumentException { - try { - initDrives(); - //initArm(); - initIMU(); - } catch (IllegalArgumentException e) { - throw new HardwareNotFoundException(e); - } - } - - public void initIMU() { - // Get the imu - this.imu = hardwareMap.get(BNO055IMU.class, Hardware.imuName); - - // Set the IMU parameters - BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); - parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; - parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; - parameters.loggingEnabled = true; - parameters.loggingTag = "IMU"; - - // Initialize the imu with specified parameter - imu.initialize(parameters); - - // Wait while the gyro is calibrating... - while (!imu.isGyroCalibrated()) { - Thread.yield(); - } - - Timeout t = new Timeout(5); - while (!imu.isAccelerometerCalibrated() && !t.expired()) { - Thread.yield(); - } - } - - /*public void initArm() { - this.arm = new Arm( - this.hardwareMap.get(DcMotor.class, "Arm"), // EXM0 - this.hardwareMap.get(Servo.class, "pivotCollector"), // CHS3 - this.hardwareMap.get(CRServo.class, "spinCollector") // CHS2 - ); - }*/ - - - public void initDrives() { - this.Drives.put(DrivePos.FRONT_LEFT, - new Drive(this.hardwareMap.get(DcMotorEx.class, Hardware.frontLeftMotorName), - Hardware.revCounts,Hardware.gearReduction,Hardware.wheelDiameter)); - this.Drives.put(DrivePos.FRONT_RIGHT, - new Drive(this.hardwareMap.get(DcMotorEx.class, Hardware.frontRightMotorName), - Hardware.revCounts,Hardware.gearReduction,Hardware.wheelDiameter)); - this.Drives.put(DrivePos.BACK_LEFT, - new Drive(this.hardwareMap.get(DcMotorEx.class, Hardware.backLeftMotorName), - Hardware.revCounts,Hardware.gearReduction,Hardware.wheelDiameter)); - this.Drives.put(DrivePos.BACK_RIGHT, - new Drive(this.hardwareMap.get(DcMotorEx.class, Hardware.backRightMotorName), - Hardware.revCounts,Hardware.gearReduction,Hardware.wheelDiameter)); - - for (Drive drive : this.Drives.values()) - drive.run(); - - this.Drives.get(DrivePos.FRONT_LEFT).setDirection(DcMotorSimple.Direction.REVERSE); - this.Drives.get(DrivePos.BACK_LEFT).setDirection(DcMotorSimple.Direction.REVERSE); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/PowerPlayBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/PowerPlayBot.java deleted file mode 100644 index 0aed36a7..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/PowerPlayBot.java +++ /dev/null @@ -1,57 +0,0 @@ -package org.firstinspires.ftc.teamcode.robot; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.teamcode.robot.hardware.Drive; -import org.firstinspires.ftc.teamcode.util.Logger; - -public class PowerPlayBot extends Robot { - - static final double REV_COUNTS = 28; - static final double GEAR_REDUCTION = 40; - static final double WHEEL_DIAMETER = 3; - - //public Grabber grabber; - - public PowerPlayBot(HardwareMap hardwareMap, Logger logger) { - super(hardwareMap, logger); - } - - public PowerPlayBot(HardwareMap hardwareMap) { - super(hardwareMap); - } - - @Override - public void initHardware() { - super.initHardware(); - initGrabber(); - } - - protected void initGrabber() { - /*grabber = new Grabber( - hardwareMap.get(DcMotor.class, "reach"), - hardwareMap.get(DcMotorEx.class, "pivot"), - hardwareMap.get(Servo.class, "wrist"), - hardwareMap.get(Servo.class, "grab") - );*/ - } - - @Override - protected void initDrives() { - drives.put(DrivePos.FRONT_LEFT, new Drive(this.hardwareMap.get(DcMotorEx.class, "front_left"), REV_COUNTS, GEAR_REDUCTION, WHEEL_DIAMETER)); - drives.put(DrivePos.FRONT_RIGHT, new Drive(this.hardwareMap.get(DcMotorEx.class, "front_right"), REV_COUNTS, GEAR_REDUCTION, WHEEL_DIAMETER)); - drives.put(DrivePos.BACK_LEFT, new Drive(this.hardwareMap.get(DcMotorEx.class, "back_left"), REV_COUNTS, GEAR_REDUCTION, WHEEL_DIAMETER)); - drives.put(DrivePos.BACK_RIGHT, new Drive(this.hardwareMap.get(DcMotorEx.class, "back_right"), REV_COUNTS, GEAR_REDUCTION, WHEEL_DIAMETER)); - - for (Drive drive : drives.values()) - drive.run(); - - // Shouldn't produce a NullPointerException because we literally just put drives in it - drives.get(DrivePos.FRONT_LEFT).setDirection(DcMotorSimple.Direction.REVERSE); - drives.get(DrivePos.BACK_LEFT).setDirection(DcMotorSimple.Direction.REVERSE); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/PowerPlayBotV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/PowerPlayBotV2.java deleted file mode 100644 index 85926e34..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/PowerPlayBotV2.java +++ /dev/null @@ -1,75 +0,0 @@ -package org.firstinspires.ftc.teamcode.robot; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.teamcode.robot.hardware.Arm; -import org.firstinspires.ftc.teamcode.robot.hardware.Drive; -import org.firstinspires.ftc.teamcode.robot.hardware.Webcam; -import org.firstinspires.ftc.teamcode.util.Logger; - -public class PowerPlayBotV2 extends Robot { - - public final double REV_COUNTS = 28; - public final double GEAR_REDUCTION = 40; - public final double WHEEL_DIAMETER = 3; - - public Arm arm; - - public Webcam camera; - - public PowerPlayBotV2(HardwareMap hardwareMap, Logger logger) { - super(hardwareMap, logger); - } - - public PowerPlayBotV2(HardwareMap hardwareMap) { - super(hardwareMap); - } - - @Override - public void initHardware() { - super.initHardware(); - initCamera(); - initArm(); - initCamera(); - } - - protected void initCamera() { - camera = new Webcam("Webcam 1", hardwareMap); - } - - protected void initArm() { - arm = new Arm( - new Arm.Lift( - hardwareMap.get(DcMotor.class, "lift_low"), - hardwareMap.get(DcMotor.class, "lift_mid"), - hardwareMap.get(DcMotor.class, "lift_high") - ), - new Arm.Reacher( - hardwareMap.get(DcMotor.class, "reach") - ), - new Arm.Pincher( - hardwareMap.get(Servo.class, "pinch") - ) - ); - - } - - @Override - protected void initDrives() { - drives.put(DrivePos.FRONT_LEFT, new Drive(this.hardwareMap.get(DcMotorEx.class, "front_left"), REV_COUNTS, GEAR_REDUCTION, WHEEL_DIAMETER)); - drives.put(DrivePos.FRONT_RIGHT, new Drive(this.hardwareMap.get(DcMotorEx.class, "front_right"), REV_COUNTS, GEAR_REDUCTION, WHEEL_DIAMETER)); - drives.put(DrivePos.BACK_LEFT, new Drive(this.hardwareMap.get(DcMotorEx.class, "back_left"), REV_COUNTS, GEAR_REDUCTION, WHEEL_DIAMETER)); - drives.put(DrivePos.BACK_RIGHT, new Drive(this.hardwareMap.get(DcMotorEx.class, "back_right"), REV_COUNTS, GEAR_REDUCTION, WHEEL_DIAMETER)); - - for (Drive drive : drives.values()) - drive.run(); - - // Shouldn't produce a NullPointerException because we literally just put drives in it - drives.get(DrivePos.FRONT_LEFT).setDirection(DcMotorSimple.Direction.REVERSE); - drives.get(DrivePos.BACK_LEFT).setDirection(DcMotorSimple.Direction.REVERSE); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Robot.java deleted file mode 100644 index df909faa..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Robot.java +++ /dev/null @@ -1,83 +0,0 @@ -package org.firstinspires.ftc.teamcode.robot; - -import com.qualcomm.hardware.bosch.BNO055IMU; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.teamcode.robot.hardware.Drive; -import org.firstinspires.ftc.teamcode.util.Logger; -import org.firstinspires.ftc.teamcode.util.Timeout; - -import java.util.EnumMap; - -public abstract class Robot { - - public final double REV_COUNTS = 28; - public final double GEAR_REDUCTION = 40; - public final double WHEEL_DIAMETER = 3; - - protected final HardwareMap hardwareMap; - protected Logger logger; - - public BNO055IMU imu; - - public enum DrivePos { - FRONT_LEFT, - FRONT_RIGHT, - BACK_LEFT, - BACK_RIGHT - } - - public EnumMap drives = new EnumMap<>(DrivePos.class); - - public Robot(HardwareMap hardwareMap) { - this(hardwareMap, new Logger()); - } - - public Robot(HardwareMap hardwareMap, Logger logger) { - this.hardwareMap = hardwareMap; - this.logger = logger; - } - - public void initHardware() { - initIMU(); - initDrives(); - } - - protected void initIMU() { - // Get the imu - this.imu = hardwareMap.get(BNO055IMU.class, "imu"); - - // Set the IMU parameters - BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); - parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; - parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; - parameters.loggingEnabled = true; - parameters.loggingTag = "IMU"; - - // Initialize the imu with specified parameter - imu.initialize(parameters); - - logger.debug("Waiting for the gyroscope to calibrate..."); - Timeout t = new Timeout(2); - while(!imu.isGyroCalibrated() && !t.expired()) { - Thread.yield(); - } - if (!imu.isGyroCalibrated()) { - logger.warning("Gyroscope was not properly calibrated!"); - } else { - logger.debug("Gyroscope calibrated successfully."); - } - - // TODO: We don't wait for the accelerometer or magnetometer to calibrate because it never seems to complete - // Should investigate sometime... - } - - /** - * This method must be overridden by the child class - * get motors from the hardware map and initialize them - * in the drives EnumMap - */ - protected abstract void initDrives(); - - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/TestRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/TestRobot.java deleted file mode 100644 index 9eed47f4..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/TestRobot.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.firstinspires.ftc.teamcode.robot; - -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.teamcode.robot.hardware.Drive; -import org.firstinspires.ftc.teamcode.robot.hardware.Webcam; -import org.firstinspires.ftc.teamcode.util.Logger; - -public class TestRobot extends Robot{ - - static final double REV_COUNTS = 28; - static final double GEAR_REDUCTION = 40; - static final double WHEEL_DIAMETER = 3; - - public Webcam camera; - - public TestRobot(HardwareMap hardwareMap, Logger logger) { - super(hardwareMap, logger); - } - - public TestRobot(HardwareMap hardwareMap) { - super(hardwareMap); - } - - @Override - protected void initDrives() { - drives.put(DrivePos.FRONT_LEFT, new Drive(this.hardwareMap.get(DcMotorEx.class, "left_front"), REV_COUNTS, GEAR_REDUCTION, WHEEL_DIAMETER)); - drives.put(DrivePos.FRONT_RIGHT, new Drive(this.hardwareMap.get(DcMotorEx.class, "right_front"), REV_COUNTS, GEAR_REDUCTION, WHEEL_DIAMETER)); - drives.put(DrivePos.BACK_LEFT, new Drive(this.hardwareMap.get(DcMotorEx.class, "left_back"), REV_COUNTS, GEAR_REDUCTION, WHEEL_DIAMETER)); - drives.put(DrivePos.BACK_RIGHT, new Drive(this.hardwareMap.get(DcMotorEx.class, "right_back"), REV_COUNTS, GEAR_REDUCTION, WHEEL_DIAMETER)); - - for (Drive drive : drives.values()) - drive.run(); - - // Shouldn't produce a NullPointerException because we literally just put drives in it - drives.get(DrivePos.FRONT_LEFT).setDirection(DcMotorSimple.Direction.REVERSE); - drives.get(DrivePos.BACK_LEFT).setDirection(DcMotorSimple.Direction.REVERSE); - } - - @Override - public void initHardware() { - super.initHardware(); - initCamera(); - } - - protected void initCamera() { - camera = new Webcam("Webcam 1", hardwareMap); - } - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/Arm.java deleted file mode 100644 index 65455b69..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/Arm.java +++ /dev/null @@ -1,149 +0,0 @@ -package org.firstinspires.ftc.teamcode.robot.hardware; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Servo; - -/** - * Arm Component. Currently uses hard-coded calibration data. - * The Arm consists of 3 major components: - * 1. The Lift, which is 3 motors attached to a vertical linear slide - * 2. The Reacher, which is a motor attached to a horizontal linear slide - * 3. The Pincher, which is a servo attached to a custom mechanism that lowers into the cone and expands - */ -public class Arm { - public Lift lift; - public Reacher reacher; - public Pincher pincher; - - public static class Lift { - private final DcMotor motor1; - private final DcMotor motor2; - private final DcMotor motor3; - - public Lift(DcMotor motor1, DcMotor motor2, DcMotor motor3) { - this.motor1 = motor1; - this.motor2 = motor2; - this.motor3 = motor3; - - this.motor1.setDirection(DcMotor.Direction.REVERSE); - this.motor3.setDirection(DcMotor.Direction.REVERSE); - } - - public void setPower(double power) { - // TODO: Enforce limits - motor1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - motor2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - motor3.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - motor1.setPower(power); - motor2.setPower(power); - motor3.setPower(power); - } - - public void setTargetPosition(int position) { - if (position < getCurrentPosition()) { - setTargetPosition(position, -0.5); - } else { - setTargetPosition(position, 1); - } - } - - public void setTargetPosition(int position, double power) { - motor1.setTargetPosition(position); - motor2.setTargetPosition(position); - motor3.setTargetPosition(position); - - motor1.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motor2.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motor3.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - motor1.setPower(power); // TODO: Don't hardcode motor power - motor2.setPower(power); - motor3.setPower(power); - } - - public int getTargetPosition() { - return motor3.getTargetPosition(); - } - - public int getCurrentPosition() { - return motor3.getCurrentPosition(); - } - - public boolean isBusy() { - return motor1.isBusy() || motor2.isBusy() || motor3.isBusy(); - } - - public enum Preset { - GRAB (40), - GROUND (60), - DRIVING (500), - LOW_POLE (1230), - MEDIUM_POLE (2010), - HIGH_POLE (2735); - - final int height; - Preset(int height) { - this.height = height; - } - } - - public void setPreset(Preset preset) { - setTargetPosition(preset.height); - } - } - - public static class Reacher { - private final DcMotor motor; - - public Reacher(DcMotor motor) { - this.motor = motor; - this.motor.setDirection(DcMotor.Direction.REVERSE); - } - - public void setPower(double power) { - motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - motor.setPower(power); - } - - public void setTargetPosition(int position) { - motor.setTargetPosition(position); - motor.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motor.setPower(1); // TODO: Don't hardcode motor power - } - - public int getTargetPosition() { - return motor.getTargetPosition(); - } - - public int getCurrentPosition() { - return motor.getCurrentPosition(); - } - - public boolean isBusy() { - return motor.isBusy(); - } - } - - public static class Pincher { - private final Servo servo; - - public Pincher(Servo servo) { - this.servo = servo; - } - - public void expand() { - servo.setPosition(0.45); - } - - public void contract() { - servo.setPosition(0); - } - } - - public Arm(Lift lift, Reacher reacher, Pincher pincher) { - this.lift = lift; - this.reacher = reacher; - this.pincher = pincher; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/Drive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/Drive.java deleted file mode 100644 index 0928d961..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/Drive.java +++ /dev/null @@ -1,113 +0,0 @@ -package org.firstinspires.ftc.teamcode.robot.hardware; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; - -import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; - -/** - * Wrapper around DcMotor providing enhanced and commonly used functionality such as resetting the encoder and calculating the total number of inches traveled. - */ -public class Drive { - private final DcMotorEx motor; - private final double inchCounts; - - /** - * Constructs a new Drive with specified motor, wheel, and gear reduction. - * @param motor the DcMotor object to wrap - * @param revCounts the number of counts in one motor revolution - * @param gearReduction the reduction ratio of the drive's gearing - * @param wheelDiameter the diameter of the wheel, in inches - */ - public Drive(DcMotorEx motor, double revCounts, double gearReduction, double wheelDiameter) { - this.motor = motor; - this.inchCounts = (revCounts * gearReduction) / (wheelDiameter * Math.PI); - } - - /** - * Sets the drive's power level - * @param power power level to set the motor to - */ - public void setPower(double power) { - this.motor.setPower(power); - } - - /** - * Sets the drives mode - * @param mode mode to set the motor to - */ - public void setMode(DcMotorEx.RunMode mode) { - this.motor.setMode(mode); - } - - /** - * Sets the direction of the motor - * @param direction - */ - public void setDirection(DcMotorSimple.Direction direction) { - this.motor.setDirection(direction); - } - - /** - * Sets the zero power behavior - * @param zeroPowerBehavior - */ - public void setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior zeroPowerBehavior) { - this.motor.setZeroPowerBehavior(zeroPowerBehavior); - } - - /** - * Gets the current drive mode - * @return current mode - */ - public DcMotor.RunMode getMode() { - return this.motor.getMode(); - } - - /** - * Gets the current reading the of drive's encoder - * @return current encode reading - */ - public int getCurrentPosition() { - return this.motor.getCurrentPosition(); - } - - /** - * Gets the number of inches traveled since the last encoder reset. - * Calculated by dividing the result of getCurrentPosition() by inchCounts. - * @return inches traveled - */ - public double getCurrentInches() { - return this.getCurrentPosition() / this.inchCounts; - } - - /** - * Resets the encoder to zero. This will remove power to the motor temporarily, use with caution. It attempts to resume it's previous mode after reset. - */ - public void resetEncoder() { - DcMotor.RunMode current = this.getMode(); - this.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - this.setMode(current); - } - - /** - * Sets zero power behavior to float as well as setting the mode to run with encoder - */ - public void run() { - this.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - this.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } - /** - * Sets the zero power behavior to break and sets the power to zero - */ - public void brake() { - this.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - this.setPower(0); - } - - public double getCurrent(){ - return motor.getCurrent(CurrentUnit.AMPS); - } -} - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/Webcam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/Webcam.java deleted file mode 100644 index a1eda426..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/Webcam.java +++ /dev/null @@ -1,111 +0,0 @@ -package org.firstinspires.ftc.teamcode.robot.hardware; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.teamcode.util.HardwareNotFoundException; -import org.firstinspires.ftc.teamcode.util.Logger; -import org.firstinspires.ftc.teamcode.util.Timeout; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; -import org.openftc.easyopencv.OpenCvPipeline; -import org.openftc.easyopencv.OpenCvWebcam; - -public class Webcam { - // Constants - private static final OpenCvCameraRotation ROTATION = OpenCvCameraRotation.UPRIGHT; - private static final int WIDTH = 960; - private static final int HEIGHT = 720; - - public boolean opened = false; - - private final OpenCvWebcam webcam; - - /** - * Convenience class for interacting with OpenCV webcams. - * @param webcam OpenCV webcam to wrap - */ - public Webcam(@NonNull OpenCvWebcam webcam) { - this.webcam = webcam; - } - - /** - * Convenience class for interacting with OpenCV webcams. - * @param name name of the device in the hardware map - * @param hardwareMap hardware map to get the camera from - */ - public Webcam(String name, HardwareMap hardwareMap) { - try { - this.webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, name)); - } catch (IllegalArgumentException e) { - throw new HardwareNotFoundException(e, "Webcam"); // Use Webcam instead of WebcamName - } - FtcDashboard.getInstance().startCameraStream(this.webcam, 30); - } - - /** - * Opens the camera device. Will automatically unpause the stream and pipeline. - * This will prevent Vuforia and Tensorflow from accessing the camera. - */ - public void openAsync() { - this.webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - @Override - public void onOpened() { - opened = true; - resume(); - } - - @Override - public void onError(int errorCode) { - new Logger().error("Webcam returned error code: " + errorCode); - } - }); - } - - public void open() { - open(new Timeout(10)); - } - - public void open(Timeout timeout) { - openAsync(); - while (!this.opened && !timeout.expired()) { Thread.yield(); } - } - - /** - * Resumes the camera stream. Will continue with the previous pipeline. - */ - public void resume() { - this.webcam.startStreaming(WIDTH, HEIGHT, ROTATION); - } - - /** - * Pauses the camera stream and pipeline. Note that this will not free the - * hardware, so Vuforia and TensorFlow will not work. - */ - public void pause() { - this.webcam.stopStreaming(); - } - - /** - * Closes the camera device. This deactivates the hardware so that it can be used - * by Vuforia or TensorFlow. - */ - public void close() { - pause(); - this.webcam.closeCameraDeviceAsync(() -> { }); - } - - /** - * Changes the image-processing pipeline being used. Note that this can either - * be called before opening, or on-the-fly while the camera is streaming. - * @param pipeline - */ - public void setPipeline(OpenCvPipeline pipeline) { - this.webcam.setPipeline(pipeline); - } - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/demos/ArmDemo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/demos/ArmDemo.java deleted file mode 100644 index 750d92cb..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/demos/ArmDemo.java +++ /dev/null @@ -1,209 +0,0 @@ -package org.firstinspires.ftc.teamcode.robot.hardware.demos; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.CRServo; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.Servo; - -@TeleOp(name = "Arm Demo", group = "Hardware Demos") -@Disabled -public class ArmDemo extends LinearOpMode { - //private final String TAG = getClass().getName(); - DcMotor Arm = null; - Servo Wrist = null; - CRServo Collector = null; - double pivotCollectorFactor = 0.17 / 2; - double pivotCollectorDifference = (0.17 / 2) + 0.36; - DcMotor leftFrontDrive, rightFrontDrive, leftBackDrive, rightBackDrive = null; - - - @Override - public void runOpMode() { - //final Drive2 drive = new Drive2(this); - - //drive.init(); - - Arm = hardwareMap.get(DcMotor.class, "Arm"); - Wrist = hardwareMap.get(Servo.class, "pivotCollector"); - Collector = hardwareMap.get(CRServo.class, "spinCollector"); - // Initialize the hardware variables. Note that the strings used here as parameters - // to 'get' must correspond to the names assigned during the robot configuration - // step (using the FTC Robot Controller app on the phone). - leftFrontDrive = hardwareMap.get(DcMotor.class, "FL"); - rightFrontDrive = hardwareMap.get(DcMotor.class, "FR"); - leftBackDrive = hardwareMap.get(DcMotor.class, "BL"); - rightBackDrive = hardwareMap.get(DcMotor.class, "BR"); - - rightFrontDrive.setDirection(DcMotorSimple.Direction.REVERSE); - rightBackDrive.setDirection(DcMotorSimple.Direction.REVERSE); - - - waitForStart(); - - - while (opModeIsActive()) { - - // Run wheels in tank mode (note: The joystick goes negative when pushed forwards, so negate it) - double frontRightPowerFactor, frontLeftPowerFactor, backRightPowerFactor, backLeftPowerFactor; - double magRight = Math.hypot(gamepad1.right_stick_x, gamepad1.right_stick_y); - double thetaRight = Math.atan2(-gamepad1.right_stick_y, gamepad1.right_stick_x); - double magLeft = Math.hypot(gamepad1.left_stick_x, gamepad1.left_stick_y); - double thetaLeft = Math.atan2(-gamepad1.left_stick_y, gamepad1.left_stick_x); - double pi = Math.PI; - - if (thetaRight > 0 && thetaRight < pi / 2) { - frontRightPowerFactor = -Math.cos(2 * thetaRight); - } else if (thetaRight >= -pi && thetaRight < -pi / 2) { - frontRightPowerFactor = Math.cos(2 * thetaRight); - } else if (thetaRight >= pi / 2 && thetaRight <= pi) { - frontRightPowerFactor = 1; - } else { - frontRightPowerFactor = -1; - } - - if (thetaLeft > 0 && thetaLeft < pi / 2) { - backLeftPowerFactor = -Math.cos(2 * thetaLeft); - } else if (thetaLeft >= -pi && thetaLeft < -pi / 2) { - backLeftPowerFactor = Math.cos(2 * thetaLeft); - } else if (thetaLeft >= pi / 2 && thetaLeft <= pi) { - backLeftPowerFactor = 1; - } else { - backLeftPowerFactor = -1; - } - - if (thetaRight > -pi / 2 && thetaRight < 0) { - backRightPowerFactor = Math.cos(2 * thetaRight); - } else if (thetaRight > pi / 2 && thetaRight < pi) { - backRightPowerFactor = -Math.cos(2 * thetaRight); - } else if (thetaRight >= 0 && thetaRight <= pi / 2) { - backRightPowerFactor = 1; - } else { - backRightPowerFactor = -1; - } - - if (thetaLeft > -pi / 2 && thetaLeft < 0) { - frontLeftPowerFactor = Math.cos(2 * thetaLeft); - } else if (thetaLeft > pi / 2 && thetaLeft < pi) { - frontLeftPowerFactor = -Math.cos(2 * thetaLeft); - } else if (thetaLeft >= 0 && thetaLeft <= pi / 2) { - frontLeftPowerFactor = 1; - } else { - frontLeftPowerFactor = -1; - } - - // leftFrontDrive.setPower((frontLeftPowerFactor * magLeft)*(frontLeftPowerFactor * magLeft)); - // rightFrontDrive.setPower(-(frontRightPowerFactor * magRight)*(frontRightPowerFactor * magRight)); - //leftBackDrive.setPower((backLeftPowerFactor * magLeft)*(backLeftPowerFactor * magLeft)); - //rightBackDrive.setPower(-(backRightPowerFactor * magRight)*(backRightPowerFactor * magRight)); - - leftFrontDrive.setPower((frontLeftPowerFactor * magLeft)); - rightFrontDrive.setPower(-(frontRightPowerFactor * magRight)); - leftBackDrive.setPower((backLeftPowerFactor * magLeft)); - rightBackDrive.setPower(-(backRightPowerFactor * magRight)); - - if (gamepad2.back) { - if(gamepad2.dpad_down){ - Arm.setTargetPosition(0); - Arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Arm.setPower(0.5); - Wrist.setPosition(0.53); - } else if (gamepad2.dpad_up){ - Arm.setTargetPosition(-340); - Arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Arm.setPower(0.5); - Wrist.setPosition(0.5); - }} - else if (gamepad2.y) { - GoToHubLevel(1); - } else if (gamepad2.x) { - GoToHubLevel(2); - } else if (gamepad2.b) { - GoToHubLevel(3); - } else if (gamepad2.a) { - GoToHubLevel(4); - } else if (gamepad2.dpad_right) { - Wrist.setPosition(-gamepad2.right_stick_y * pivotCollectorFactor + pivotCollectorDifference); - } else if (gamepad2.dpad_up) { - Arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - Arm.setPower((gamepad2.right_trigger + (-gamepad2.left_trigger)) / 2); - } - - Collector.setPower(-gamepad2.left_stick_y / 2); - - telemetry.addData("Ticks", Arm.getCurrentPosition()); - telemetry.addData("WristPos", Wrist.getPosition()); - telemetry.addData("Power", Collector.getPower()); - telemetry.addData("ArmPower", gamepad2.right_trigger + (-gamepad2.left_trigger)); - telemetry.addData("front right power ", ((float) Math.round(rightFrontDrive.getPower() * 100)) / 100); - telemetry.addData("front left power ", ((float) Math.round(leftFrontDrive.getPower() * 100)) / 100); - telemetry.addData("back right power ", ((float) Math.round(rightBackDrive.getPower() * 100)) / 100); - telemetry.addData("back left power ", ((float) Math.round(leftBackDrive.getPower() * 100)) / 100); - telemetry.addData("left joystick x", ((float) Math.round(gamepad1.left_stick_x * 100)) / 100); - telemetry.addData("left joystick y", ((float) Math.round(-gamepad1.left_stick_y * 100)) / 100); - telemetry.addData("magnitude left", ((float) Math.round(magLeft * 100)) / 100); - telemetry.addData("thetaLeft", ((float) Math.round(thetaLeft / pi * 100)) / 100); - - telemetry.update(); - - - } - - // Only do this in simulator; real robot needs time to stop. - //drive.ceaseMotion(); - } - - private void GoToHubLevel(int hubLevel) { - - if (hubLevel == 1) { - Arm.setTargetPosition(-3720); - Arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Arm.setPower(1); - Wrist.setPosition(0.38); - } - - if (hubLevel == 2) { - if (gamepad2.dpad_down) { - Arm.setTargetPosition(-4100); - Arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Wrist.setPosition(0.36); - Arm.setPower(1); - } else if (gamepad2.dpad_up) { - Arm.setTargetPosition(-2200); - Arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Wrist.setPosition(0.53); - Arm.setPower(1); - } - } - if (hubLevel == 3) { - if (gamepad2.dpad_down) { - Arm.setTargetPosition(-5238); - Arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Wrist.setPosition(0.44); - Arm.setPower(1); - } else if (gamepad2.dpad_up) { - Arm.setTargetPosition(-1450); - Arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Wrist.setPosition(0.48); - Arm.setPower(1); - } - } - - if (hubLevel == 4) { - if (gamepad2.dpad_down) { - Arm.setTargetPosition(-6300); - Arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Wrist.setPosition(0.49); - Arm.setPower(1); - } else if (gamepad2.dpad_up) { - Arm.setTargetPosition(-430); - Arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Wrist.setPosition(0.43); - Arm.setPower(1); - } - - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/demos/ConceptScanServoLM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/demos/ConceptScanServoLM.java deleted file mode 100644 index 46b54f4e..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/demos/ConceptScanServoLM.java +++ /dev/null @@ -1,154 +0,0 @@ -/* Copyright (c) 2017 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.CRServo; - -/** - * This OpMode scans a single servo back and forwards until Stop is pressed. - * The code is structured as a LinearOpMode - * INCREMENT sets how much to increase/decrease the servo position each cycle - * CYCLE_MS sets the update period. - * - * This code assumes a Servo configured with the name "left_hand" as is found on a pushbot. - * - * NOTE: When any servo position is set, ALL attached servos are activated, so ensure that any other - * connected servos are able to move freely before running this test. - * - * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list - */ -@TeleOp(name = "Concept: Scan Servo LM", group = "Concept") -@Disabled -public class ConceptScanServoLM extends LinearOpMode { - - static final double INCREMENT = 0.01; // amount to slew servo each CYCLE_MS cycle - static final int CYCLE_MS = 50; // period of each cycle - double MAX_V = 1.0; // Maximum rotational position - double MIN_V = -1.0; // Minimum rotational position - - // Define class members - CRServo rightCarousel; - CRServo leftCarousel; - double velocity = (MAX_V - MIN_V) / 2; // Start at halfway position - boolean rampUp = true; - - - - - @Override - public void runOpMode() { - - // Connect to servo (Assume PushBot Left Hand) - // Change the text in quotes to match any servo name on your robot. - rightCarousel = hardwareMap.get(CRServo.class, "rightCarousel"); // CHS1 - leftCarousel = hardwareMap.get(CRServo.class, "leftCarousel"); // CHS0 - //gamepad = hardwareMap.get(Gamepad.class, "gamepad1"); - - // Wait for the start button - telemetry.addData(">", "Press Start to scan Servo." ); - telemetry.update(); - waitForStart(); - velocity = 0; - long time = System.currentTimeMillis(); - // Scan servo till stop pressed. - while(opModeIsActive()){ - - // slew the servo, according to the rampUp (direction) variable. - - /*if (rampUp = true) { - // Keep stepping up until we hit the max value. - velocity += INCREMENT ; - if (velocity >= MAX_V) { - velocity = MAX_V; - rampUp = false; // Switch ramp direction - } - } - else if (rampUp = false){ - // Keep stepping down until we hit the min value. - velocity -= INCREMENT ; - if (velocity <= MIN_V) { - velocity = MIN_V; - rampUp = true; // Switch ramp direction - } - }*/ - if (gamepad1.right_bumper) { - velocity = MAX_V; - } - if (gamepad1.left_bumper) { - velocity = MIN_V; - } - if (gamepad1.dpad_up && MAX_V < 1 && System.currentTimeMillis() - time >= 150) { - MAX_V = MAX_V + .05; - MIN_V = MIN_V - .05; - if( velocity == MAX_V -.05) { - velocity = MAX_V; - }else { - velocity = MIN_V; - } - - time = System.currentTimeMillis(); - } - if (gamepad1.dpad_down && MAX_V > 0 && System.currentTimeMillis() - time >= 150) { - MAX_V = MAX_V - .05; - MIN_V = MIN_V + .05; - if( velocity == MAX_V +.05) { - velocity = MAX_V; - }else { - velocity = MIN_V; - } - time = System.currentTimeMillis(); - } - if(gamepad1.b) { - velocity = 0; - } - - // Display the current value - telemetry.addData("Servo Position", "%5.2f", velocity); - telemetry.addData("Max clockwise", "%5.2f", MAX_V); - telemetry.addData("Max counterclockwise", "%5.2f", MIN_V); - telemetry.addData(">", "Press Stop to end test." ); - telemetry.update(); - - // Set the servo to the new position and pause; - rightCarousel.setPower(velocity); - leftCarousel.setPower(velocity); - sleep(CYCLE_MS); - idle(); - } - - // Signal done; - telemetry.addData(">", "Done"); - telemetry.update(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/demos/WebcamDemo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/demos/WebcamDemo.java deleted file mode 100644 index 0f0a0877..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/hardware/demos/WebcamDemo.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.firstinspires.ftc.teamcode.robot.hardware.demos; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.robot.hardware.Webcam; -import org.firstinspires.ftc.teamcode.vision.HSVColor; -import org.firstinspires.ftc.teamcode.vision.TeamElementDetector; - -@TeleOp(name = "Webcam Demo", group = "Hardware Demos") -@Disabled -public class WebcamDemo extends LinearOpMode { - - @Override - public void runOpMode() throws InterruptedException { - // Get the webcam device from the hardware map - // "Webcam 1" is the name of the camera, and may need to be changed depending on your config - Webcam webcam = new Webcam("Webcam 1", hardwareMap); - - // Initialize your pipeline, see TeamElementDetectorDemo for a better example - TeamElementDetector detector = new TeamElementDetector(null); - - // Tell the camera to use that pipeline - webcam.setPipeline(detector); - - // Open the camera device - webcam.open(); - - // You can now use the pipeline, see the TeamElementDetectorDemo for details - - // If we want to switch pipelines, we would do: - //webcam.setPipeline(newpipeline); - - // If we want to save processing and stop processing *any* pipeline: - webcam.pause(); - - // To begin processing again: - webcam.resume(); - - // If we want to use Vuforia, we need to close the device; use open (from above) to re-open it. - webcam.close(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/BroadcastHandler.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/BroadcastHandler.java deleted file mode 100644 index f4540f62..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/BroadcastHandler.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.firstinspires.ftc.teamcode.util; - - -import android.content.BroadcastReceiver; -import android.content.Context; -import android.content.Intent; -import android.content.IntentFilter; - -import org.firstinspires.ftc.robotcore.internal.system.AppUtil; - - -public class BroadcastHandler extends BroadcastReceiver { - private final Lambda lambda; - - public BroadcastHandler(String name, Lambda lambda) { - this.lambda = lambda; - AppUtil.getDefContext().getApplicationContext().registerReceiver(this, new IntentFilter(name)); - } - - @Override - public void onReceive(Context context, Intent intent) { - this.lambda.run(intent); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurator.java deleted file mode 100644 index 35f5aa2a..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurator.java +++ /dev/null @@ -1,57 +0,0 @@ -package org.firstinspires.ftc.teamcode.util; - -import com.google.gson.Gson; -import com.google.gson.GsonBuilder; -import com.google.gson.reflect.TypeToken; -import com.qualcomm.robotcore.util.ReadWriteFile; - -import org.firstinspires.ftc.robotcore.internal.system.AppUtil; - -import java.io.File; -import java.lang.reflect.Type; - - -/** - * This is a helper class used to store and retrieve non-volatile configuration values - * as JSON files. - * - * This version is *not* backed by a map, instead, *all* types must be defined in Config. - * - * These files can be accessed by looking in the FIRST folder on the Control Hub. - * - * Note: on macOS you may need Android File Transfer. - */ -public class Configurator { - - public static Configuration load() { - return load("config.json"); - } - - public static void save(Configuration config) { - save(config, "config.json"); - } - - - /** - * Loads the configuration from the JSON file - * Note that if no configuration file exists, a new one will be created automatically. - */ - public static Configuration load(String filename) { - Gson gson = new GsonBuilder() - .setPrettyPrinting() - .create(); - File file = AppUtil.getInstance().getSettingsFile(filename); - return gson.fromJson(ReadWriteFile.readFile(file), Configuration.class); - } - - /** - * Saves the configuration to the JSON file - */ - public static void save(Configuration config, String filename) { - Gson gson = new GsonBuilder() - .setPrettyPrinting() - .create(); - File file = AppUtil.getInstance().getSettingsFile(filename); - ReadWriteFile.writeFile(file, gson.toJson(config)); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ExMath.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ExMath.java deleted file mode 100644 index ab92758c..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ExMath.java +++ /dev/null @@ -1,11 +0,0 @@ -package org.firstinspires.ftc.teamcode.util; - -public class ExMath { - public static double square_with_sign(double input) { - if (input > 0) { - return input * input; - } else { - return input * input * -1; - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/External.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/External.java deleted file mode 100644 index 5ecb9eb8..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/External.java +++ /dev/null @@ -1,5 +0,0 @@ -package org.firstinspires.ftc.teamcode.util; - -public interface External { - boolean shouldStop(); -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Interpolator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Interpolator.java deleted file mode 100644 index 22eb91ac..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Interpolator.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.firstinspires.ftc.teamcode.util; - -public class Interpolator { - - - /*public Interpolator(double input_start, double input_end, double output_start, double output_end) { - - - } - - public double interpolate(double input) { - - }*/ - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Lambda.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Lambda.java deleted file mode 100644 index 51e5f621..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Lambda.java +++ /dev/null @@ -1,5 +0,0 @@ -package org.firstinspires.ftc.teamcode.util; - -public interface Lambda { - void run(T parameter); -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java index 8338eb1c..5dc2bd41 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java @@ -18,7 +18,6 @@ import org.firstinspires.ftc.robotcore.internal.system.AppUtil; import org.firstinspires.ftc.teamcode.drive.DriveConstants; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer; import java.io.File; @@ -74,15 +73,15 @@ public static class LogFile { public double mecHeadingD = SampleMecanumDrive.HEADING_PID.kD; public double mecLateralMultiplier = SampleMecanumDrive.LATERAL_MULTIPLIER; - public double tankAxialP = SampleTankDrive.AXIAL_PID.kP; - public double tankAxialI = SampleTankDrive.AXIAL_PID.kI; - public double tankAxialD = SampleTankDrive.AXIAL_PID.kD; - public double tankCrossTrackP = SampleTankDrive.CROSS_TRACK_PID.kP; - public double tankCrossTrackI = SampleTankDrive.CROSS_TRACK_PID.kI; - public double tankCrossTrackD = SampleTankDrive.CROSS_TRACK_PID.kD; - public double tankHeadingP = SampleTankDrive.HEADING_PID.kP; - public double tankHeadingI = SampleTankDrive.HEADING_PID.kI; - public double tankHeadingD = SampleTankDrive.HEADING_PID.kD; +// public double tankAxialP = SampleTankDrive.AXIAL_PID.kP; +// public double tankAxialI = SampleTankDrive.AXIAL_PID.kI; +// public double tankAxialD = SampleTankDrive.AXIAL_PID.kD; +// public double tankCrossTrackP = SampleTankDrive.CROSS_TRACK_PID.kP; +// public double tankCrossTrackI = SampleTankDrive.CROSS_TRACK_PID.kI; +// public double tankCrossTrackD = SampleTankDrive.CROSS_TRACK_PID.kD; +// public double tankHeadingP = SampleTankDrive.HEADING_PID.kP; +// public double tankHeadingI = SampleTankDrive.HEADING_PID.kI; +// public double tankHeadingD = SampleTankDrive.HEADING_PID.kD; public double trackingTicksPerRev = StandardTrackingWheelLocalizer.TICKS_PER_REV; public double trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/NeverStops.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/NeverStops.java deleted file mode 100644 index ed754989..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/NeverStops.java +++ /dev/null @@ -1,9 +0,0 @@ -package org.firstinspires.ftc.teamcode.util; - -public class NeverStops implements External { - - @Override - public boolean shouldStop() { - return false; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/demos/BroadcastDemo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/demos/BroadcastDemo.java deleted file mode 100644 index 8a202aa4..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/demos/BroadcastDemo.java +++ /dev/null @@ -1,73 +0,0 @@ -package org.firstinspires.ftc.teamcode.util.demos; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.util.BroadcastHandler; -import org.firstinspires.ftc.teamcode.util.Logger; - -import java.util.concurrent.atomic.AtomicReference; - -@Autonomous -@Disabled -public class BroadcastDemo extends LinearOpMode { - @Override - public void runOpMode() { - Logger logger = new Logger(telemetry); - - // You need to wrap all variables you want to be modifiable be the handlers in AtomicReferences - AtomicReference stopped = new AtomicReference<>(false); - AtomicReference color = new AtomicReference<>(""); - - // You can pass a single command to execute - // Note the use of .set because they are AtomicReferences - new BroadcastHandler("stop", intent -> stopped.set(true)); - - // Or multiple enclosed in braces - new BroadcastHandler("setColor", intent -> { - if (intent.getStringExtra("color") != null) { - color.set(intent.getStringExtra("color")); - } - }); - - new BroadcastHandler("log", intent -> { - String msg = intent.getStringExtra("msg"); - if (msg != null) { - String level = intent.getStringExtra("level"); - if (level != null) { - switch (level) { - case "info": - logger.info(msg); - break; - case "warning": - logger.warning(msg); - break; - case "error": - logger.error(msg); - break; - case "debug": - logger.debug(msg); - break; - } - } else { - logger.info(msg); - } - } else { - logger.debug("Log was called without parameters."); - } - }); - - logger.info("Use command: adb shell am broadcast -a [handler name] --es [parameter name] \\\"[parameter value]\\\""); - logger.info(" to send messages to handlers. Try passing something to the color parameter of setColor to see it appear above."); - - waitForStart(); - - while (opModeIsActive() && !stopped.get()) { - // Note the use of .get because they are AtomicReferences - telemetry.addData("Color", color.get()); - telemetry.update(); - sleep(50); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/demos/ConfigurationDemo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/demos/ConfigurationDemo.java deleted file mode 100644 index 75587fc4..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/demos/ConfigurationDemo.java +++ /dev/null @@ -1,33 +0,0 @@ -package org.firstinspires.ftc.teamcode.util.demos; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.util.Configuration; -import org.firstinspires.ftc.teamcode.util.Configurator; - -@TeleOp(name = "Configuration Demo", group = "Software Demos") -@Disabled -public class ConfigurationDemo extends LinearOpMode { - - @Override - public void runOpMode() throws InterruptedException { - // Load the configuration from the JSON file, or create a blank one if it does not exist - Configuration config = Configurator.load(); // Note the use of Configurat*or* not Configuration - - // Check if the value you want is null (meaning it was never set before) - if (config.target == null) { - // Do something sensible? - // We can set the value like this: - //config.target = new HSVColor(1.0,1.0,1.0); - } - - // Save the config when you're done modifying it. - // No need to save if you only read from it, though. - Configurator.save(config); - - // NOTE: - // If you want to add new configuration values, you need to modify the Configuration class. - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/demos/TeamElementDemo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/demos/TeamElementDemo.java deleted file mode 100644 index f543018a..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/demos/TeamElementDemo.java +++ /dev/null @@ -1,59 +0,0 @@ -package org.firstinspires.ftc.teamcode.vision.demos; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.util.Configuration; -import org.firstinspires.ftc.teamcode.util.Configurator; -import org.firstinspires.ftc.teamcode.vision.HSVColor; -import org.firstinspires.ftc.teamcode.vision.TeamElementDetector; -import org.firstinspires.ftc.teamcode.robot.hardware.Webcam; - - -@Autonomous -@Disabled -public class TeamElementDemo extends LinearOpMode { - @Override - public void runOpMode() - { - // Get the webcam from the hardware map - Webcam webcam = new Webcam("Webcam 1", hardwareMap); - Configuration config = Configurator.load(); - - HSVColor target = config.target; - - if (target == null) { - telemetry.log().add("WARNING WARNING WARNING:"); - telemetry.log().add("TARGET COLOR WAS NOT CALIBRATED!!!"); - telemetry.log().add("Please run the Calibrate Target Color OpMode!"); - android.util.Log.w("TeamElementDemo", "Target Color was NOT CALIBRATED! Falling back to default"); - target = new HSVColor(0.0,0.0,0.0); - // Save that to the file permanently, so we don't get the warning next time? - //config.target = target; - //Configurator.save(config); - } - - // Setup the detector pipeline - TeamElementDetector detector = new TeamElementDetector(config); - webcam.setPipeline(detector); - - // Open the camera - webcam.open(); - - // Wait for the OpMode to start - waitForStart(); - - // While the OpMode is running - while (opModeIsActive()) - { - // Log the result of our analysis - telemetry.addData("Analysis", detector.getAnalysis()); - telemetry.update(); - - // Don't burn CPU cycles busy-looping - // Normally, more processing would go here - sleep(50); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/pole/DoubleThresholdPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/pole/DoubleThresholdPipeline.java index 217f4519..a92dff00 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/pole/DoubleThresholdPipeline.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/pole/DoubleThresholdPipeline.java @@ -1,8 +1,6 @@ package org.firstinspires.ftc.teamcode.vision.pole; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.autonomous.AllAutomovement; -import org.firstinspires.ftc.teamcode.util.Logger; import org.opencv.core.Core; import org.opencv.core.Mat; import org.opencv.core.MatOfPoint; @@ -326,7 +324,7 @@ public Mat processFrame(Mat input) { } // TODO: Remove or abstract - Imgproc.line(input, new Point(AllAutomovement.RotationPID.TARGET, 0), new Point(AllAutomovement.RotationPID.TARGET, input.height()), new Scalar(0, 0, 255), 2); + //Imgproc.line(input, new Point(AllAutomovement.RotationPID.TARGET, 0), new Point(AllAutomovement.RotationPID.TARGET, input.height()), new Scalar(0, 0, 255), 2); poleX.set(-1); poleWidth.set(-1.0);