From c981d470a43d5c2efdf9689888567aa7292d707d Mon Sep 17 00:00:00 2001 From: ChiriacIoana Date: Mon, 23 Feb 2026 19:48:58 +0200 Subject: [PATCH 01/13] pose is put in the file --- .../ftc/teamcode/kronbot/KronBot.java | 4 +-- .../kronbot/autonomous/Auto_BackBlueOp.java | 5 ++-- .../kronbot/manual/MainDrivingOp.java | 2 +- .../teamcode/kronbot/utils/PoseStorage.java | 25 ++++--------------- 4 files changed, 11 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/KronBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/KronBot.java index b8e88e1..41a5693 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/KronBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/KronBot.java @@ -106,8 +106,8 @@ public void initFollower(HardwareMap hardwareMap, Pose startingPose) { public void initHardware(HardwareMap hardwareMap) { - if(follower != null) - initFollower(hardwareMap); +// if(follower != null) +// initFollower(hardwareMap); initMotors(hardwareMap); initServos(hardwareMap); initSensors(hardwareMap); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java index 30483ec..7d6f3d5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java @@ -217,8 +217,7 @@ public void autonomousPathUpdate() { case -1: // Idle / done - Pose finalPose = robot.follower.getPose(); - PoseStorage.savePose(finalPose); + break; } } @@ -231,6 +230,8 @@ public void setPathState(int newState) { @Override public void stop() { + Pose finalPose = robot.follower.getPose(); + PoseStorage.savePose(finalPose); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java index 6f602e2..396dc2d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java @@ -57,7 +57,7 @@ public class MainDrivingOp extends OpMode { public void init(){ lpsCounter = new LpsCounter(); lpsCounter.getLoopTime(); - robot.initFollower(hardwareMap); + robot.initFollower(hardwareMap, true); robot.init(hardwareMap); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/PoseStorage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/PoseStorage.java index 00ebddd..3056b79 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/PoseStorage.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/PoseStorage.java @@ -11,48 +11,33 @@ public class PoseStorage { private static final String FILE_NAME = "lastPose.txt"; - // SAVE pose to external storage + //save the pose public static void savePose(Pose pose) { + File file = new File(Environment.getExternalStorageDirectory().getPath() + "/" + FILE_NAME); - String path = Environment.getExternalStorageDirectory().getPath() - + "/" + FILE_NAME; - - try (PrintWriter writer = new PrintWriter(new FileWriter(path))) { - + try (PrintWriter writer = new PrintWriter(new FileWriter(file))) { writer.println(pose.getX()); writer.println(pose.getY()); writer.println(pose.getHeading()); - - RobotLog.ii("PoseStorage", "Pose saved to " + path); - } catch (IOException e) { RobotLog.ee("PoseStorage", "Failed to save pose", e); } } - // LOAD pose from external storage + //load the pose public static Pose loadPose() { - - String path = Environment.getExternalStorageDirectory().getPath() - + "/" + FILE_NAME; - - File file = new File(path); + File file = new File(Environment.getExternalStorageDirectory().getPath() + "/" + FILE_NAME); if (!file.exists()) { - RobotLog.ww("PoseStorage", "Pose file not found"); return new Pose(0, 0, 0); } try (BufferedReader reader = new BufferedReader(new FileReader(file))) { - double x = Double.parseDouble(reader.readLine()); double y = Double.parseDouble(reader.readLine()); double heading = Double.parseDouble(reader.readLine()); - RobotLog.ii("PoseStorage", "Pose loaded from " + path); - return new Pose(x, y, heading); - } catch (Exception e) { RobotLog.ee("PoseStorage", "Failed to load pose", e); return new Pose(0, 0, 0); From 57c135e7de035fee31a36b88cbc65700e0842054 Mon Sep 17 00:00:00 2001 From: Mihai Date: Mon, 23 Feb 2026 21:44:47 +0200 Subject: [PATCH 02/13] Made autoAim code for hoot angle and outtake velocity using linear interpolation. Refactored some code. --- .../ftc/teamcode/kronbot/Robot.java | 251 +++++++++++------- .../kronbot/autonomous/Auto_BackRedOp.java | 6 +- .../kronbot/manual/MainDrivingOp.java | 142 +++++----- .../manual/OuttakeShootingTesting.java | 4 +- .../ftc/teamcode/kronbot/utils/Constants.java | 5 + 5 files changed, 229 insertions(+), 179 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java index 101f6e0..a221508 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java @@ -5,7 +5,9 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.R; import org.firstinspires.ftc.teamcode.kronbot.utils.detection.AprilTagWebcam; +import org.opencv.core.Mat; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_CLOSE; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_MAX; @@ -17,15 +19,19 @@ import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KF; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KI; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KP; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_1; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_1_ANGLE; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_1_KS; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_1_VELOCITY; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_ANGLE; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_KS; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_VELOCITY; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_3; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_3_ANGLE; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_3_KS; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_3_VELOCITY; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_4; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_4_ANGLE; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_4_KS; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_4_VELOCITY; @@ -35,6 +41,14 @@ import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.maxVelocity; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.minVelocity; +import android.util.Pair; + +import java.util.ArrayList; +import java.util.Dictionary; +import java.util.Enumeration; +import java.util.Map; +import java.util.TreeMap; + public class Robot extends KronBot { // Singleton instance private static Robot instance = null; @@ -50,6 +64,18 @@ public class Robot extends KronBot { public final Shoot shoot; public final Heading heading; + public static class RangeConfig { + public double angle; + public double velocity; + public double kS; + + public RangeConfig(double angle, double velocity, double kS) { + this.angle = angle; + this.velocity = velocity; + this.kS = kS; + } + } + // Private constructor @@ -95,6 +121,9 @@ public void initSystems(HardwareMap hardwareMap) { // Updates all systems // Except pedro public void updateAllSystems() { + double rawHeading = follower.getHeading(); + heading.update(rawHeading); + outtake.update(); intake.update(); loader.update(); @@ -102,9 +131,6 @@ public void updateAllSystems() { flap.update(); follower.update(); - double rawHeading = follower.getHeading(); - heading.update(rawHeading); - double filtered = heading.get(); /** eg usage for turret calculations: double robotRelativeAngle = fieldRelativeAngle - filtered; @@ -117,56 +143,64 @@ public void updateAllSystems() { // webcam.update(); } + static final double basket_X = 130; + static final double basket_Y = 135; + + public class Outtake { public boolean on = false; - public double angle = 0; - public double autoAimAngle = 0; - public double velocity; - public double kS = 0; + public RangeConfig activeConfig; +// RangeConfig autoAimConfig; public boolean reversed = false; boolean braking = false; - double lastVelocity = 0; +// double lastVelocity = 0; + private TreeMap ranges; public void init() { on = false; reversed = false; - velocity = minVelocity; + activeConfig = new RangeConfig(0,0,0); + + //Initialize Range based shooter settings + ranges.put(RANGE_1, new RangeConfig(RANGE_1_ANGLE, RANGE_1_VELOCITY, RANGE_1_KS)); + ranges.put(RANGE_2, new RangeConfig(RANGE_2_ANGLE, RANGE_2_VELOCITY, RANGE_2_KS)); + ranges.put(RANGE_3, new RangeConfig(RANGE_3_ANGLE, RANGE_3_VELOCITY,RANGE_3_KS)); + ranges.put(RANGE_4, new RangeConfig(RANGE_4_ANGLE, RANGE_4_VELOCITY, RANGE_4_KS)); } /** Configures the launch angle and launch motor speed for the given distance.
* Returns true if a good configuration is possible (If the distance is in the correct range) - * @param distance The distance, measured horizontally, from the tower wall to the center of the turret. * @return Returns true if a configuration is possible */ - public boolean configureDistance(double distance) { - if(distance < 20) - return false; - double shooterVel = 0; - double servoAngle = 0; - - // magic numbers for quadratics from desmos - if(distance < 46) - servoAngle = 0; - else if(distance < 150) - servoAngle = -0.0000557692 * (distance * distance) + 0.0181423 * distance - 0.716538; - else - servoAngle = 0.75; - - if(distance < 215) { - shooterVel = -0.00460596 * (distance * distance) + 3.42411 * distance + 752.43325; - } - else if(distance > 250 && distance < 375) - shooterVel = 1400; // todo: check if this speed works - - on = true; - velocity = shooterVel; - angle = servoAngle; - - return true; - } +// public boolean configureDistance(double distance) { +// if(distance < 20) +// return false; +// double shooterVel = 0; +// double servoAngle = 0; +// +// // magic numbers for quadratics from desmos +// if(distance < 46) +// servoAngle = 0; +// else if(distance < 150) +// servoAngle = -0.0000557692 * (distance * distance) + 0.0181423 * distance - 0.716538; +// else +// servoAngle = 0.75; +// +// if(distance < 215) { +// shooterVel = -0.00460596 * (distance * distance) + 3.42411 * distance + 752.43325; +// } +// else if(distance > 250 && distance < 375) +// shooterVel = 1400; // todo: check if this speed works +// +// on = true; +// velocity = shooterVel; +// angle = servoAngle; +// +// return true; +// } public void update(){ @@ -174,22 +208,22 @@ public void update(){ if(on){ leftOuttake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); rightOuttake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - if(leftOuttake.getVelocity() < velocity * 1) { + if(leftOuttake.getVelocity() < activeConfig.velocity * 1) { leftOuttake.setPower(1); rightOuttake.setPower(1); } - else if(leftOuttake.getVelocity() > velocity * 1.1) { + else if(leftOuttake.getVelocity() > activeConfig.velocity * 1.1) { if(braking) { leftOuttake.setPower(0); rightOuttake.setPower(0); } - leftOuttake.setPower(kS * 0.8); - rightOuttake.setPower(kS * 0.8); + leftOuttake.setPower(activeConfig.kS * 0.8); + rightOuttake.setPower(activeConfig.kS * 0.8); } else { braking = false; - leftOuttake.setPower(kS); - rightOuttake.setPower(kS); + leftOuttake.setPower(activeConfig.kS); + rightOuttake.setPower(activeConfig.kS); } } else { @@ -244,19 +278,71 @@ else if(rightOuttake.getVelocity() < 500) { */ } - angleServo.setPosition(Math.min(Math.max(angle, ANGLE_SERVO_MIN), ANGLE_SERVO_MAX)); + angleServo.setPosition(Math.min(Math.max(activeConfig.angle, ANGLE_SERVO_MIN), ANGLE_SERVO_MAX)); + } + + + + public RangeConfig interpolateRange() { + double robot_X = follower.getPose().getX(); + double robot_Y = follower.getPose().getY(); + + double dx = basket_X - robot_X; + double dy = basket_Y - robot_Y; + + double distance = Math.sqrt(dx*dx + dy*dy); + + if(distance lower = ranges.floorEntry(distance); + Map.Entry upper = ranges.ceilingEntry(distance); + + if (lower == null && upper!=null) return upper.getValue(); + if (upper == null && lower!=null) return lower.getValue(); + + if (lower.getKey().equals(upper.getKey())) + return lower.getValue(); + + //Linear interpolation between the two ranges + double d1 = lower.getKey(); + double d2 = upper.getKey(); + + double angle1 = lower.getValue().angle; + double vel1 = lower.getValue().velocity; + double kS1 = lower.getValue().kS; + + double angle2 = upper.getValue().angle; + double vel2 = upper.getValue().velocity; + double kS2 = upper.getValue().kS; + + double t = (distance - d1) / (d2 - d1); + + double interpAngle = angle1 + (angle2 - angle1) * t; + double interpVel = vel1 + (vel2 - vel1) * t; + double interpKs = kS1 + (kS2 - kS1) * t; + + //Clamp manual ca Math.Clamp nu merge cu double + interpAngle = Math.max(ANGLE_SERVO_MIN, interpAngle); + interpAngle = Math.min(ANGLE_SERVO_MAX, interpAngle); + + return new RangeConfig(interpAngle, interpVel, interpKs); } public void telemetry(Telemetry telemetry) { telemetry.addLine("=== OUTTAKE STATUS ==="); telemetry.addData("On", on); telemetry.addData("Reversed", reversed); - telemetry.addData("Target Velocity", "%.0f", velocity); + telemetry.addData("Target Velocity", "%.0f", activeConfig.velocity); telemetry.addData("Left Velocity", "%.0f", leftOuttake.getVelocity()); telemetry.addData("Left Power", "%.3f", leftOuttake.getPower()); telemetry.addData("Right Velocity", "%.0f", rightOuttake.getVelocity()); telemetry.addData("Right Power", "%.3f", rightOuttake.getPower()); - telemetry.addData("Angle", "%.3f", angle); + telemetry.addData("Angle", "%.3f", activeConfig.angle); telemetry.addData("Angle Servo Pos", "%.3f", angleServo.getPosition()); } @@ -328,10 +414,11 @@ public void update() { if (turretServo == null || follower == null) return; if(autoAimEnabled) { + + //Turret angle double robot_X = follower.getPose().getX(); double robot_Y = follower.getPose().getY(); - double robotHeading =follower.getPose().getHeading(); - + double robotHeading = heading.get(); double dx = basket_X - robot_X; double dy = basket_Y - robot_Y; @@ -348,6 +435,8 @@ public void update() { ); servoPosition = robotRelativeAngle * TURRET_SERVO_UNITS_PER_RAD + 0.5; + + } else { servoPosition = driverOffset * TURRET_SERVO_UNITS_PER_RAD + 0.5; @@ -457,84 +546,50 @@ public void telemetry(Telemetry telemetry) { } - - public class ShootClose { - - public void activate() { - // Turn shooter on - outtake.on = true; - - // Set shooter velocity - outtake.velocity = minVelocity; - - // Set angle servo - outtake.angle = ANGLE_SERVO_CLOSE; - - // Set turret position - //turret.angle = TURRET_SERVO_MIN; - } - - public void deactivate() { - outtake.on = false; - } - } - - public class ShootFar { - - public void activate() { - outtake.on = true; - outtake.velocity = maxVelocity; - outtake.angle = ANGLE_SERVO_MAX; - //turret.angle = TURRET_SERVO_MAX; - } - - public void deactivate() { - outtake.on = false; - } - } - public class Shoot { - public void activateRange(int range, Gamepad gamepad) { + public void activateRange(int range) { + RangeConfig config; switch (range) { case 1: outtake.on = true; - outtake.velocity = RANGE_1_VELOCITY; - outtake.angle = RANGE_1_ANGLE; - outtake.kS = RANGE_1_KS; + config = new RangeConfig(RANGE_1_ANGLE, RANGE_1_VELOCITY, RANGE_1_KS); //if(outtake.velocity>=RANGE_1_VELOCITY-100) // gamepad.rumble(1, 0, 100); break; case 2: outtake.on = true; - outtake.velocity = RANGE_2_VELOCITY; - outtake.angle = RANGE_2_ANGLE; - outtake.kS = RANGE_2_KS; + config = new RangeConfig(RANGE_2_ANGLE, RANGE_2_VELOCITY, RANGE_2_KS); //if(outtake.velocity>=RANGE_2_VELOCITY-100) // gamepad.rumble(1, 0, 100); break; case 3: outtake.on = true; - outtake.velocity = RANGE_3_VELOCITY; - outtake.angle = RANGE_3_ANGLE; - outtake.kS = RANGE_3_KS; + config = new RangeConfig(RANGE_3_ANGLE, RANGE_3_VELOCITY, RANGE_3_KS); //if(outtake.velocity>=RANGE_3_VELOCITY-100) // gamepad.rumble(1, 0, 100); break; case 4: outtake.on = true; - outtake.velocity = RANGE_4_VELOCITY; - outtake.angle = RANGE_4_ANGLE; - outtake.kS = RANGE_4_KS; + config = new RangeConfig(RANGE_4_ANGLE, RANGE_4_VELOCITY, RANGE_4_KS); //if(outtake.velocity>=RANGE_4_VELOCITY-100) // gamepad.rumble(1, 0, 100); + break; + case 0: + outtake.on = true; + config = outtake.interpolateRange(); + break; + + default: + outtake.on=false; + outtake.activeConfig = new RangeConfig(0,0,0); + break; } } public void deactivate() { outtake.on = false; - outtake.velocity = 0; - outtake.angle = ANGLE_SERVO_MIN; + outtake.activeConfig = new RangeConfig(0,0,0); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java index 1f07d41..b5c1e73 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java @@ -121,8 +121,8 @@ public void autonomousPathUpdate() { // Start outtake motors if(pathTimer.getElapsedTimeSeconds() >= 0.5) { robot.outtake.on = true; - robot.outtake.velocity = launchSpeedBack; - robot.outtake.kS = 0.5; // magic number from constants + robot.outtake.activeConfig.velocity = launchSpeedBack; + robot.outtake.activeConfig.kS = 0.5; // magic number from constants robot.flap.open = true; launchState++; robot.updateAllSystems(); @@ -135,7 +135,7 @@ public void autonomousPathUpdate() { // Wait for motors to reach speed and launch 1 if (motorVel+40 >= launchSpeedBack) { robot.loaderServo.runContinuous(false, true); - robot.outtake.angle = 1; + robot.outtake.activeConfig.angle = 1; launchState++; robot.updateAllSystems(); pathTimer.resetTimer(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java index 396dc2d..82bd463 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java @@ -37,8 +37,8 @@ @TeleOp(name = "Main Driving", group = Constants.MAIN_GROUP) public class MainDrivingOp extends OpMode { private final Robot robot = Robot.getInstance(); - private Controls drivingGP; - private Controls utilityGP; + private Controls drivingGP; + private Controls utilityGP; private TurretAligner turretAligner; @@ -54,7 +54,7 @@ public class MainDrivingOp extends OpMode { boolean rumbled = false; @Override - public void init(){ + public void init() { lpsCounter = new LpsCounter(); lpsCounter.getLoopTime(); robot.initFollower(hardwareMap, true); @@ -83,7 +83,7 @@ public void init(){ } @Override - public void init_loop(){ + public void init_loop() { lpsCounter.getLoopTime(); telemetry.addLine("Initialization Ready"); @@ -91,14 +91,14 @@ public void init_loop(){ } @Override - public void start(){ + public void start() { robot.follower.startTeleopDrive(); } @Override - public void loop(){ + public void loop() { // Update Loops/s delta lpsCounter.getLoopTime(); @@ -116,16 +116,15 @@ public void loop(){ turretAligner.update(); //Loader - if(!drivingGP.rightBumper.pressed()) { + if (!drivingGP.rightBumper.pressed()) { robot.loader.speed = utilityGP.leftStick.y; robot.flap.open = false; - } - else { + } else { robot.loader.speed = drivingGP.rightTrigger - drivingGP.leftTrigger; robot.flap.open = true; - if(robot.loader.speed > 0.1) + if (robot.loader.speed > 0.1) robot.intake.speed = INTAKE_DRIVER_POWER; - else if(robot.loader.speed < -0.2) + else if (robot.loader.speed < -0.2) robot.intake.speed = INTAKE_DRIVER_REVERSE; else robot.intake.speed = 0; @@ -135,95 +134,86 @@ else if(robot.loader.speed < -0.2) //autoAim.telemetry(telemetry, tag); //Turret/Angle aiming - if(autoAimEnabled){ - //To do -// robot.webcam.update(); - - //robot.turret.angle = autoAim.calculateServoPosition(tag); - - } else { - //Turret aiming - if(drivingGP.dpadLeft.pressed()) { - //if button is pressed for longer, increase increment - if (turretTimer.seconds() == 0) { - turretTimer.reset(); - } - double increment = 0.03; + //Turret aiming + if (drivingGP.dpadLeft.pressed()) { - if (turretTimer.seconds() > 1) { - increment = 0.07; - } - - if (turretTimer.seconds() > 1.5) { - increment = 0.1; - } - robot.turret.driverOffset += increment; + //if button is pressed for longer, increase increment + if (turretTimer.seconds() == 0) { + turretTimer.reset(); + } + double increment = 0.03; + if (turretTimer.seconds() > 1) { + increment = 0.07; } - else if(drivingGP.dpadRight.pressed()) { + if (turretTimer.seconds() > 1.5) { + increment = 0.1; + } + robot.turret.driverOffset += increment; - double decrement = 0.03; + } else if (drivingGP.dpadRight.pressed()) { - if (turretTimer.seconds() == 0) { - turretTimer.reset(); - } + double decrement = 0.03; - if (turretTimer.seconds() > 1) { - decrement = 0.07; - } + if (turretTimer.seconds() == 0) { + turretTimer.reset(); + } - if (turretTimer.seconds() > 1.5) { - decrement = 0.1; - } + if (turretTimer.seconds() > 1) { + decrement = 0.07; + } - robot.turret.driverOffset -= decrement; - } else { - turretTimer.reset(); + if (turretTimer.seconds() > 1.5) { + decrement = 0.1; } - //Angle aiming - if(drivingGP.dpadUp.pressed()) - robot.outtake.angle += 0.01; - else if(drivingGP.dpadDown.pressed()) - robot.outtake.angle -= 0.01; + robot.turret.driverOffset -= decrement; + } else { + turretTimer.reset(); } +// //Angle aiming +// if(drivingGP.dpadUp.pressed()) +// robot.outtake.activeConfig.angle += 0.01; +// else if(drivingGP.dpadDown.pressed()) +// robot.outtake.activeConfig.angle -= 0.01; + + if(drivingGP.dpadDown.justPressed()) + robot.turret.autoAimEnabled = !robot.turret.autoAimEnabled; + + if(drivingGP.dpadUp.justPressed()) + robot.shoot.activateRange(0); //Shoot Close/Far if (drivingGP.triangle.justPressed()) { - robot.turret.autoAimEnabled = false; - robot.shoot.activateRange(1, gamepad1); + robot.shoot.activateRange(1); } - if(drivingGP.square.justPressed()) { - robot.turret.autoAimEnabled = false; - robot.shoot.activateRange(2, gamepad1); + if (drivingGP.square.justPressed()) { + robot.shoot.activateRange(2); } if (drivingGP.cross.justPressed()) { - robot.turret.autoAimEnabled = false; - robot.shoot.activateRange(3, gamepad1); + robot.shoot.activateRange(3); } - if(drivingGP.circle.justPressed()) { - robot.turret.autoAimEnabled = false; - robot.shoot.activateRange(4, gamepad1); + if (drivingGP.circle.justPressed()) { + robot.shoot.activateRange(4); } - if( robot.outtake.on && - robot.leftOuttake.getVelocity() >= robot.outtake.velocity - 30 && - robot.leftOuttake.getVelocity() <= robot.outtake.velocity + 90 ) - { + if (robot.outtake.on && + robot.leftOuttake.getVelocity() >= robot.outtake.activeConfig.velocity - 30 && + robot.leftOuttake.getVelocity() <= robot.outtake.activeConfig.velocity + 90) { gamepad1.rumble(1, 0, 150); rumbled = true; } - if(!autoAimEnabled && drivingGP.leftBumper.justPressed()) { - robot.turret.autoAimEnabled = true; - if(robot.outtake.on) { - robot.shoot.deactivate(); - gamepad1.rumble(1, 1, 100); - rumbled = false; - } - } +// if (!autoAimEnabled && drivingGP.leftBumper.justPressed()) { +// robot.turret.autoAimEnabled = true; +// if (robot.outtake.on) { +// robot.shoot.deactivate(); +// gamepad1.rumble(1, 1, 100); +// rumbled = false; +// } +// } //Update robot systems status robot.follower.setTeleOpDrive(-drivingGP.leftStick.y, -drivingGP.leftStick.x, -drivingGP.rightStick.x, true); @@ -234,11 +224,11 @@ else if(drivingGP.dpadDown.pressed()) @Override - public void stop(){ + public void stop() { robot.webcam.stop(); } - public void _telemetry(){ + public void _telemetry() { telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta); telemetry.addData("x", robot.follower.getPose().getX()); telemetry.addData("y", robot.follower.getPose().getY()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/OuttakeShootingTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/OuttakeShootingTesting.java index f8c1900..300577d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/OuttakeShootingTesting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/OuttakeShootingTesting.java @@ -85,8 +85,8 @@ public void loop(){ telemetry.addData("Turret Angle", "%.4f", robot.turret.angle); robot.outtake.on = true; - robot.outtake.velocity = shooterVel; - robot.outtake.angle = anglePos; + robot.outtake.activeConfig.velocity = shooterVel; + robot.outtake.activeConfig.angle = anglePos; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java index 56f264c..5f2f75f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java @@ -61,18 +61,23 @@ public class Constants { public static double minVelocity = 1140; public static double maxVelocity = 1500; + public static double RANGE_1 = 10; public static double RANGE_1_ANGLE = 0; public static double RANGE_1_VELOCITY = 1150; public static double RANGE_1_KS = 0.15; + public static double RANGE_2 = 30; public static double RANGE_2_ANGLE = 0.3; public static double RANGE_2_VELOCITY = 1280; public static double RANGE_2_KS = 0.2; + public static double RANGE_3 = 50; public static double RANGE_3_ANGLE = 0.72; public static double RANGE_3_VELOCITY = 1350; public static double RANGE_3_KS = 0.3; + + public static double RANGE_4 = 100; public static double RANGE_4_ANGLE = 0.72; public static double RANGE_4_VELOCITY = 1400; public static double RANGE_4_KS = 0.5; From f76720012ff59f8523518839edf70fe0dd83b505 Mon Sep 17 00:00:00 2001 From: ChiriacIoana Date: Mon, 23 Feb 2026 22:04:25 +0200 Subject: [PATCH 03/13] nine artefact close auto to be tested --- .../kronbot/autonomous/Auto_CloseRedOp.java | 166 +++++++++++------- .../autonomous/AutonomousConstants.java | 18 +- 2 files changed, 115 insertions(+), 69 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java index 8e00891..45e9224 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java @@ -1,6 +1,8 @@ package org.firstinspires.ftc.teamcode.kronbot.autonomous; import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.*; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_CLOSE; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN; import com.acmerobotics.dashboard.FtcDashboard; @@ -26,16 +28,19 @@ public class Auto_CloseRedOp extends OpMode { private int launchState; // Define poses - Pose startingPose = coordinates(StartingPoseCloseRed); - Pose launchZone = coordinates(LaunchZoneClose); - Pose launchZone2 = coordinates(LaunchZoneClose2); + Pose start = coordinates(StartingPoseCloseRed); + Pose launch1 = coordinates(LaunchZoneClose1); + Pose launch2 = coordinates(LaunchZoneClose2); + Pose intake1 = coordinates(IntakeZoneClose1); + Pose load1 = coordinates(LoadZoneClose1); + Pose parkZone = coordinates(ParkClose); private double motorVel; // Paths and PathChains - private PathChain goToLaunch, goToPark; + private PathChain goToLaunch1, goToLaunch2, goToIntake1, goToLoad1, goToPark; @Override public void init() { @@ -43,7 +48,10 @@ public void init() { robot.init(hardwareMap); - robot.initFollower(hardwareMap, startingPose); + robot.initFollower(hardwareMap, start); + robot.flapsServo.setPosition(FLAP_OPEN); + robot.angleServo.setPosition(ANGLE_SERVO_CLOSE); + robot.turretServo.setPosition(0.7); pathTimer = new Timer(); opmodeTimer = new Timer(); @@ -63,16 +71,30 @@ public void init() { public void buildPaths() { - goToLaunch = robot.follower.pathBuilder() - .addPath(new BezierLine(startingPose, launchZone)) - .setLinearHeadingInterpolation(startingPose.getHeading(), launchZone.getHeading()) - .addPath(new BezierLine(launchZone, launchZone2)) - .setLinearHeadingInterpolation(launchZone.getHeading(), launchZone2.getHeading()) + goToLaunch1 = robot.follower.pathBuilder() + .addPath(new BezierLine(start, launch1)) + .setLinearHeadingInterpolation(start.getHeading(), launch1.getHeading()) + .build(); + + goToIntake1 = robot.follower.pathBuilder() + .addPath(new BezierLine(launch1, intake1)) + .setLinearHeadingInterpolation(launch1.getHeading(), intake1.getHeading()) + .build(); + + goToLoad1 = robot.follower.pathBuilder() + .addPath(new BezierLine(intake1, load1)) + .setLinearHeadingInterpolation(intake1.getHeading(), load1.getHeading()) + .build(); + + goToLaunch2 = robot.follower.pathBuilder() + .addPath(new BezierLine(load1, launch2)) + .setLinearHeadingInterpolation(load1.getHeading(), launch2.getHeading()) .build(); + goToPark = robot.follower.pathBuilder() - .addPath(new BezierLine(startingPose, parkZone)) - .setLinearHeadingInterpolation(startingPose.getHeading(), parkZone.getHeading()) + .addPath(new BezierLine(start, parkZone)) + .setLinearHeadingInterpolation(start.getHeading(), parkZone.getHeading()) .build(); } @@ -115,104 +137,119 @@ public void autonomousPathUpdate() { switch (pathState) { case 0: - robot.follower.followPath(goToPark); - setPathState(-1); + //also start motors to save time + robot.leftOuttake.setVelocity(launchSpeedClose); + robot.rightOuttake.setVelocity(launchSpeedClose); + //go to pose + robot.follower.followPath(goToLaunch1); + setPathState(1); break; case 1: - /* if (!robot.follower.isBusy()) { switch (launchState) { case 0: // Start outtake motors - robot.leftOuttake.setVelocity(launchSpeedClose); - robot.flapsServo.setPosition(FLAP_OPEN); launchState++; pathTimer.resetTimer(); break; case 1: - // Wait for motors to reach speed and launch 1 - if (motorVel+100 >= launchSpeedClose && motorVel+100 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 4.0) { - robot.loaderServo.runContinuous(false, true); + // Wait for motors to reach speed and launch first 2 + if (motorVel + 100 >= launchSpeedClose && motorVel + 100 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.loaderMotor.setPower(1); launchState++; pathTimer.resetTimer(); } break; case 2: - // Use color sensor to detect when ball is launched and stop servo (+timer for fallback safety) + // intake on and launch the third if (pathTimer.getElapsedTimeSeconds() > 1.0) { - robot.loaderServo.runContinuous(false, false); + robot.intakeMotor.setPower(1); launchState++; pathTimer.resetTimer(); } break; case 3: - // Launch 2 - if (motorVel+100 >= launchSpeedClose && motorVel+100 >= launchSpeedClose) { - robot.loaderServo.runContinuous(false, true); - launchState++; + // timer to see when all 3 are launched + if (pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.intakeMotor.setPower(0); + robot.loaderMotor.setPower(0); + launchState = -1; pathTimer.resetTimer(); } break; + case -1: + break; + } + } + case 2: + if (!robot.follower.isBusy()) { + robot.follower.followPath(goToIntake1); + robot.intakeMotor.setPower(1); + robot.loaderMotor.setPower(1); + setPathState(3); + } + break; - case 4: - // Stop servo between shots - if (pathTimer.getElapsedTimeSeconds() > 1.0) { - robot.loaderServo.runContinuous(false, false); - robot.intakeMotor.setPower(-1); - launchState++; - pathTimer.resetTimer(); - } + case 3: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>2) { + robot.follower.followPath(goToLoad1); + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.intakeMotor.setPower(0); + robot.loaderMotor.setPower(0); + } + } + break; + + case 4: + if (!robot.follower.isBusy()) { + robot.follower.followPath(goToLaunch2); + } + break; + + case 5: + if (!robot.follower.isBusy()) { + switch (launchState) { + case 0: + // Start outtake motors + launchState++; + pathTimer.resetTimer(); break; - case 5: - // Launch 3 - if (motorVel+100 >= launchSpeedClose && motorVel+100 >= launchSpeedClose) { - robot.loaderServo.runContinuous(false, true); + case 1: + // Wait for motors to reach speed and launch first 2 + if (motorVel + 100 >= launchSpeedClose && motorVel + 100 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.loaderMotor.setPower(1); launchState++; pathTimer.resetTimer(); } break; - case 6: - // Empty, stop motors + case 2: + // intake on and launch the third if (pathTimer.getElapsedTimeSeconds() > 1.0) { - robot.leftOuttake.setPower(0); - robot.intakeMotor.setPower(0); - robot.loaderServo.runContinuous(false, false); + robot.intakeMotor.setPower(1); launchState++; pathTimer.resetTimer(); } break; - case 7: - // Exit shooting loop - if (pathTimer.getElapsedTimeSeconds() >= 3.0) { - launchState = 0; - setPathState(2); + case 3: + // timer to see when all 3 are launched + if (pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.intakeMotor.setPower(0); + robot.loaderMotor.setPower(0); + launchState = -1; + pathTimer.resetTimer(); } break; + case -1: + break; } } - break; - */ - case 2: - - if (!robot.follower.isBusy()) { - robot.follower.followPath(goToPark); - setPathState(3); - } - break; - - case 3: - if (!robot.follower.isBusy()) { - setPathState(-1); - } - break; - case -1: Pose finalPose = robot.follower.getPose(); @@ -220,6 +257,7 @@ public void autonomousPathUpdate() { break; } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java index ec22518..48f5fa4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java @@ -23,11 +23,19 @@ public Coordinates(double x, double y, double heading) { public static double angleServoBack = 0.6; public static double angleServoClose = 0.65; - /// RED auto movement - public static Coordinates StartingPoseCloseRed = new Coordinates(0, 0, 0); - public static Coordinates LaunchZoneClose = new Coordinates(-25, 32, -0.8); - public static Coordinates LaunchZoneClose2 = new Coordinates(-19, 55, -0.84); - public static Coordinates ParkClose = new Coordinates(-20, 18, 0); + /// RED CLOSE auto movement + public static Coordinates StartingPoseCloseRed = new Coordinates(130, 111, 1.57); + public static Coordinates LaunchZoneClose1 = new Coordinates(108.7, 107.40, 0.8); + public static Coordinates IntakeZoneClose1 = new Coordinates(100, 83, 0); + public static Coordinates LoadZoneClose1 = new Coordinates(128, 83, 0); + public static Coordinates LaunchZoneClose2 = new Coordinates(108.7, 107.40, 0.8); + public static Coordinates IntakeZoneClose2 = new Coordinates(102, 60, 0); + public static Coordinates LoadZoneClose2 = new Coordinates(130, 59, 0); + + public static Coordinates LaunchZoneClose3 = new Coordinates(108.7, 107.40, 0.8); + public static Coordinates ParkClose = new Coordinates(128, 104, 0); + + /// RED FAR auto movement public static Coordinates StartingPoseBackRed = new Coordinates(79, 7.4, 0); public static Coordinates LaunchZoneBack = new Coordinates(83, 19, -0.4); public static Coordinates ParkBack = new Coordinates(81, 35, 0); From 1ee122096f695d69b724a0db88ddb3a19b7d158d Mon Sep 17 00:00:00 2001 From: ChiriacIoana Date: Tue, 24 Feb 2026 17:00:46 +0200 Subject: [PATCH 04/13] auto? --- .../kronbot/autonomous/Auto_CloseRedOp.java | 222 ++++++++++++++---- .../autonomous/AutonomousConstants.java | 23 +- .../ftc/teamcode/kronbot/utils/Constants.java | 2 +- .../kronbot/utils/tests/TestAuto.java | 170 +++++++------- 4 files changed, 274 insertions(+), 143 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java index 45e9224..624fd2b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java @@ -2,7 +2,10 @@ import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.*; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_CLOSE; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_CLOSED; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_KS; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_VELOCITY; import com.acmerobotics.dashboard.FtcDashboard; @@ -25,14 +28,19 @@ public class Auto_CloseRedOp extends OpMode { private Robot robot = Robot.getInstance(); private Timer pathTimer, opmodeTimer; private int pathState; - private int launchState; + private int launchState=0; // Define poses Pose start = coordinates(StartingPoseCloseRed); Pose launch1 = coordinates(LaunchZoneClose1); + Pose launch11 = coordinates(LaunchZoneClose11); Pose launch2 = coordinates(LaunchZoneClose2); Pose intake1 = coordinates(IntakeZoneClose1); - Pose load1 = coordinates(LoadZoneClose1); + Pose intake11 = coordinates(IntakeZoneClose11); + + Pose intake2 = coordinates(IntakeZoneClose2); + Pose intake22 = coordinates(IntakeZoneClose22); + Pose launch3 = coordinates(LaunchZoneClose3); Pose parkZone = coordinates(ParkClose); @@ -40,7 +48,7 @@ public class Auto_CloseRedOp extends OpMode { private double motorVel; // Paths and PathChains - private PathChain goToLaunch1, goToLaunch2, goToIntake1, goToLoad1, goToPark; + private PathChain goToLaunch1, goToLaunch11, goToLaunch2, goToLaunch3, goToIntake1, goToIntake11, goToIntake2, goToIntake22, goToPark; @Override public void init() { @@ -49,9 +57,6 @@ public void init() { robot.initFollower(hardwareMap, start); - robot.flapsServo.setPosition(FLAP_OPEN); - robot.angleServo.setPosition(ANGLE_SERVO_CLOSE); - robot.turretServo.setPosition(0.7); pathTimer = new Timer(); opmodeTimer = new Timer(); @@ -76,22 +81,41 @@ public void buildPaths() { .setLinearHeadingInterpolation(start.getHeading(), launch1.getHeading()) .build(); - goToIntake1 = robot.follower.pathBuilder() - .addPath(new BezierLine(launch1, intake1)) - .setLinearHeadingInterpolation(launch1.getHeading(), intake1.getHeading()) + goToLaunch11 = robot.follower.pathBuilder() + .addPath(new BezierLine(launch1, launch11)) + .setLinearHeadingInterpolation(launch1.getHeading(), launch11.getHeading()) .build(); - goToLoad1 = robot.follower.pathBuilder() - .addPath(new BezierLine(intake1, load1)) - .setLinearHeadingInterpolation(intake1.getHeading(), load1.getHeading()) + goToIntake1 = robot.follower.pathBuilder() + .addPath(new BezierLine(launch11, intake1)) + .setLinearHeadingInterpolation(launch11.getHeading(), intake1.getHeading()) + .build(); + goToIntake11 = robot.follower.pathBuilder() + .addPath(new BezierLine(intake1, intake11)) + .setLinearHeadingInterpolation(intake1.getHeading(), intake11.getHeading()) + .setBrakingStrength(0.2) .build(); goToLaunch2 = robot.follower.pathBuilder() - .addPath(new BezierLine(load1, launch2)) - .setLinearHeadingInterpolation(load1.getHeading(), launch2.getHeading()) + .addPath(new BezierLine(intake11, launch2)) + .setLinearHeadingInterpolation(intake11.getHeading(), launch2.getHeading()) .build(); + goToIntake2 = robot.follower.pathBuilder() + .addPath(new BezierLine(launch2, intake2)) + .setLinearHeadingInterpolation(launch2.getHeading(), intake2.getHeading()) + .build(); + goToIntake22 = robot.follower.pathBuilder() + .addPath(new BezierLine(intake2, intake22)) + .setLinearHeadingInterpolation(intake2.getHeading(), intake22.getHeading()) + .setBrakingStrength(0.2) + .build(); + goToLaunch3 = robot.follower.pathBuilder() + .addPath(new BezierLine(intake22, launch3)) + .setLinearHeadingInterpolation(intake22.getHeading(), launch3.getHeading()) + .build(); + goToPark = robot.follower.pathBuilder() .addPath(new BezierLine(start, parkZone)) .setLinearHeadingInterpolation(start.getHeading(), parkZone.getHeading()) @@ -109,8 +133,11 @@ public void start() { opmodeTimer.resetTimer(); setPathState(0); - robot.turretServo.setPosition(0.5); + robot.turretServo.setPosition(0); robot.angleServo.setPosition(angleServoClose); + robot.flapsServo.setPosition(FLAP_OPEN); + robot.turretServo.setPosition(0.46); + robot.intakeMotor.setPower(-1); robot.loaderServo.runContinuous(false, false); } @@ -120,6 +147,8 @@ public void loop() { motorVel = robot.leftOuttake.getVelocity(); + robot.outtake.update(); + autonomousPathUpdate(); Pose currentPose = robot.follower.getPose(); @@ -138,26 +167,35 @@ public void autonomousPathUpdate() { switch (pathState) { case 0: //also start motors to save time - robot.leftOuttake.setVelocity(launchSpeedClose); - robot.rightOuttake.setVelocity(launchSpeedClose); + robot.outtake.velocity = RANGE_2_VELOCITY; + robot.outtake.kS = RANGE_2_KS; + robot.outtake.on = true; //go to pose robot.follower.followPath(goToLaunch1); setPathState(1); break; case 1: + //go to pose + if (!robot.follower.isBusy()) { + robot.follower.followPath(goToLaunch11); + launchState = 0; + setPathState(2); + } + break; + + case 2: if (!robot.follower.isBusy()) { switch (launchState) { case 0: - // Start outtake motors launchState++; pathTimer.resetTimer(); break; case 1: // Wait for motors to reach speed and launch first 2 - if (motorVel + 100 >= launchSpeedClose && motorVel + 100 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 2.0) { - robot.loaderMotor.setPower(1); + if (motorVel + 50 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 1.0) { + robot.loaderMotor.setPower(0.8); launchState++; pathTimer.resetTimer(); } @@ -166,7 +204,7 @@ public void autonomousPathUpdate() { case 2: // intake on and launch the third if (pathTimer.getElapsedTimeSeconds() > 1.0) { - robot.intakeMotor.setPower(1); + robot.loaderMotor.setPower(0.8); launchState++; pathTimer.resetTimer(); } @@ -176,53 +214,64 @@ public void autonomousPathUpdate() { // timer to see when all 3 are launched if (pathTimer.getElapsedTimeSeconds() > 2.0) { robot.intakeMotor.setPower(0); - robot.loaderMotor.setPower(0); - launchState = -1; + //robot.loaderMotor.setPower(0); + launchState++; pathTimer.resetTimer(); } break; - case -1: + case 4: + setPathState(3); break; + } + break; + } - case 2: - if (!robot.follower.isBusy()) { + case 3: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.flapsServo.setPosition(FLAP_CLOSED); robot.follower.followPath(goToIntake1); - robot.intakeMotor.setPower(1); + robot.intakeMotor.setPower(-1); robot.loaderMotor.setPower(1); - setPathState(3); + + pathTimer.resetTimer(); + setPathState(4); } break; - case 3: - if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>2) { - robot.follower.followPath(goToLoad1); - if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { - robot.intakeMotor.setPower(0); - robot.loaderMotor.setPower(0); - } + case 4: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToIntake11); + pathTimer.resetTimer(); + setPathState(5); } break; - case 4: - if (!robot.follower.isBusy()) { + case 5: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { robot.follower.followPath(goToLaunch2); + robot.loaderMotor.setPower(0); + + launchState = 0; + pathTimer.resetTimer(); + setPathState(6); } break; - case 5: - if (!robot.follower.isBusy()) { + case 6: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { switch (launchState) { case 0: - // Start outtake motors launchState++; pathTimer.resetTimer(); + robot.flapsServo.setPosition(FLAP_OPEN); + robot.turretServo.setPosition(0.5); break; case 1: // Wait for motors to reach speed and launch first 2 - if (motorVel + 100 >= launchSpeedClose && motorVel + 100 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 2.0) { - robot.loaderMotor.setPower(1); + if (motorVel + 50 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.loaderMotor.setPower(0.8); launchState++; pathTimer.resetTimer(); } @@ -231,7 +280,7 @@ public void autonomousPathUpdate() { case 2: // intake on and launch the third if (pathTimer.getElapsedTimeSeconds() > 1.0) { - robot.intakeMotor.setPower(1); + robot.loaderMotor.setPower(0.8); launchState++; pathTimer.resetTimer(); } @@ -241,14 +290,95 @@ public void autonomousPathUpdate() { // timer to see when all 3 are launched if (pathTimer.getElapsedTimeSeconds() > 2.0) { robot.intakeMotor.setPower(0); - robot.loaderMotor.setPower(0); - launchState = -1; + //robot.loaderMotor.setPower(0); + launchState++; pathTimer.resetTimer(); } break; - case -1: + case 4: + setPathState(7); break; + } + break; + + } + + case 7: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.flapsServo.setPosition(FLAP_CLOSED); + robot.follower.followPath(goToIntake2); + robot.intakeMotor.setPower(-1); + robot.loaderMotor.setPower(1); + + pathTimer.resetTimer(); + setPathState(8); + } + break; + + case 8: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToIntake22); + pathTimer.resetTimer(); + setPathState(9); + } + break; + + case 9: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToLaunch3); + robot.loaderMotor.setPower(0); + + launchState = 0; + pathTimer.resetTimer(); + setPathState(10); + } + break; + + case 10: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + switch (launchState) { + case 0: + launchState++; + pathTimer.resetTimer(); + robot.flapsServo.setPosition(FLAP_OPEN); + robot.turretServo.setPosition(0.47); + break; + + case 1: + // Wait for motors to reach speed and launch first 2 + if (motorVel + 50 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 3.0) { + robot.loaderMotor.setPower(0.8); + launchState++; + pathTimer.resetTimer(); + } + break; + + case 2: + // intake on and launch the third + if (pathTimer.getElapsedTimeSeconds() > 1.0) { + robot.loaderMotor.setPower(0.8); + launchState++; + pathTimer.resetTimer(); + } + break; + + case 3: + // timer to see when all 3 are launched + if (pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.intakeMotor.setPower(0); + //robot.loaderMotor.setPower(0); + launchState++; + pathTimer.resetTimer(); + } + break; + case 4: + setPathState(-1); + break; + + } + break; + } case -1: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java index 48f5fa4..9fd56bd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java @@ -18,21 +18,22 @@ public Coordinates(double x, double y, double heading) { } } - public static double launchSpeedClose = 1150; + public static double launchSpeedClose = 1200; public static double launchSpeedBack = 1400; public static double angleServoBack = 0.6; - public static double angleServoClose = 0.65; + public static double angleServoClose = 0.4; /// RED CLOSE auto movement - public static Coordinates StartingPoseCloseRed = new Coordinates(130, 111, 1.57); - public static Coordinates LaunchZoneClose1 = new Coordinates(108.7, 107.40, 0.8); - public static Coordinates IntakeZoneClose1 = new Coordinates(100, 83, 0); - public static Coordinates LoadZoneClose1 = new Coordinates(128, 83, 0); - public static Coordinates LaunchZoneClose2 = new Coordinates(108.7, 107.40, 0.8); - public static Coordinates IntakeZoneClose2 = new Coordinates(102, 60, 0); - public static Coordinates LoadZoneClose2 = new Coordinates(130, 59, 0); - - public static Coordinates LaunchZoneClose3 = new Coordinates(108.7, 107.40, 0.8); + public static Coordinates StartingPoseCloseRed = new Coordinates(130, 110, 0); + public static Coordinates LaunchZoneClose1 = new Coordinates(130, 140, 0); + public static Coordinates LaunchZoneClose11 = new Coordinates(120, 144, -0.8); + public static Coordinates IntakeZoneClose1 = new Coordinates(105, 140, -1.5); + public static Coordinates IntakeZoneClose11 = new Coordinates(105, 110, -1.5); + public static Coordinates LaunchZoneClose2 = new Coordinates(120, 144, -0.8); + public static Coordinates IntakeZoneClose2 = new Coordinates(80, 140, -1.5); + public static Coordinates IntakeZoneClose22 = new Coordinates(80, 100, -1.5); + + public static Coordinates LaunchZoneClose3 = new Coordinates(120, 144, -0.8); public static Coordinates ParkClose = new Coordinates(128, 104, 0); /// RED FAR auto movement diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java index 56f264c..1abffd0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java @@ -53,7 +53,7 @@ public class Constants { public static double ANGLE_SERVO_FAR = 0.72; public static double ANGLE_SERVO_MIN = 0; public static double FLAP_CLOSED = 0.29; - public static double FLAP_OPEN = 0.55; + public static double FLAP_OPEN = 0.59; public static double INTAKE_DRIVER_POWER = 0.55; public static double INTAKE_DRIVER_REVERSE = -0.55; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/tests/TestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/tests/TestAuto.java index 66f9f62..4b65a15 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/tests/TestAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/tests/TestAuto.java @@ -1,85 +1,85 @@ -package org.firstinspires.ftc.teamcode.kronbot.utils.tests; -import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.LaunchZoneClose; -import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.StartingPoseCloseRed; -import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.coordinates; - -import org.firstinspires.ftc.teamcode.pedroPathing.Constants; - - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.pedropathing.follower.Follower; -import com.pedropathing.geometry.BezierLine; -import com.pedropathing.geometry.Pose; -import com.pedropathing.paths.PathChain; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.kronbot.KronBot; - - -@Autonomous(name = "Test Autonomy") -public class TestAuto extends LinearOpMode { - - KronBot robot = new KronBot(); - @Override - public void runOpMode() throws InterruptedException { - robot.initHardware(hardwareMap); - Follower follower = Constants.createFollower(hardwareMap); - - telemetry.addLine(follower == null ? "Follower is NULL!" : "Follower created!"); - telemetry.update(); - - FtcDashboard dashboard = FtcDashboard.getInstance(); - telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry()); - - Pose startingPose = coordinates(StartingPoseCloseRed); - Pose launchZone = coordinates(LaunchZoneClose); - - follower.setStartingPose(startingPose); - - telemetry.addData("Pedro heading", Math.toDegrees(follower.getPose().getHeading())); - telemetry.update(); - - PathChain pathChain1; - - pathChain1 = follower.pathBuilder() - .addPath(new BezierLine(startingPose, launchZone)) - .setLinearHeadingInterpolation(startingPose.getHeading(), launchZone.getHeading()) - .setBrakingStrength(1.3) - .addPath(new BezierLine(launchZone, startingPose)) - .setLinearHeadingInterpolation(launchZone.getHeading(), startingPose.getHeading()) - .setBrakingStrength(1.3) - .build(); - - waitForStart(); - - if(opModeIsActive()) { - follower.followPath(pathChain1); - } - - while (opModeIsActive() && !isStopRequested()) { - follower.update(); - - Pose currentPose = follower.getPose(); - double coordx = currentPose.getX(); - double coordy = currentPose.getY(); - currentPose.getHeading(); - telemetry.addData("current coordx is: ", coordx); - telemetry.addData("current coordy is: ", coordy); - telemetry.addData("Heading", currentPose.getHeading()); - - while(!follower.isBusy()) { - robot.loaderServo.runContinuous(false, true); - robot.leftOuttake.setPower(1); - robot.rightOuttake.setPower(1); - sleep(4000); - robot.leftOuttake.setPower(0); - robot.rightOuttake.setPower(0); - sleep(4000); - - } - telemetry.update(); - } - } -} \ No newline at end of file +//package org.firstinspires.ftc.teamcode.kronbot.utils.tests; +//import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.LaunchZoneClose; +//import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.StartingPoseCloseRed; +//import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.coordinates; +// +//import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +// +// +//import com.acmerobotics.dashboard.FtcDashboard; +//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +//import com.pedropathing.follower.Follower; +//import com.pedropathing.geometry.BezierLine; +//import com.pedropathing.geometry.Pose; +//import com.pedropathing.paths.PathChain; +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +// +//import org.firstinspires.ftc.teamcode.kronbot.KronBot; +// +// +//@Autonomous(name = "Test Autonomy") +//public class TestAuto extends LinearOpMode { +// +// KronBot robot = new KronBot(); +// @Override +// public void runOpMode() throws InterruptedException { +// robot.initHardware(hardwareMap); +// Follower follower = Constants.createFollower(hardwareMap); +// +// telemetry.addLine(follower == null ? "Follower is NULL!" : "Follower created!"); +// telemetry.update(); +// +// FtcDashboard dashboard = FtcDashboard.getInstance(); +// telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry()); +// +// Pose startingPose = coordinates(StartingPoseCloseRed); +// Pose launchZone = coordinates(LaunchZoneClose); +// +// follower.setStartingPose(startingPose); +// +// telemetry.addData("Pedro heading", Math.toDegrees(follower.getPose().getHeading())); +// telemetry.update(); +// +// PathChain pathChain1; +// +// pathChain1 = follower.pathBuilder() +// .addPath(new BezierLine(startingPose, launchZone)) +// .setLinearHeadingInterpolation(startingPose.getHeading(), launchZone.getHeading()) +// .setBrakingStrength(1.3) +// .addPath(new BezierLine(launchZone, startingPose)) +// .setLinearHeadingInterpolation(launchZone.getHeading(), startingPose.getHeading()) +// .setBrakingStrength(1.3) +// .build(); +// +// waitForStart(); +// +// if(opModeIsActive()) { +// follower.followPath(pathChain1); +// } +// +// while (opModeIsActive() && !isStopRequested()) { +// follower.update(); +// +// Pose currentPose = follower.getPose(); +// double coordx = currentPose.getX(); +// double coordy = currentPose.getY(); +// currentPose.getHeading(); +// telemetry.addData("current coordx is: ", coordx); +// telemetry.addData("current coordy is: ", coordy); +// telemetry.addData("Heading", currentPose.getHeading()); +// +// while(!follower.isBusy()) { +// robot.loaderServo.runContinuous(false, true); +// robot.leftOuttake.setPower(1); +// robot.rightOuttake.setPower(1); +// sleep(4000); +// robot.leftOuttake.setPower(0); +// robot.rightOuttake.setPower(0); +// sleep(4000); +// +// } +// telemetry.update(); +// } +// } +//} \ No newline at end of file From 8e4c1341fe290ad5b1dea22970162a57797de34b Mon Sep 17 00:00:00 2001 From: ChiriacIoana Date: Wed, 25 Feb 2026 10:05:45 +0200 Subject: [PATCH 05/13] 9 close auto --- .../kronbot/autonomous/Auto_CloseRedOp.java | 58 +++++++------------ .../autonomous/AutonomousConstants.java | 4 +- 2 files changed, 22 insertions(+), 40 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java index 624fd2b..110a5ce 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java @@ -117,8 +117,8 @@ public void buildPaths() { .build(); goToPark = robot.follower.pathBuilder() - .addPath(new BezierLine(start, parkZone)) - .setLinearHeadingInterpolation(start.getHeading(), parkZone.getHeading()) + .addPath(new BezierLine(launch3, parkZone)) + .setLinearHeadingInterpolation(launch3.getHeading(), parkZone.getHeading()) .build(); } @@ -167,8 +167,8 @@ public void autonomousPathUpdate() { switch (pathState) { case 0: //also start motors to save time - robot.outtake.velocity = RANGE_2_VELOCITY; - robot.outtake.kS = RANGE_2_KS; + robot.outtake.activeConfig.velocity = RANGE_2_VELOCITY; + robot.outtake.activeConfig.kS = RANGE_2_KS; robot.outtake.on = true; //go to pose robot.follower.followPath(goToLaunch1); @@ -202,24 +202,15 @@ public void autonomousPathUpdate() { break; case 2: - // intake on and launch the third - if (pathTimer.getElapsedTimeSeconds() > 1.0) { - robot.loaderMotor.setPower(0.8); - launchState++; - pathTimer.resetTimer(); - } - break; - - case 3: // timer to see when all 3 are launched - if (pathTimer.getElapsedTimeSeconds() > 2.0) { + if (pathTimer.getElapsedTimeSeconds() > 3.0) { robot.intakeMotor.setPower(0); //robot.loaderMotor.setPower(0); launchState++; pathTimer.resetTimer(); } break; - case 4: + case 3: setPathState(3); break; @@ -278,24 +269,15 @@ public void autonomousPathUpdate() { break; case 2: - // intake on and launch the third - if (pathTimer.getElapsedTimeSeconds() > 1.0) { - robot.loaderMotor.setPower(0.8); - launchState++; - pathTimer.resetTimer(); - } - break; - - case 3: // timer to see when all 3 are launched - if (pathTimer.getElapsedTimeSeconds() > 2.0) { + if (pathTimer.getElapsedTimeSeconds() > 3.0) { robot.intakeMotor.setPower(0); //robot.loaderMotor.setPower(0); launchState++; pathTimer.resetTimer(); } break; - case 4: + case 3: setPathState(7); break; @@ -355,25 +337,16 @@ public void autonomousPathUpdate() { break; case 2: - // intake on and launch the third - if (pathTimer.getElapsedTimeSeconds() > 1.0) { - robot.loaderMotor.setPower(0.8); - launchState++; - pathTimer.resetTimer(); - } - break; - - case 3: // timer to see when all 3 are launched - if (pathTimer.getElapsedTimeSeconds() > 2.0) { + if (pathTimer.getElapsedTimeSeconds() > 3.0) { robot.intakeMotor.setPower(0); //robot.loaderMotor.setPower(0); launchState++; pathTimer.resetTimer(); } break; - case 4: - setPathState(-1); + case 3: + setPathState(11); break; } @@ -381,6 +354,15 @@ public void autonomousPathUpdate() { } + case 11: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToPark); + robot.loaderMotor.setPower(0); + pathTimer.resetTimer(); + setPathState(-1); + } + break; + case -1: Pose finalPose = robot.follower.getPose(); PoseStorage.savePose(finalPose); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java index 9fd56bd..4510846 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java @@ -18,10 +18,10 @@ public Coordinates(double x, double y, double heading) { } } - public static double launchSpeedClose = 1200; + public static double launchSpeedClose = 1150; public static double launchSpeedBack = 1400; public static double angleServoBack = 0.6; - public static double angleServoClose = 0.4; + public static double angleServoClose = 0.45; /// RED CLOSE auto movement public static Coordinates StartingPoseCloseRed = new Coordinates(130, 110, 0); From a604719cdf5c46a667d6cc9a8ee5d136305c197c Mon Sep 17 00:00:00 2001 From: Robi2903 <113847997+Robi2903@users.noreply.github.com> Date: Wed, 25 Feb 2026 15:37:31 +0200 Subject: [PATCH 06/13] Semi-working auto-aim and stuff --- .../ftc/teamcode/kronbot/Robot.java | 49 ++++++++++++------- .../kronbot/manual/MainDrivingOp.java | 21 ++++---- .../ftc/teamcode/kronbot/utils/Constants.java | 22 ++++----- 3 files changed, 53 insertions(+), 39 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java index a221508..dad97af 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java @@ -51,6 +51,9 @@ public class Robot extends KronBot { // Singleton instance + + double dx; + double dy; private static Robot instance = null; // Systems used in all opModes @@ -64,6 +67,8 @@ public class Robot extends KronBot { public final Shoot shoot; public final Heading heading; + double distance; + public static class RangeConfig { public double angle; public double velocity; @@ -154,8 +159,9 @@ public class Outtake { public boolean reversed = false; boolean braking = false; + private double selectedRange1, selectedRange2; // double lastVelocity = 0; - private TreeMap ranges; + private TreeMap ranges = new TreeMap<>(); public void init() { on = false; @@ -287,30 +293,35 @@ public RangeConfig interpolateRange() { double robot_X = follower.getPose().getX(); double robot_Y = follower.getPose().getY(); - double dx = basket_X - robot_X; - double dy = basket_Y - robot_Y; + dx = basket_X - robot_X; + dy = basket_Y - robot_Y; - double distance = Math.sqrt(dx*dx + dy*dy); + distance = Math.sqrt(dx*dx + dy*dy); - if(distance= ranges.lastKey()) return new RangeConfig(ranges.get(ranges.lastKey()).angle, ranges.get(ranges.lastKey()).velocity, ranges.get(ranges.lastKey()).kS); //I used TreeMaps as its ordered and offers floorEntry (the biggest entry smaller than the value searched) and ceilingEntry (the opposite) Map.Entry lower = ranges.floorEntry(distance); Map.Entry upper = ranges.ceilingEntry(distance); - if (lower == null && upper!=null) return upper.getValue(); - if (upper == null && lower!=null) return lower.getValue(); - - if (lower.getKey().equals(upper.getKey())) - return lower.getValue(); + // Null checks moved before d1/d2 are accessed + if (lower == null && upper != null) return upper.getValue(); + if (upper == null && lower != null) return lower.getValue(); + if (lower == null) return new RangeConfig(0, 0, 0); //Linear interpolation between the two ranges double d1 = lower.getKey(); double d2 = upper.getKey(); + selectedRange1=d1; selectedRange2=d2; + + + if (lower.getKey().equals(upper.getKey())) + return lower.getValue(); + double angle1 = lower.getValue().angle; double vel1 = lower.getValue().velocity; @@ -333,8 +344,12 @@ public RangeConfig interpolateRange() { return new RangeConfig(interpAngle, interpVel, interpKs); } + public void telemetry(Telemetry telemetry) { telemetry.addLine("=== OUTTAKE STATUS ==="); + telemetry.addData("d1", selectedRange1); + telemetry.addData("d2", selectedRange2); + telemetry.addData("Distance", distance); telemetry.addData("On", on); telemetry.addData("Reversed", reversed); telemetry.addData("Target Velocity", "%.0f", activeConfig.velocity); @@ -409,6 +424,7 @@ public void init() { servoPosition = 0.5; } + public void update() { //angle to the basket if (turretServo == null || follower == null) return; @@ -426,7 +442,7 @@ public void update() { double targetFieldAngle = Math.atan2(dy, dx); //calculate - double robotRelativeAngle = targetFieldAngle - robotHeading; + double robotRelativeAngle = targetFieldAngle - robotHeading + driverOffset; //normalize robotRelativeAngle = Math.atan2( @@ -548,7 +564,7 @@ public void telemetry(Telemetry telemetry) { public class Shoot { public void activateRange(int range) { - RangeConfig config; + RangeConfig config = new RangeConfig(0,0,0); switch (range) { case 1: outtake.on = true; @@ -578,13 +594,8 @@ public void activateRange(int range) { outtake.on = true; config = outtake.interpolateRange(); break; - - default: - outtake.on=false; - outtake.activeConfig = new RangeConfig(0,0,0); - break; } - + outtake.activeConfig=config; } public void deactivate() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java index 82bd463..c952991 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java @@ -120,7 +120,7 @@ public void loop() { robot.loader.speed = utilityGP.leftStick.y; robot.flap.open = false; } else { - robot.loader.speed = drivingGP.rightTrigger - drivingGP.leftTrigger; + robot.loader.speed = (drivingGP.rightTrigger - drivingGP.leftTrigger) * 0.9; robot.flap.open = true; if (robot.loader.speed > 0.1) robot.intake.speed = INTAKE_DRIVER_POWER; @@ -184,6 +184,9 @@ else if (robot.loader.speed < -0.2) robot.turret.autoAimEnabled = !robot.turret.autoAimEnabled; if(drivingGP.dpadUp.justPressed()) + autoAimEnabled=!autoAimEnabled; + + if(autoAimEnabled) robot.shoot.activateRange(0); //Shoot Close/Far if (drivingGP.triangle.justPressed()) { @@ -206,14 +209,14 @@ else if (robot.loader.speed < -0.2) rumbled = true; } -// if (!autoAimEnabled && drivingGP.leftBumper.justPressed()) { -// robot.turret.autoAimEnabled = true; -// if (robot.outtake.on) { -// robot.shoot.deactivate(); -// gamepad1.rumble(1, 1, 100); -// rumbled = false; -// } -// } + if (!autoAimEnabled && drivingGP.leftBumper.justPressed()) { + robot.turret.autoAimEnabled = true; + if (robot.outtake.on) { + robot.shoot.deactivate(); + gamepad1.rumble(1, 1, 100); + rumbled = false; + } + } //Update robot systems status robot.follower.setTeleOpDrive(-drivingGP.leftStick.y, -drivingGP.leftStick.x, -drivingGP.rightStick.x, true); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java index f6b28be..7256a1f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java @@ -61,25 +61,25 @@ public class Constants { public static double minVelocity = 1140; public static double maxVelocity = 1500; - public static double RANGE_1 = 10; + public static double RANGE_1 = 100; public static double RANGE_1_ANGLE = 0; - public static double RANGE_1_VELOCITY = 1150; + public static double RANGE_1_VELOCITY = 1000; public static double RANGE_1_KS = 0.15; - public static double RANGE_2 = 30; - public static double RANGE_2_ANGLE = 0.3; - public static double RANGE_2_VELOCITY = 1280; + public static double RANGE_2 = 120; + public static double RANGE_2_ANGLE = 0.32; + public static double RANGE_2_VELOCITY = 1150; public static double RANGE_2_KS = 0.2; - public static double RANGE_3 = 50; - public static double RANGE_3_ANGLE = 0.72; - public static double RANGE_3_VELOCITY = 1350; + public static double RANGE_3 = 185; + public static double RANGE_3_ANGLE = 0.64; + public static double RANGE_3_VELOCITY = 1450; public static double RANGE_3_KS = 0.3; - public static double RANGE_4 = 100; - public static double RANGE_4_ANGLE = 0.72; - public static double RANGE_4_VELOCITY = 1400; + public static double RANGE_4 = 230; + public static double RANGE_4_ANGLE = 0.70; + public static double RANGE_4_VELOCITY = 1600; public static double RANGE_4_KS = 0.5; public static AutonomousConstants.Coordinates TestPoseStart = new AutonomousConstants.Coordinates(0, 0, 0); From 5ec2c3225b434b8c9960de0ee4055961053a6400 Mon Sep 17 00:00:00 2001 From: ChiriacIoana Date: Thu, 26 Feb 2026 14:44:47 +0200 Subject: [PATCH 07/13] auto fixex --- .../ftc/teamcode/kronbot/Robot.java | 7 +++++++ .../kronbot/autonomous/Auto_CloseRedOp.java | 21 ++++++++++--------- .../autonomous/AutonomousConstants.java | 12 +++++------ .../kronbot/manual/MainDrivingOp.java | 6 +----- 4 files changed, 25 insertions(+), 21 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java index dad97af..05f4528 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java @@ -111,6 +111,13 @@ public void init(HardwareMap hardwareMap) { public void initSystems(HardwareMap hardwareMap) { if(follower == null) initFollower(hardwareMap); + + try { + follower.getPoseTracker().resetIMU(); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + follower.update(); outtake.init(); intake.init(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java index 110a5ce..e5d48ca 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java @@ -203,7 +203,7 @@ public void autonomousPathUpdate() { case 2: // timer to see when all 3 are launched - if (pathTimer.getElapsedTimeSeconds() > 3.0) { + if (pathTimer.getElapsedTimeSeconds() > 2.0) { robot.intakeMotor.setPower(0); //robot.loaderMotor.setPower(0); launchState++; @@ -241,8 +241,9 @@ public void autonomousPathUpdate() { case 5: if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { robot.follower.followPath(goToLaunch2); - robot.loaderMotor.setPower(0); + robot.loaderMotor.setPower(0); + robot.loaderMotor.setPower(-0.3); launchState = 0; pathTimer.resetTimer(); setPathState(6); @@ -250,7 +251,7 @@ public void autonomousPathUpdate() { break; case 6: - if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>0.6) { switch (launchState) { case 0: launchState++; @@ -262,7 +263,7 @@ public void autonomousPathUpdate() { case 1: // Wait for motors to reach speed and launch first 2 if (motorVel + 50 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 2.0) { - robot.loaderMotor.setPower(0.8); + robot.loaderMotor.setPower(0.7); launchState++; pathTimer.resetTimer(); } @@ -270,7 +271,7 @@ public void autonomousPathUpdate() { case 2: // timer to see when all 3 are launched - if (pathTimer.getElapsedTimeSeconds() > 3.0) { + if (pathTimer.getElapsedTimeSeconds() > 1.0) { robot.intakeMotor.setPower(0); //robot.loaderMotor.setPower(0); launchState++; @@ -310,7 +311,7 @@ public void autonomousPathUpdate() { if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { robot.follower.followPath(goToLaunch3); robot.loaderMotor.setPower(0); - + robot.loaderMotor.setPower(-0.3); launchState = 0; pathTimer.resetTimer(); setPathState(10); @@ -318,7 +319,7 @@ public void autonomousPathUpdate() { break; case 10: - if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>0.6) { switch (launchState) { case 0: launchState++; @@ -329,8 +330,8 @@ public void autonomousPathUpdate() { case 1: // Wait for motors to reach speed and launch first 2 - if (motorVel + 50 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 3.0) { - robot.loaderMotor.setPower(0.8); + if (motorVel + 50 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.loaderMotor.setPower(0.7); launchState++; pathTimer.resetTimer(); } @@ -338,7 +339,7 @@ public void autonomousPathUpdate() { case 2: // timer to see when all 3 are launched - if (pathTimer.getElapsedTimeSeconds() > 3.0) { + if (pathTimer.getElapsedTimeSeconds() > 1.0) { robot.intakeMotor.setPower(0); //robot.loaderMotor.setPower(0); launchState++; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java index 4510846..5f6a24a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java @@ -21,20 +21,20 @@ public Coordinates(double x, double y, double heading) { public static double launchSpeedClose = 1150; public static double launchSpeedBack = 1400; public static double angleServoBack = 0.6; - public static double angleServoClose = 0.45; + public static double angleServoClose = 0.32; /// RED CLOSE auto movement public static Coordinates StartingPoseCloseRed = new Coordinates(130, 110, 0); - public static Coordinates LaunchZoneClose1 = new Coordinates(130, 140, 0); + public static Coordinates LaunchZoneClose1 = new Coordinates(125, 140, 0); public static Coordinates LaunchZoneClose11 = new Coordinates(120, 144, -0.8); public static Coordinates IntakeZoneClose1 = new Coordinates(105, 140, -1.5); public static Coordinates IntakeZoneClose11 = new Coordinates(105, 110, -1.5); - public static Coordinates LaunchZoneClose2 = new Coordinates(120, 144, -0.8); + public static Coordinates LaunchZoneClose2 = new Coordinates(125, 144, -0.8); public static Coordinates IntakeZoneClose2 = new Coordinates(80, 140, -1.5); - public static Coordinates IntakeZoneClose22 = new Coordinates(80, 100, -1.5); + public static Coordinates IntakeZoneClose22 = new Coordinates(80, 110, -1.5); - public static Coordinates LaunchZoneClose3 = new Coordinates(120, 144, -0.8); - public static Coordinates ParkClose = new Coordinates(128, 104, 0); + public static Coordinates LaunchZoneClose3 = new Coordinates(125, 144, -0.8); + public static Coordinates ParkClose = new Coordinates(100, 110, 0); /// RED FAR auto movement public static Coordinates StartingPoseBackRed = new Coordinates(79, 7.4, 0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java index c952991..549630e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java @@ -75,11 +75,7 @@ public void init() { drivingGP = new Controls(gamepad1); utilityGP = new Controls(gamepad2); - try { - robot.follower.getPoseTracker().resetIMU(); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } + } @Override From 41244fa4c0fedabe5a9eee07cd61bda5de8183ec Mon Sep 17 00:00:00 2001 From: Mihai Date: Thu, 26 Feb 2026 20:34:33 +0200 Subject: [PATCH 08/13] Autoaim fixez --- .../ftc/teamcode/kronbot/Robot.java | 21 +++++++------------ .../kronbot/autonomous/Auto_CloseRedOp.java | 7 ++++--- .../ftc/teamcode/kronbot/utils/Constants.java | 3 +++ 3 files changed, 15 insertions(+), 16 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java index 05f4528..b0070e0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java @@ -12,6 +12,8 @@ import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_CLOSE; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_MAX; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_MIN; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.BASKET_X; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.BASKET_Y; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.DELTA_THRESHOLD; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_CLOSED; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN; @@ -52,8 +54,6 @@ public class Robot extends KronBot { // Singleton instance - double dx; - double dy; private static Robot instance = null; // Systems used in all opModes @@ -67,7 +67,7 @@ public class Robot extends KronBot { public final Shoot shoot; public final Heading heading; - double distance; + public boolean Blue_Target = false; public static class RangeConfig { public double angle; @@ -155,10 +155,6 @@ public void updateAllSystems() { // webcam.update(); } - static final double basket_X = 130; - static final double basket_Y = 135; - - public class Outtake { public boolean on = false; public RangeConfig activeConfig; @@ -167,6 +163,7 @@ public class Outtake { boolean braking = false; private double selectedRange1, selectedRange2; + private double distance; // double lastVelocity = 0; private TreeMap ranges = new TreeMap<>(); @@ -300,8 +297,8 @@ public RangeConfig interpolateRange() { double robot_X = follower.getPose().getX(); double robot_Y = follower.getPose().getY(); - dx = basket_X - robot_X; - dy = basket_Y - robot_Y; + double dx = BASKET_X * (Blue_Target ? 1 : -1) - robot_X; + double dy = BASKET_Y - robot_Y; distance = Math.sqrt(dx*dx + dy*dy); @@ -423,8 +420,6 @@ public class Turret { public boolean autoAimEnabled = true; - static final double basket_X = 130; - static final double basket_Y = 135; public void init() { angle = 0; @@ -443,8 +438,8 @@ public void update() { double robot_Y = follower.getPose().getY(); double robotHeading = heading.get(); - double dx = basket_X - robot_X; - double dy = basket_Y - robot_Y; + double dx = BASKET_X * (Blue_Target ? 1 : -1) - robot_X; + double dy = BASKET_Y - robot_Y; double targetFieldAngle = Math.atan2(dy, dx); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java index e5d48ca..eb476b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java @@ -33,14 +33,14 @@ public class Auto_CloseRedOp extends OpMode { // Define poses Pose start = coordinates(StartingPoseCloseRed); Pose launch1 = coordinates(LaunchZoneClose1); - Pose launch11 = coordinates(LaunchZoneClose11); - Pose launch2 = coordinates(LaunchZoneClose2); + Pose launch11 = coordinates(LaunchZoneClose1); + Pose launch2 = coordinates(LaunchZoneClose1); Pose intake1 = coordinates(IntakeZoneClose1); Pose intake11 = coordinates(IntakeZoneClose11); Pose intake2 = coordinates(IntakeZoneClose2); Pose intake22 = coordinates(IntakeZoneClose22); - Pose launch3 = coordinates(LaunchZoneClose3); + Pose launch3 = coordinates(LaunchZoneClose1); Pose parkZone = coordinates(ParkClose); @@ -158,6 +158,7 @@ public void loop() { telemetry.addData("Heading (rad)", currentPose.getHeading()); //telemetry.addData("Outtake Alpha", robot.outtakeColor.alpha()); telemetry.addData("Shooter Motor vel", robot.leftOuttake.getVelocity()); + telemetry.addData("Launchstate: ", launchState); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java index 7256a1f..a2813d0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java @@ -109,6 +109,9 @@ public class Constants { public static double DELTA_THRESHOLD = 0.01; public static double MAX_ROTATION_POWER = 0.5; + public static double BASKET_X = 140; + public static double BASKET_Y = 140; + public static AutonomousConstants.Coordinates RedTowerCoords = new AutonomousConstants.Coordinates(130, 130, 0); public static AutonomousConstants.Coordinates BlueTowerCoords = new AutonomousConstants.Coordinates(10, 135, 0); From d8b3baf6b6adb4777530b9caabe9a260f6c41ba9 Mon Sep 17 00:00:00 2001 From: Mihai Date: Fri, 27 Feb 2026 10:43:12 +0200 Subject: [PATCH 09/13] auto aim works? --- .../firstinspires/ftc/teamcode/kronbot/Robot.java | 4 ++-- .../teamcode/kronbot/autonomous/Auto_CloseRedOp.java | 4 ++-- .../ftc/teamcode/kronbot/manual/MainDrivingOp.java | 7 +++++-- .../ftc/teamcode/kronbot/utils/Constants.java | 12 ++++++------ 4 files changed, 15 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java index b0070e0..37d0c37 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java @@ -297,7 +297,7 @@ public RangeConfig interpolateRange() { double robot_X = follower.getPose().getX(); double robot_Y = follower.getPose().getY(); - double dx = BASKET_X * (Blue_Target ? 1 : -1) - robot_X; + double dx = BASKET_X * (Blue_Target ? -1 : 1) - robot_X; double dy = BASKET_Y - robot_Y; distance = Math.sqrt(dx*dx + dy*dy); @@ -438,7 +438,7 @@ public void update() { double robot_Y = follower.getPose().getY(); double robotHeading = heading.get(); - double dx = BASKET_X * (Blue_Target ? 1 : -1) - robot_X; + double dx = BASKET_X * (Blue_Target ? -1 : 1) - robot_X; double dy = BASKET_Y - robot_Y; double targetFieldAngle = Math.atan2(dy, dx); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java index eb476b8..f7bab95 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java @@ -33,14 +33,14 @@ public class Auto_CloseRedOp extends OpMode { // Define poses Pose start = coordinates(StartingPoseCloseRed); Pose launch1 = coordinates(LaunchZoneClose1); - Pose launch11 = coordinates(LaunchZoneClose1); + Pose launch11 = coordinates(LaunchZoneClose11); Pose launch2 = coordinates(LaunchZoneClose1); Pose intake1 = coordinates(IntakeZoneClose1); Pose intake11 = coordinates(IntakeZoneClose11); Pose intake2 = coordinates(IntakeZoneClose2); Pose intake22 = coordinates(IntakeZoneClose22); - Pose launch3 = coordinates(LaunchZoneClose1); + Pose launch3 = coordinates(LaunchZoneClose3); Pose parkZone = coordinates(ParkClose); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java index 549630e..343831e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java @@ -109,14 +109,14 @@ public void loop() { robot.intake.reversed = INTAKE_REVERSE; //Aliniere - turretAligner.update(); +// turretAligner.update(); //Loader if (!drivingGP.rightBumper.pressed()) { robot.loader.speed = utilityGP.leftStick.y; robot.flap.open = false; } else { - robot.loader.speed = (drivingGP.rightTrigger - drivingGP.leftTrigger) * 0.9; + robot.loader.speed = (drivingGP.rightTrigger - drivingGP.leftTrigger) * 0.8; robot.flap.open = true; if (robot.loader.speed > 0.1) robot.intake.speed = INTAKE_DRIVER_POWER; @@ -214,6 +214,9 @@ else if (robot.loader.speed < -0.2) } } + if(drivingGP.rightStick.button.justPressed()) + robot.Blue_Target = !robot.Blue_Target; + //Update robot systems status robot.follower.setTeleOpDrive(-drivingGP.leftStick.y, -drivingGP.leftStick.x, -drivingGP.rightStick.x, true); robot.updateAllSystems(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java index a2813d0..c17f1b5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java @@ -61,23 +61,23 @@ public class Constants { public static double minVelocity = 1140; public static double maxVelocity = 1500; - public static double RANGE_1 = 100; + public static double RANGE_1 = 40; public static double RANGE_1_ANGLE = 0; public static double RANGE_1_VELOCITY = 1000; public static double RANGE_1_KS = 0.15; - public static double RANGE_2 = 120; + public static double RANGE_2 = 70; public static double RANGE_2_ANGLE = 0.32; public static double RANGE_2_VELOCITY = 1150; public static double RANGE_2_KS = 0.2; - public static double RANGE_3 = 185; + public static double RANGE_3 = 100; public static double RANGE_3_ANGLE = 0.64; public static double RANGE_3_VELOCITY = 1450; public static double RANGE_3_KS = 0.3; - public static double RANGE_4 = 230; + public static double RANGE_4 = 140; public static double RANGE_4_ANGLE = 0.70; public static double RANGE_4_VELOCITY = 1600; public static double RANGE_4_KS = 0.5; @@ -109,8 +109,8 @@ public class Constants { public static double DELTA_THRESHOLD = 0.01; public static double MAX_ROTATION_POWER = 0.5; - public static double BASKET_X = 140; - public static double BASKET_Y = 140; + public static double BASKET_X = 72; + public static double BASKET_Y = 72; public static AutonomousConstants.Coordinates RedTowerCoords = new AutonomousConstants.Coordinates(130, 130, 0); From f0857677818ae0343efd046c439efe4ca8cdf516 Mon Sep 17 00:00:00 2001 From: ChiriacIoana Date: Fri, 27 Feb 2026 11:23:13 +0200 Subject: [PATCH 10/13] ? --- .../org/firstinspires/ftc/teamcode/kronbot/Robot.java | 4 ++-- .../teamcode/kronbot/autonomous/Auto_CloseRedOp.java | 8 ++++---- .../kronbot/autonomous/AutonomousConstants.java | 4 ++-- .../ftc/teamcode/kronbot/manual/MainDrivingOp.java | 11 ++++++----- .../ftc/teamcode/kronbot/utils/PoseStorage.java | 4 ++-- 5 files changed, 16 insertions(+), 15 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java index 37d0c37..006a9f1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java @@ -109,8 +109,8 @@ public void init(HardwareMap hardwareMap) { } public void initSystems(HardwareMap hardwareMap) { - if(follower == null) - initFollower(hardwareMap); +// if(follower == null) +// initFollower(hardwareMap, true); try { follower.getPoseTracker().resetIMU(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java index f7bab95..dfcf616 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java @@ -34,7 +34,7 @@ public class Auto_CloseRedOp extends OpMode { Pose start = coordinates(StartingPoseCloseRed); Pose launch1 = coordinates(LaunchZoneClose1); Pose launch11 = coordinates(LaunchZoneClose11); - Pose launch2 = coordinates(LaunchZoneClose1); + Pose launch2 = coordinates(LaunchZoneClose2); Pose intake1 = coordinates(IntakeZoneClose1); Pose intake11 = coordinates(IntakeZoneClose11); @@ -53,10 +53,8 @@ public class Auto_CloseRedOp extends OpMode { @Override public void init() { - robot.init(hardwareMap); - - robot.initFollower(hardwareMap, start); + robot.init(hardwareMap); pathTimer = new Timer(); opmodeTimer = new Timer(); @@ -342,6 +340,8 @@ public void autonomousPathUpdate() { // timer to see when all 3 are launched if (pathTimer.getElapsedTimeSeconds() > 1.0) { robot.intakeMotor.setPower(0); + robot.outtake.on = false; + robot.outtake.activeConfig = new Robot.RangeConfig(0, 0, 0); //robot.loaderMotor.setPower(0); launchState++; pathTimer.resetTimer(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java index 5f6a24a..83c6d95 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java @@ -30,8 +30,8 @@ public Coordinates(double x, double y, double heading) { public static Coordinates IntakeZoneClose1 = new Coordinates(105, 140, -1.5); public static Coordinates IntakeZoneClose11 = new Coordinates(105, 110, -1.5); public static Coordinates LaunchZoneClose2 = new Coordinates(125, 144, -0.8); - public static Coordinates IntakeZoneClose2 = new Coordinates(80, 140, -1.5); - public static Coordinates IntakeZoneClose22 = new Coordinates(80, 110, -1.5); + public static Coordinates IntakeZoneClose2 = new Coordinates(76, 140, -1.5); + public static Coordinates IntakeZoneClose22 = new Coordinates(76, 110, -1.5); public static Coordinates LaunchZoneClose3 = new Coordinates(125, 144, -0.8); public static Coordinates ParkClose = new Coordinates(100, 110, 0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java index 343831e..e58df1a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java @@ -15,6 +15,7 @@ import com.acmerobotics.dashboard.FtcDashboard; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.ElapsedTime; @@ -57,16 +58,16 @@ public class MainDrivingOp extends OpMode { public void init() { lpsCounter = new LpsCounter(); lpsCounter.getLoopTime(); - robot.initFollower(hardwareMap, true); + robot.initFollower(hardwareMap, new Pose(68, 68, 0)); robot.init(hardwareMap); dashboard = FtcDashboard.getInstance(); - robot.webcam.init(hardwareMap, telemetry); +// robot.webcam.init(hardwareMap, telemetry); - if (robot.webcam.getVisionPortal() != null) { - dashboard.startCameraStream(robot.webcam.getVisionPortal(), 30); - } +// if (robot.webcam.getVisionPortal() != null) { +// dashboard.startCameraStream(robot.webcam.getVisionPortal(), 30); +// } // Initialize the new coordinate aligner turretAligner = new TurretAligner(robot); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/PoseStorage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/PoseStorage.java index 3056b79..6b97daf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/PoseStorage.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/PoseStorage.java @@ -29,7 +29,7 @@ public static Pose loadPose() { File file = new File(Environment.getExternalStorageDirectory().getPath() + "/" + FILE_NAME); if (!file.exists()) { - return new Pose(0, 0, 0); + return new Pose(67, 67, 0); } try (BufferedReader reader = new BufferedReader(new FileReader(file))) { @@ -40,7 +40,7 @@ public static Pose loadPose() { return new Pose(x, y, heading); } catch (Exception e) { RobotLog.ee("PoseStorage", "Failed to load pose", e); - return new Pose(0, 0, 0); + return new Pose(69, 69, 0); } } } \ No newline at end of file From 43da70d7deaca513133e7e6c7468a5f13f847485 Mon Sep 17 00:00:00 2001 From: ChiriacIoana Date: Fri, 27 Feb 2026 13:33:42 +0200 Subject: [PATCH 11/13] before regio --- .../ftc/teamcode/kronbot/KronBot.java | 10 ++++++++++ .../ftc/teamcode/kronbot/Robot.java | 17 +++++++---------- .../kronbot/autonomous/Auto_BackBlueOp.java | 6 +++--- .../kronbot/autonomous/AutonomousConstants.java | 12 +++++++----- .../teamcode/kronbot/manual/MainDrivingOp.java | 6 +++++- .../ftc/teamcode/kronbot/utils/Constants.java | 11 ++++++----- .../ftc/teamcode/pedroPathing/Constants.java | 2 +- 7 files changed, 39 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/KronBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/KronBot.java index 41a5693..d2532f7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/KronBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/KronBot.java @@ -80,6 +80,11 @@ public void initSensors(HardwareMap hardwareMap) { */ public void initFollower(HardwareMap hardwareMap, boolean loadPose) { follower = org.firstinspires.ftc.teamcode.pedroPathing.Constants.createFollower(hardwareMap); +// try { +// follower.getPoseTracker().resetIMU(); +// } catch (InterruptedException e) { +// throw new RuntimeException(e); +// } if(loadPose) { Pose startingPose = PoseStorage.loadPose(); follower.setStartingPose(startingPose); @@ -101,6 +106,11 @@ public void initFollower(HardwareMap hardwareMap, boolean loadPose) { */ public void initFollower(HardwareMap hardwareMap, Pose startingPose) { follower = org.firstinspires.ftc.teamcode.pedroPathing.Constants.createFollower(hardwareMap); +// try { +// follower.getPoseTracker().resetIMU(); +// } catch (InterruptedException e) { +// throw new RuntimeException(e); +// } follower.setStartingPose(startingPose); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java index 006a9f1..882f1e5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java @@ -12,6 +12,7 @@ import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_CLOSE; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_MAX; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_MIN; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.BASKET_BLUE_Y; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.BASKET_X; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.BASKET_Y; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.DELTA_THRESHOLD; @@ -112,13 +113,9 @@ public void initSystems(HardwareMap hardwareMap) { // if(follower == null) // initFollower(hardwareMap, true); - try { - follower.getPoseTracker().resetIMU(); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } - follower.update(); + +// follower.update(); outtake.init(); intake.init(); loader.init(); @@ -297,8 +294,8 @@ public RangeConfig interpolateRange() { double robot_X = follower.getPose().getX(); double robot_Y = follower.getPose().getY(); - double dx = BASKET_X * (Blue_Target ? -1 : 1) - robot_X; - double dy = BASKET_Y - robot_Y; + double dy = (Blue_Target ? BASKET_BLUE_Y : BASKET_Y) - robot_Y; + double dx = BASKET_X - robot_X; distance = Math.sqrt(dx*dx + dy*dy); @@ -438,8 +435,8 @@ public void update() { double robot_Y = follower.getPose().getY(); double robotHeading = heading.get(); - double dx = BASKET_X * (Blue_Target ? -1 : 1) - robot_X; - double dy = BASKET_Y - robot_Y; + double dy = (Blue_Target ? BASKET_BLUE_Y : BASKET_Y) - robot_Y; + double dx = BASKET_X - robot_X; double targetFieldAngle = Math.atan2(dy, dx); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java index 7d6f3d5..5c7183f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java @@ -25,7 +25,8 @@ public class Auto_BackBlueOp extends OpMode { private Robot robot = Robot.getInstance(); private Timer pathTimer, opmodeTimer; - private int pathState, launchState; + private int pathState; + private int launchState=0; // Define poses Pose startingPoseBack = coordinates(StartingPoseBackBlue); @@ -41,9 +42,8 @@ public class Auto_BackBlueOp extends OpMode { @Override public void init() { - robot.init(hardwareMap); - robot.initFollower(hardwareMap, startingPoseBack); + robot.init(hardwareMap); pathTimer = new Timer(); opmodeTimer = new Timer(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java index 83c6d95..70ce1de 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java @@ -34,21 +34,23 @@ public Coordinates(double x, double y, double heading) { public static Coordinates IntakeZoneClose22 = new Coordinates(76, 110, -1.5); public static Coordinates LaunchZoneClose3 = new Coordinates(125, 144, -0.8); - public static Coordinates ParkClose = new Coordinates(100, 110, 0); + public static Coordinates ParkClose = new Coordinates(110, 110, 0); /// RED FAR auto movement public static Coordinates StartingPoseBackRed = new Coordinates(79, 7.4, 0); public static Coordinates LaunchZoneBack = new Coordinates(83, 19, -0.4); public static Coordinates ParkBack = new Coordinates(81, 35, 0); - /// BLUE auto movement + /// BLUE CLOSE auto movement public static Coordinates StartingPoseCloseBlue = new Coordinates(0, 0, 0); public static Coordinates LaunchZoneCloseBlue = new Coordinates(16, -32, 0.7); public static Coordinates LaunchZoneClose2Blue = new Coordinates(-25, -58, 0.8); public static Coordinates ParkCloseBlue = new Coordinates(-5, 0, 0); - public static Coordinates StartingPoseBackBlue = new Coordinates(0, 0, 0); - public static Coordinates LaunchZoneBackBlue = new Coordinates(11, 2.3, 0.4); - public static Coordinates ParkBackBlue = new Coordinates(30, 0, 0); + + /// BLUE FAR auto movement + public static Coordinates StartingPoseBackBlue = new Coordinates(130, 110, 0); + public static Coordinates LaunchZoneBackBlue = new Coordinates(141, 112.3, 0.4); + public static Coordinates ParkBackBlue = new Coordinates(160, 110, 0); public static Pose coordinates(Coordinates coord) { return new Pose(coord.x, coord.y, coord.heading); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java index e58df1a..35fd81f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java @@ -54,11 +54,15 @@ public class MainDrivingOp extends OpMode { boolean rumbled = false; +// Pose idk67; + @Override public void init() { lpsCounter = new LpsCounter(); lpsCounter.getLoopTime(); - robot.initFollower(hardwareMap, new Pose(68, 68, 0)); +// idk67 = new Pose(68, 68, 0); + robot.initFollower(hardwareMap, true); +// robot.follower.setStartingPose(idk67); robot.init(hardwareMap); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java index c17f1b5..77e64d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java @@ -66,14 +66,14 @@ public class Constants { public static double RANGE_1_VELOCITY = 1000; public static double RANGE_1_KS = 0.15; - public static double RANGE_2 = 70; + public static double RANGE_2 = 20; public static double RANGE_2_ANGLE = 0.32; public static double RANGE_2_VELOCITY = 1150; public static double RANGE_2_KS = 0.2; - public static double RANGE_3 = 100; + public static double RANGE_3 = 70; public static double RANGE_3_ANGLE = 0.64; - public static double RANGE_3_VELOCITY = 1450; + public static double RANGE_3_VELOCITY = 1350; public static double RANGE_3_KS = 0.3; @@ -109,8 +109,9 @@ public class Constants { public static double DELTA_THRESHOLD = 0.01; public static double MAX_ROTATION_POWER = 0.5; - public static double BASKET_X = 72; - public static double BASKET_Y = 72; + public static double BASKET_Y = 140; + public static double BASKET_BLUE_Y = 20; + public static double BASKET_X = 140; public static AutonomousConstants.Coordinates RedTowerCoords = new AutonomousConstants.Coordinates(130, 130, 0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index eb697a3..fd68f48 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -85,7 +85,7 @@ public class Constants { .hardwareMapName("pinpoint") .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD) - .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD) + .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED) ; From d0ccace9e0365ffba7a2daa6a0c2cd9617016fa1 Mon Sep 17 00:00:00 2001 From: ChiriacIoana Date: Fri, 27 Feb 2026 18:14:37 +0200 Subject: [PATCH 12/13] before regio 2 --- .../kronbot/autonomous/Auto_BackBlueOp.java | 109 +++---- .../kronbot/autonomous/Auto_BackRedOp.java | 145 ++++------ .../kronbot/autonomous/Auto_CloseBlueOp.java | 273 ++++++++++++++---- .../autonomous/AutonomousConstants.java | 27 +- 4 files changed, 315 insertions(+), 239 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java index 5c7183f..ede3951 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java @@ -2,6 +2,10 @@ import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.*; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_KS; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_VELOCITY; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_4_KS; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_4_VELOCITY; import static java.lang.Thread.sleep; @@ -29,20 +33,19 @@ public class Auto_BackBlueOp extends OpMode { private int launchState=0; // Define poses - Pose startingPoseBack = coordinates(StartingPoseBackBlue); + Pose start = coordinates(StartingPoseBackBlue); Pose launchZoneBack = coordinates(LaunchZoneBackBlue); Pose parkBack = coordinates(ParkBackBlue); private double motorVel; // Paths and PathChains - private PathChain goToLaunch; - private PathChain goToPark; + private PathChain goToLaunch, goToPark; @Override public void init() { - robot.initFollower(hardwareMap, startingPoseBack); + robot.initFollower(hardwareMap, start); robot.init(hardwareMap); pathTimer = new Timer(); @@ -64,8 +67,8 @@ public void init() { public void buildPaths() { goToLaunch = robot.follower.pathBuilder() - .addPath(new BezierLine(startingPoseBack, launchZoneBack)) - .setLinearHeadingInterpolation(startingPoseBack.getHeading(), launchZoneBack.getHeading()) + .addPath(new BezierLine(start, launchZoneBack)) + .setLinearHeadingInterpolation(start.getHeading(), launchZoneBack.getHeading()) .build(); goToPark = robot.follower.pathBuilder() @@ -85,9 +88,11 @@ public void start() { opmodeTimer.resetTimer(); setPathState(0); - - robot.turretServo.setPosition(0.5); - robot.angleServo.setPosition(angleServoBack); + robot.turretServo.setPosition(0); + robot.angleServo.setPosition(angleServoClose); + robot.flapsServo.setPosition(FLAP_OPEN); + robot.turretServo.setPosition(0.46); + robot.intakeMotor.setPower(-1); robot.loaderServo.runContinuous(false, false); } @@ -97,9 +102,9 @@ public void loop() { motorVel = robot.leftOuttake.getVelocity(); - autonomousPathUpdate(); - + robot.outtake.update(); + autonomousPathUpdate(); Pose currentPose = robot.follower.getPose(); telemetry.addData("Path State", pathState); @@ -107,6 +112,7 @@ public void loop() { telemetry.addData("Y", currentPose.getY()); telemetry.addData("Heading (rad)", currentPose.getHeading()); telemetry.addData("Shooter Motor vel", robot.leftOuttake.getVelocity()); + telemetry.addData("Launchstate: ", launchState); telemetry.update(); } @@ -115,6 +121,11 @@ public void autonomousPathUpdate() { switch (pathState) { case 0: + //also start motors to save time + robot.outtake.activeConfig.velocity = RANGE_4_VELOCITY; + robot.outtake.activeConfig.kS = RANGE_4_KS; + robot.outtake.on = true; + //go to pose robot.follower.followPath(goToLaunch); setPathState(1); break; @@ -123,91 +134,43 @@ public void autonomousPathUpdate() { if (!robot.follower.isBusy()) { switch (launchState) { case 0: - // Start outtake motors - robot.leftOuttake.setVelocity(launchSpeedBack); - robot.rightOuttake.setVelocity(launchSpeedBack); - robot.flapsServo.setPosition(FLAP_OPEN); launchState++; pathTimer.resetTimer(); break; case 1: - // Wait for motors to reach speed and launch 1 - if (motorVel+100 >= launchSpeedBack && motorVel+100 >= launchSpeedBack && pathTimer.getElapsedTimeSeconds() >= 4.0) { - robot.loaderServo.runContinuous(false, true); + // Wait for motors to reach speed and launch first 2 + if (motorVel + 50 >= RANGE_4_VELOCITY && pathTimer.getElapsedTimeSeconds() > 1.0) { + robot.loaderMotor.setPower(0.8); launchState++; pathTimer.resetTimer(); } break; case 2: - // Use color sensor to detect when ball is launched and stop servo (+timer for fallback safety) + // timer to see when all 3 are launched if (pathTimer.getElapsedTimeSeconds() > 2.0) { - robot.loaderServo.runContinuous(false, false); + robot.intakeMotor.setPower(0); + //robot.loaderMotor.setPower(0); launchState++; pathTimer.resetTimer(); } break; - case 3: - // Launch 2 - if (motorVel+100 >= launchSpeedBack && motorVel+100 >= launchSpeedBack) { - robot.loaderServo.runContinuous(false, true); - launchState++; - pathTimer.resetTimer(); - } - break; - - case 4: - // Stop servo between shots - if (pathTimer.getElapsedTimeSeconds() > 1.0) { - robot.loaderServo.runContinuous(false, false); - robot.intakeMotor.setPower(-1); - launchState++; - pathTimer.resetTimer(); - } - break; - - case 5: - // Launch 3 - if (motorVel+100 >= launchSpeedBack && motorVel+100 >= launchSpeedBack) { - robot.loaderServo.runContinuous(false, true); - launchState++; - pathTimer.resetTimer(); - } + setPathState(2); break; - case 6: - // Empty, stop motors - if (pathTimer.getElapsedTimeSeconds() > 3.0) { - robot.leftOuttake.setPower(0); - robot.rightOuttake.setPower(0); - robot.intakeMotor.setPower(0); - robot.loaderServo.runContinuous(false, false); - launchState++; - pathTimer.resetTimer(); - } - break; - - case 7: - // Exit shooting loop - if (pathTimer.getElapsedTimeSeconds() >= 2.0) { - launchState = 0; - setPathState(2); - } - break; } - } - break; - - case 2: + break; - if (!robot.follower.isBusy()) { - robot.follower.followPath(goToPark); - setPathState(3); } + case 2: + //go to pose + robot.follower.followPath(goToPark); + setPathState(-1); + robot.outtake.on = false; + robot.outtake.activeConfig = new Robot.RangeConfig(0, 0, 0); break; - case 3: if (!robot.follower.isBusy()) { setPathState(-1); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java index b5c1e73..1453963 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java @@ -1,11 +1,17 @@ package org.firstinspires.ftc.teamcode.kronbot.autonomous; import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.*; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_KS; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_VELOCITY; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_4_KS; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_4_VELOCITY; import static java.lang.Thread.sleep; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.Follower; import com.pedropathing.geometry.BezierLine; import com.pedropathing.geometry.Pose; import com.pedropathing.paths.PathChain; @@ -13,6 +19,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; +//import org.firstinspires.ftc.teamcode.kronbot.KronBot; import org.firstinspires.ftc.teamcode.kronbot.Robot; import org.firstinspires.ftc.teamcode.kronbot.utils.PoseStorage; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; @@ -22,26 +29,25 @@ public class Auto_BackRedOp extends OpMode { private Robot robot = Robot.getInstance(); private Timer pathTimer, opmodeTimer; - private int pathState, launchState; + private int pathState; + private int launchState=0; // Define poses - Pose startingPose = coordinates(StartingPoseBackRed); - Pose launchZone = coordinates(LaunchZoneBack); - Pose parkZone = coordinates(ParkBack); - - + Pose start = coordinates(StartingPoseBackRed); + Pose launchZoneBack = coordinates(LaunchZoneBack); + Pose parkBack = coordinates(ParkBack); private double motorVel; + // Paths and PathChains private PathChain goToLaunch, goToPark; @Override public void init() { + robot.initFollower(hardwareMap, start); robot.init(hardwareMap); - robot.initFollower(hardwareMap, startingPose); - pathTimer = new Timer(); opmodeTimer = new Timer(); opmodeTimer.resetTimer(); @@ -57,17 +63,17 @@ public void init() { telemetry.update(); } - + /** Build all paths for the auto **/ public void buildPaths() { goToLaunch = robot.follower.pathBuilder() - .addPath(new BezierLine(startingPose, launchZone)) - .setLinearHeadingInterpolation(startingPose.getHeading(), launchZone.getHeading()) + .addPath(new BezierLine(start, launchZoneBack)) + .setLinearHeadingInterpolation(start.getHeading(), launchZoneBack.getHeading()) .build(); goToPark = robot.follower.pathBuilder() - .addPath(new BezierLine(launchZone, parkZone)) - .setLinearHeadingInterpolation(launchZone.getHeading(), parkZone.getHeading()) + .addPath(new BezierLine(launchZoneBack, parkBack)) + .setLinearHeadingInterpolation(launchZoneBack.getHeading(), parkBack.getHeading()) .build(); } @@ -82,9 +88,11 @@ public void start() { opmodeTimer.resetTimer(); setPathState(0); - - robot.turretServo.setPosition(0.5); - robot.angleServo.setPosition(angleServoBack); + robot.turretServo.setPosition(0); + robot.angleServo.setPosition(angleServoClose); + robot.flapsServo.setPosition(FLAP_OPEN); + robot.turretServo.setPosition(0.46); + robot.intakeMotor.setPower(-1); robot.loaderServo.runContinuous(false, false); } @@ -94,6 +102,8 @@ public void loop() { motorVel = robot.leftOuttake.getVelocity(); + robot.outtake.update(); + autonomousPathUpdate(); Pose currentPose = robot.follower.getPose(); @@ -102,6 +112,7 @@ public void loop() { telemetry.addData("Y", currentPose.getY()); telemetry.addData("Heading (rad)", currentPose.getHeading()); telemetry.addData("Shooter Motor vel", robot.leftOuttake.getVelocity()); + telemetry.addData("Launchstate: ", launchState); telemetry.update(); } @@ -110,6 +121,11 @@ public void autonomousPathUpdate() { switch (pathState) { case 0: + //also start motors to save time + robot.outtake.activeConfig.velocity = RANGE_4_VELOCITY; + robot.outtake.activeConfig.kS = RANGE_4_KS; + robot.outtake.on = true; + //go to pose robot.follower.followPath(goToLaunch); setPathState(1); break; @@ -118,103 +134,43 @@ public void autonomousPathUpdate() { if (!robot.follower.isBusy()) { switch (launchState) { case 0: - // Start outtake motors - if(pathTimer.getElapsedTimeSeconds() >= 0.5) { - robot.outtake.on = true; - robot.outtake.activeConfig.velocity = launchSpeedBack; - robot.outtake.activeConfig.kS = 0.5; // magic number from constants - robot.flap.open = true; - launchState++; - robot.updateAllSystems(); - pathTimer.resetTimer(); - } - + launchState++; + pathTimer.resetTimer(); break; case 1: - // Wait for motors to reach speed and launch 1 - if (motorVel+40 >= launchSpeedBack) { - robot.loaderServo.runContinuous(false, true); - robot.outtake.activeConfig.angle = 1; + // Wait for motors to reach speed and launch first 2 + if (motorVel + 50 >= RANGE_4_VELOCITY && pathTimer.getElapsedTimeSeconds() > 1.0) { + robot.loaderMotor.setPower(0.8); launchState++; - robot.updateAllSystems(); pathTimer.resetTimer(); } break; case 2: - // Use color sensor to detect when ball is launched and stop servo (+timer for fallback safety) - if (pathTimer.getElapsedTimeSeconds() > 3.0) { - robot.loaderServo.runContinuous(false, false); + // timer to see when all 3 are launched + if (pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.intakeMotor.setPower(0); + //robot.loaderMotor.setPower(0); launchState++; - robot.updateAllSystems(); pathTimer.resetTimer(); } break; - case 3: - // Launch 2 - if (motorVel+40 >= launchSpeedBack) { - robot.loaderServo.runContinuous(false, true); - launchState++; - robot.updateAllSystems(); - pathTimer.resetTimer(); - } - break; - - case 4: - // Stop servo between shots - if (pathTimer.getElapsedTimeSeconds() > 1.5) { - robot.loaderServo.runContinuous(false, false); - robot.intake.reversed = true; - robot.intake.speed = 1; - launchState++; - robot.updateAllSystems(); - pathTimer.resetTimer(); - } + setPathState(2); break; - case 5: - // Launch 3 - if (motorVel+40 >= launchSpeedBack) { - robot.loaderServo.runContinuous(false, true); - launchState++; - robot.updateAllSystems(); - pathTimer.resetTimer(); - } - break; - - case 6: - // Empty, stop motors - if (pathTimer.getElapsedTimeSeconds() > 1.0) { - robot.outtake.on = false; - robot.intake.speed = 0; - robot.loaderServo.runContinuous(false, false); - launchState++; - robot.updateAllSystems(); - pathTimer.resetTimer(); - } - break; - - case 7: - // Exit shooting loop - if (pathTimer.getElapsedTimeSeconds() >= 3.0) { - launchState = 0; - setPathState(2); - } - break; } - } - break; + break; - case 2: - - if (!robot.follower.isBusy()) { - robot.follower.followPath(goToPark); - setPathState(3); } + case 2: + //go to pose + robot.follower.followPath(goToPark); + setPathState(-1); + robot.outtake.on = false; + robot.outtake.activeConfig = new Robot.RangeConfig(0, 0, 0); break; - case 3: if (!robot.follower.isBusy()) { setPathState(-1); @@ -223,8 +179,7 @@ public void autonomousPathUpdate() { case -1: -// Pose finalPose = robot.follower.getPose(); -// PoseStorage.savePose(finalPose); + // Idle / done break; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseBlueOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseBlueOp.java index e8158e2..e4b1c0e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseBlueOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseBlueOp.java @@ -1,7 +1,11 @@ package org.firstinspires.ftc.teamcode.kronbot.autonomous; import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.*; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_CLOSE; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_CLOSED; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_KS; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_VELOCITY; import com.acmerobotics.dashboard.FtcDashboard; @@ -24,28 +28,34 @@ public class Auto_CloseBlueOp extends OpMode { private Robot robot = Robot.getInstance(); private Timer pathTimer, opmodeTimer; private int pathState; - private int launchState; + private int launchState=0; // Define poses - Pose startingPose = coordinates(StartingPoseCloseBlue); - Pose launchZone = coordinates(LaunchZoneCloseBlue); + Pose start = coordinates(StartingPoseCloseBlue); + Pose launch1 = coordinates(LaunchZoneClose1Blue); + Pose launch11 = coordinates(LaunchZoneClose11Blue); + Pose launch2 = coordinates(LaunchZoneClose2Blue); + Pose intake1 = coordinates(IntakeZoneClose1Blue); + Pose intake11 = coordinates(IntakeZoneClose11Blue); + + Pose intake2 = coordinates(IntakeZoneClose2Blue); + Pose intake22 = coordinates(IntakeZoneClose22Blue); + Pose launch3 = coordinates(LaunchZoneClose3Blue); - Pose launchZone2 = coordinates(LaunchZoneClose2Blue); Pose parkZone = coordinates(ParkCloseBlue); private double motorVel; // Paths and PathChains - private PathChain goToLaunch, goToPark; + private PathChain goToLaunch1, goToLaunch11, goToLaunch2, goToLaunch3, goToIntake1, goToIntake11, goToIntake2, goToIntake22, goToPark; @Override public void init() { + robot.initFollower(hardwareMap, start); robot.init(hardwareMap); - robot.initFollower(hardwareMap, startingPose); - pathTimer = new Timer(); opmodeTimer = new Timer(); opmodeTimer.resetTimer(); @@ -64,16 +74,49 @@ public void init() { public void buildPaths() { - goToLaunch = robot.follower.pathBuilder() - .addPath(new BezierLine(startingPose, launchZone)) - .setLinearHeadingInterpolation(startingPose.getHeading(), launchZone.getHeading()) - .addPath(new BezierLine(launchZone, launchZone2)) - .setLinearHeadingInterpolation(launchZone.getHeading(), launchZone2.getHeading()) + goToLaunch1 = robot.follower.pathBuilder() + .addPath(new BezierLine(start, launch1)) + .setLinearHeadingInterpolation(start.getHeading(), launch1.getHeading()) + .build(); + + goToLaunch11 = robot.follower.pathBuilder() + .addPath(new BezierLine(launch1, launch11)) + .setLinearHeadingInterpolation(launch1.getHeading(), launch11.getHeading()) + .build(); + + goToIntake1 = robot.follower.pathBuilder() + .addPath(new BezierLine(launch11, intake1)) + .setLinearHeadingInterpolation(launch11.getHeading(), intake1.getHeading()) + .build(); + goToIntake11 = robot.follower.pathBuilder() + .addPath(new BezierLine(intake1, intake11)) + .setLinearHeadingInterpolation(intake1.getHeading(), intake11.getHeading()) + .setBrakingStrength(0.2) + .build(); + + goToLaunch2 = robot.follower.pathBuilder() + .addPath(new BezierLine(intake11, launch2)) + .setLinearHeadingInterpolation(intake11.getHeading(), launch2.getHeading()) + .build(); + + + goToIntake2 = robot.follower.pathBuilder() + .addPath(new BezierLine(launch2, intake2)) + .setLinearHeadingInterpolation(launch2.getHeading(), intake2.getHeading()) + .build(); + goToIntake22 = robot.follower.pathBuilder() + .addPath(new BezierLine(intake2, intake22)) + .setLinearHeadingInterpolation(intake2.getHeading(), intake22.getHeading()) + .setBrakingStrength(0.2) + .build(); + goToLaunch3 = robot.follower.pathBuilder() + .addPath(new BezierLine(intake22, launch3)) + .setLinearHeadingInterpolation(intake22.getHeading(), launch3.getHeading()) .build(); goToPark = robot.follower.pathBuilder() - .addPath(new BezierLine(launchZone2, parkZone)) - .setLinearHeadingInterpolation(launchZone2.getHeading(), parkZone.getHeading()) + .addPath(new BezierLine(launch3, parkZone)) + .setLinearHeadingInterpolation(launch3.getHeading(), parkZone.getHeading()) .build(); } @@ -88,8 +131,11 @@ public void start() { opmodeTimer.resetTimer(); setPathState(0); - robot.turretServo.setPosition(0.5); + robot.turretServo.setPosition(0); robot.angleServo.setPosition(angleServoClose); + robot.flapsServo.setPosition(FLAP_OPEN); + robot.turretServo.setPosition(0.46); + robot.intakeMotor.setPower(-1); robot.loaderServo.runContinuous(false, false); } @@ -99,6 +145,8 @@ public void loop() { motorVel = robot.leftOuttake.getVelocity(); + robot.outtake.update(); + autonomousPathUpdate(); Pose currentPose = robot.follower.getPose(); @@ -108,6 +156,7 @@ public void loop() { telemetry.addData("Heading (rad)", currentPose.getHeading()); //telemetry.addData("Outtake Alpha", robot.outtakeColor.alpha()); telemetry.addData("Shooter Motor vel", robot.leftOuttake.getVelocity()); + telemetry.addData("Launchstate: ", launchState); telemetry.update(); } @@ -116,111 +165,213 @@ public void autonomousPathUpdate() { switch (pathState) { case 0: - robot.follower.followPath(goToLaunch); + //also start motors to save time + robot.outtake.activeConfig.velocity = RANGE_2_VELOCITY; + robot.outtake.activeConfig.kS = RANGE_2_KS; + robot.outtake.on = true; + //go to pose + robot.follower.followPath(goToLaunch1); setPathState(1); break; case 1: + //go to pose + if (!robot.follower.isBusy()) { + robot.follower.followPath(goToLaunch11); + launchState = 0; + setPathState(2); + } + break; + + case 2: if (!robot.follower.isBusy()) { switch (launchState) { case 0: - // Start outtake motors - robot.leftOuttake.setVelocity(launchSpeedClose); - robot.rightOuttake.setVelocity(launchSpeedClose); - robot.flapsServo.setPosition(FLAP_OPEN); launchState++; pathTimer.resetTimer(); break; case 1: - // Wait for motors to reach speed and launch 1 - if (motorVel+100 >= launchSpeedClose && motorVel+100 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 4.0) { - robot.loaderServo.runContinuous(false, true); + // Wait for motors to reach speed and launch first 2 + if (motorVel + 50 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 1.0) { + robot.loaderMotor.setPower(0.8); launchState++; pathTimer.resetTimer(); } break; case 2: - // Use color sensor to detect when ball is launched and stop servo (+timer for fallback safety) - if (pathTimer.getElapsedTimeSeconds() > 1.0) { - robot.loaderServo.runContinuous(false, false); + // timer to see when all 3 are launched + if (pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.intakeMotor.setPower(0); + //robot.loaderMotor.setPower(0); launchState++; pathTimer.resetTimer(); } break; - case 3: - // Launch 2 - if (motorVel+100 >= launchSpeedClose && motorVel+100 >= launchSpeedClose) { - robot.loaderServo.runContinuous(false, true); + setPathState(3); + break; + + } + break; + + } + case 3: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.flapsServo.setPosition(FLAP_CLOSED); + robot.follower.followPath(goToIntake1); + robot.intakeMotor.setPower(-1); + robot.loaderMotor.setPower(1); + + pathTimer.resetTimer(); + setPathState(4); + } + break; + + case 4: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToIntake11); + pathTimer.resetTimer(); + setPathState(5); + } + break; + + case 5: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToLaunch2); + + robot.loaderMotor.setPower(0); + robot.loaderMotor.setPower(-0.3); + launchState = 0; + pathTimer.resetTimer(); + setPathState(6); + } + break; + + case 6: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>0.6) { + switch (launchState) { + case 0: + launchState++; + pathTimer.resetTimer(); + robot.flapsServo.setPosition(FLAP_OPEN); + robot.turretServo.setPosition(0.5); + break; + + case 1: + // Wait for motors to reach speed and launch first 2 + if (motorVel + 50 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.loaderMotor.setPower(0.7); launchState++; pathTimer.resetTimer(); } break; - case 4: - // Stop servo between shots - if (pathTimer.getElapsedTimeSeconds() > 2.0) { - robot.loaderServo.runContinuous(false, false); - robot.intakeMotor.setPower(-1); + case 2: + // timer to see when all 3 are launched + if (pathTimer.getElapsedTimeSeconds() > 1.0) { + robot.intakeMotor.setPower(0); + //robot.loaderMotor.setPower(0); launchState++; pathTimer.resetTimer(); } break; + case 3: + setPathState(7); + break; + + } + break; + + } - case 5: - // Launch 3 - if (motorVel+100 >= launchSpeedClose && motorVel+100 >= launchSpeedClose) { - robot.loaderServo.runContinuous(false, true); + case 7: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.flapsServo.setPosition(FLAP_CLOSED); + robot.follower.followPath(goToIntake2); + robot.intakeMotor.setPower(-1); + robot.loaderMotor.setPower(1); + + pathTimer.resetTimer(); + setPathState(8); + } + break; + + case 8: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToIntake22); + pathTimer.resetTimer(); + setPathState(9); + } + break; + + case 9: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToLaunch3); + robot.loaderMotor.setPower(0); + robot.loaderMotor.setPower(-0.3); + launchState = 0; + pathTimer.resetTimer(); + setPathState(10); + } + break; + + case 10: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>0.6) { + switch (launchState) { + case 0: + launchState++; + pathTimer.resetTimer(); + robot.flapsServo.setPosition(FLAP_OPEN); + robot.turretServo.setPosition(0.47); + break; + + case 1: + // Wait for motors to reach speed and launch first 2 + if (motorVel + 50 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.loaderMotor.setPower(0.7); launchState++; pathTimer.resetTimer(); } break; - case 6: - // Empty, stop motors + case 2: + // timer to see when all 3 are launched if (pathTimer.getElapsedTimeSeconds() > 1.0) { - robot.leftOuttake.setPower(0); robot.intakeMotor.setPower(0); - robot.loaderServo.runContinuous(false, false); + robot.outtake.on = false; + robot.outtake.activeConfig = new Robot.RangeConfig(0, 0, 0); + //robot.loaderMotor.setPower(0); launchState++; pathTimer.resetTimer(); } break; - - case 7: - // Exit shooting loop - if (pathTimer.getElapsedTimeSeconds() >= 3.0) { - launchState = 0; - setPathState(2); - } + case 3: + setPathState(11); break; - } - } - break; - case 2: + } + break; - if (!robot.follower.isBusy()) { - robot.follower.followPath(goToPark); - setPathState(3); } - break; - case 3: - if (!robot.follower.isBusy()) { + case 11: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToPark); + robot.loaderMotor.setPower(0); + pathTimer.resetTimer(); setPathState(-1); } break; - case -1: Pose finalPose = robot.follower.getPose(); PoseStorage.savePose(finalPose); break; } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java index 70ce1de..3876bbb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java @@ -37,20 +37,27 @@ public Coordinates(double x, double y, double heading) { public static Coordinates ParkClose = new Coordinates(110, 110, 0); /// RED FAR auto movement - public static Coordinates StartingPoseBackRed = new Coordinates(79, 7.4, 0); - public static Coordinates LaunchZoneBack = new Coordinates(83, 19, -0.4); - public static Coordinates ParkBack = new Coordinates(81, 35, 0); + public static Coordinates StartingPoseBackRed = new Coordinates(80, 8.7, 1.57); + public static Coordinates LaunchZoneBack = new Coordinates(85, 17, 1.17); + public static Coordinates ParkBack = new Coordinates(84, 34.5, 1.57); /// BLUE CLOSE auto movement - public static Coordinates StartingPoseCloseBlue = new Coordinates(0, 0, 0); - public static Coordinates LaunchZoneCloseBlue = new Coordinates(16, -32, 0.7); - public static Coordinates LaunchZoneClose2Blue = new Coordinates(-25, -58, 0.8); - public static Coordinates ParkCloseBlue = new Coordinates(-5, 0, 0); + public static Coordinates StartingPoseCloseBlue = new Coordinates(14, 110, 0); + public static Coordinates LaunchZoneClose1Blue = new Coordinates(19, 140, 0); + public static Coordinates LaunchZoneClose11Blue = new Coordinates(25, 144, 0.8); + public static Coordinates IntakeZoneClose1Blue = new Coordinates(40, 140, 1.5); + public static Coordinates IntakeZoneClose11Blue = new Coordinates(40, 110, 1.5); + public static Coordinates LaunchZoneClose2Blue = new Coordinates(30, 144, 0.8); + public static Coordinates IntakeZoneClose2Blue = new Coordinates(10, 140, 1.5); + public static Coordinates IntakeZoneClose22Blue = new Coordinates(10, 110, 1.5); + + public static Coordinates LaunchZoneClose3Blue = new Coordinates(30, 144, 0.8); + public static Coordinates ParkCloseBlue = new Coordinates(-6, 110, 0); /// BLUE FAR auto movement - public static Coordinates StartingPoseBackBlue = new Coordinates(130, 110, 0); - public static Coordinates LaunchZoneBackBlue = new Coordinates(141, 112.3, 0.4); - public static Coordinates ParkBackBlue = new Coordinates(160, 110, 0); + public static Coordinates StartingPoseBackBlue = new Coordinates(63, 8, 1.57); + public static Coordinates LaunchZoneBackBlue = new Coordinates(60, 17, 1.97); + public static Coordinates ParkBackBlue = new Coordinates(61, 32, 1.57); public static Pose coordinates(Coordinates coord) { return new Pose(coord.x, coord.y, coord.heading); From d7cbea22436176fc0b19e715b24846a4a44b2d06 Mon Sep 17 00:00:00 2001 From: ChiriacIoana Date: Mon, 2 Mar 2026 16:33:31 +0200 Subject: [PATCH 13/13] after regio --- .../kronbot/autonomous/Auto_BackBlueOp.java | 135 +++++++++++++++--- .../kronbot/autonomous/Auto_BackRedOp.java | 123 ++++++++++++++-- .../kronbot/autonomous/Auto_CloseRedOp.java | 10 +- .../autonomous/AutonomousConstants.java | 26 ++-- .../kronbot/manual/MainDrivingOp.java | 3 - .../ftc/teamcode/kronbot/utils/Constants.java | 18 +-- .../ftc/teamcode/pedroPathing/Constants.java | 6 +- 7 files changed, 266 insertions(+), 55 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java index ede3951..b85c25c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.kronbot.autonomous; import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.*; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_CLOSED; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_KS; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_VELOCITY; @@ -35,12 +36,17 @@ public class Auto_BackBlueOp extends OpMode { // Define poses Pose start = coordinates(StartingPoseBackBlue); Pose launchZoneBack = coordinates(LaunchZoneBackBlue); + Pose launch2 = coordinates(LaunchZoneBackBlue); + + Pose intake1 = coordinates(IntakeZoneBack1Blue); + + Pose intake11 = coordinates(IntakeZoneBack11Blue); Pose parkBack = coordinates(ParkBackBlue); private double motorVel; // Paths and PathChains - private PathChain goToLaunch, goToPark; + private PathChain goToLaunch, goToPark, goToIntake1, goToIntake11, goToLaunch2; @Override public void init() { @@ -71,6 +77,21 @@ public void buildPaths() { .setLinearHeadingInterpolation(start.getHeading(), launchZoneBack.getHeading()) .build(); + goToIntake1 = robot.follower.pathBuilder() + .addPath(new BezierLine(launchZoneBack, intake1)) + .setLinearHeadingInterpolation(launchZoneBack.getHeading(), intake1.getHeading()) + .build(); + + goToIntake11 = robot.follower.pathBuilder() + .addPath(new BezierLine(intake1, intake11)) + .setLinearHeadingInterpolation(intake1.getHeading(), intake11.getHeading()) + .build(); + + goToLaunch2 = robot.follower.pathBuilder() + .addPath(new BezierLine(intake11, launch2)) + .setLinearHeadingInterpolation(intake11.getHeading(), launch2.getHeading()) + .build(); + goToPark = robot.follower.pathBuilder() .addPath(new BezierLine(launchZoneBack, parkBack)) .setLinearHeadingInterpolation(launchZoneBack.getHeading(), parkBack.getHeading()) @@ -89,9 +110,9 @@ public void start() { setPathState(0); robot.turretServo.setPosition(0); - robot.angleServo.setPosition(angleServoClose); + robot.angleServo.setPosition(angleServoBack); robot.flapsServo.setPosition(FLAP_OPEN); - robot.turretServo.setPosition(0.46); + robot.turretServo.setPosition(0.54); robot.intakeMotor.setPower(-1); robot.loaderServo.runContinuous(false, false); } @@ -121,14 +142,18 @@ public void autonomousPathUpdate() { switch (pathState) { case 0: - //also start motors to save time - robot.outtake.activeConfig.velocity = RANGE_4_VELOCITY; - robot.outtake.activeConfig.kS = RANGE_4_KS; + //also start motors to save time robot.outtake.on = true; - //go to pose - robot.follower.followPath(goToLaunch); - setPathState(1); - break; + robot.shoot.activateRange(4); + robot.outtake.update(); +// robot.leftOuttake.setVelocity(launchSpeedBack); +// robot.rightOuttake.setVelocity(launchSpeedBack); +// robot.angleServo.setPosition(angleServoBack); +// + //go to pose + robot.follower.followPath(goToLaunch); + setPathState(1); + break; case 1: if (!robot.follower.isBusy()) { @@ -140,7 +165,7 @@ public void autonomousPathUpdate() { case 1: // Wait for motors to reach speed and launch first 2 - if (motorVel + 50 >= RANGE_4_VELOCITY && pathTimer.getElapsedTimeSeconds() > 1.0) { + if (motorVel + 50 >= launchSpeedBack && pathTimer.getElapsedTimeSeconds() > 1.5) { robot.loaderMotor.setPower(0.8); launchState++; pathTimer.resetTimer(); @@ -164,14 +189,92 @@ public void autonomousPathUpdate() { break; } + + case 2: - //go to pose - robot.follower.followPath(goToPark); - setPathState(-1); - robot.outtake.on = false; - robot.outtake.activeConfig = new Robot.RangeConfig(0, 0, 0); + if (!robot.follower.isBusy() && robot.follower.getHeadingError()<0.1 && pathTimer.getElapsedTimeSeconds()>1) { + robot.flapsServo.setPosition(FLAP_CLOSED); + robot.follower.followPath(goToIntake1); + robot.intakeMotor.setPower(-1); + robot.loaderMotor.setPower(1); + + pathTimer.resetTimer(); + setPathState(3); + } break; + case 3: + if (!robot.follower.isBusy() && robot.follower.getHeadingError()<0.1 && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToIntake11); + pathTimer.resetTimer(); + setPathState(4); + } + break; + + + case 4: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToLaunch2); + + robot.loaderMotor.setPower(0); + robot.loaderMotor.setPower(-0.3); + launchState = 0; + pathTimer.resetTimer(); + setPathState(5); + } + break; + + case 5: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>0.6) { + switch (launchState) { + case 0: + launchState++; + pathTimer.resetTimer(); + robot.flapsServo.setPosition(FLAP_OPEN); + robot.turretServo.setPosition(0.5); + robot.outtake.on = true; + robot.shoot.activateRange(4); + break; + + case 1: + // Wait for motors to reach speed and launch first 2 + if (pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.loaderMotor.setPower(0.7); + launchState++; + pathTimer.resetTimer(); + } + break; + + case 2: + // timer to see when all 3 are launched + if (pathTimer.getElapsedTimeSeconds() > 1.0) { + robot.intakeMotor.setPower(0); + //robot.loaderMotor.setPower(0); + launchState++; + pathTimer.resetTimer(); + } + break; + case 3: + setPathState(6); + break; + + } + break; + + } + + case 6: + //go to pose + if (!robot.follower.isBusy()) { + robot.follower.followPath(goToPark); + + robot.outtake.on = false; + robot.outtake.activeConfig = new Robot.RangeConfig(0, 0, 0); + setPathState(7); + } + break; + + case 7: if (!robot.follower.isBusy()) { setPathState(-1); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java index 1453963..76bcdcd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.kronbot.autonomous; import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.*; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_CLOSED; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_KS; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_VELOCITY; @@ -14,6 +15,7 @@ import com.pedropathing.follower.Follower; import com.pedropathing.geometry.BezierLine; import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.HeadingInterpolator; import com.pedropathing.paths.PathChain; import com.pedropathing.util.Timer; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -35,12 +37,17 @@ public class Auto_BackRedOp extends OpMode { // Define poses Pose start = coordinates(StartingPoseBackRed); Pose launchZoneBack = coordinates(LaunchZoneBack); + Pose launch2 = coordinates(LaunchZoneBack); + + Pose intake1 = coordinates(IntakeZoneBack1); + + Pose intake11 = coordinates(IntakeZoneBack11); Pose parkBack = coordinates(ParkBack); private double motorVel; // Paths and PathChains - private PathChain goToLaunch, goToPark; + private PathChain goToLaunch, goToLaunch2, goToPark, goToIntake1, goToIntake11; @Override public void init() { @@ -71,6 +78,22 @@ public void buildPaths() { .setLinearHeadingInterpolation(start.getHeading(), launchZoneBack.getHeading()) .build(); + goToIntake1 = robot.follower.pathBuilder() + .addPath(new BezierLine(launchZoneBack, intake1)) + .setLinearHeadingInterpolation(launchZoneBack.getHeading(), intake1.getHeading()) + .build(); + + goToIntake11 = robot.follower.pathBuilder() + .addPath(new BezierLine(intake1, intake11)) + .setLinearHeadingInterpolation(intake1.getHeading(), intake11.getHeading()) + .build(); + + + goToLaunch2 = robot.follower.pathBuilder() + .addPath(new BezierLine(intake11, launch2)) + .setLinearHeadingInterpolation(intake11.getHeading(), launch2.getHeading()) + .build(); + goToPark = robot.follower.pathBuilder() .addPath(new BezierLine(launchZoneBack, parkBack)) .setLinearHeadingInterpolation(launchZoneBack.getHeading(), parkBack.getHeading()) @@ -89,7 +112,7 @@ public void start() { setPathState(0); robot.turretServo.setPosition(0); - robot.angleServo.setPosition(angleServoClose); + robot.angleServo.setPosition(angleServoBack); robot.flapsServo.setPosition(FLAP_OPEN); robot.turretServo.setPosition(0.46); robot.intakeMotor.setPower(-1); @@ -111,6 +134,7 @@ public void loop() { telemetry.addData("X", currentPose.getX()); telemetry.addData("Y", currentPose.getY()); telemetry.addData("Heading (rad)", currentPose.getHeading()); + //telemetry.addData("Outtake Alpha", robot.outtakeColor.alpha()); telemetry.addData("Shooter Motor vel", robot.leftOuttake.getVelocity()); telemetry.addData("Launchstate: ", launchState); @@ -122,9 +146,13 @@ public void autonomousPathUpdate() { switch (pathState) { case 0: //also start motors to save time - robot.outtake.activeConfig.velocity = RANGE_4_VELOCITY; - robot.outtake.activeConfig.kS = RANGE_4_KS; robot.outtake.on = true; + robot.shoot.activateRange(4); + robot.outtake.update(); +// robot.leftOuttake.setVelocity(launchSpeedBack); +// robot.rightOuttake.setVelocity(launchSpeedBack); +// robot.angleServo.setPosition(angleServoBack); +// //go to pose robot.follower.followPath(goToLaunch); setPathState(1); @@ -140,7 +168,7 @@ public void autonomousPathUpdate() { case 1: // Wait for motors to reach speed and launch first 2 - if (motorVel + 50 >= RANGE_4_VELOCITY && pathTimer.getElapsedTimeSeconds() > 1.0) { + if (motorVel + 50 >= launchSpeedBack && pathTimer.getElapsedTimeSeconds() > 1.5) { robot.loaderMotor.setPower(0.8); launchState++; pathTimer.resetTimer(); @@ -164,14 +192,89 @@ public void autonomousPathUpdate() { break; } + + case 2: - //go to pose - robot.follower.followPath(goToPark); - setPathState(-1); - robot.outtake.on = false; - robot.outtake.activeConfig = new Robot.RangeConfig(0, 0, 0); + if (!robot.follower.isBusy() && robot.follower.getHeadingError()<0.1 && pathTimer.getElapsedTimeSeconds()>1) { + robot.flapsServo.setPosition(FLAP_CLOSED); + robot.follower.followPath(goToIntake1); + robot.intakeMotor.setPower(-1); + robot.loaderMotor.setPower(1); + + pathTimer.resetTimer(); + setPathState(3); + } break; + case 3: + if (!robot.follower.isBusy() && robot.follower.getHeadingError()<0.1 && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToIntake11); + pathTimer.resetTimer(); + setPathState(4); + } + break; + + + case 4: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + robot.follower.followPath(goToLaunch2); + + robot.loaderMotor.setPower(0); + robot.loaderMotor.setPower(-0.3); + launchState = 0; + pathTimer.resetTimer(); + setPathState(5); + } + break; + + case 5: + if (!robot.follower.isBusy() && pathTimer.getElapsedTimeSeconds()>0.6) { + switch (launchState) { + case 0: + launchState++; + pathTimer.resetTimer(); + robot.flapsServo.setPosition(FLAP_OPEN); + robot.turretServo.setPosition(0.5); + break; + + case 1: + // Wait for motors to reach speed and launch first 2 + if (motorVel + 50 >= launchSpeedClose && pathTimer.getElapsedTimeSeconds() > 2.0) { + robot.loaderMotor.setPower(0.7); + launchState++; + pathTimer.resetTimer(); + } + break; + + case 2: + // timer to see when all 3 are launched + if (pathTimer.getElapsedTimeSeconds() > 1.0) { + robot.intakeMotor.setPower(0); + //robot.loaderMotor.setPower(0); + launchState++; + pathTimer.resetTimer(); + } + break; + case 3: + setPathState(6); + break; + + } + break; + + } + + case 6: + //go to pose + if (!robot.follower.isBusy()) { + robot.follower.followPath(goToPark); + setPathState(7); + robot.outtake.on = false; + robot.outtake.activeConfig = new Robot.RangeConfig(0, 0, 0); + } + break; + + case 7: if (!robot.follower.isBusy()) { setPathState(-1); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java index dfcf616..4321a16 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java @@ -94,6 +94,7 @@ public void buildPaths() { .setBrakingStrength(0.2) .build(); + goToLaunch2 = robot.follower.pathBuilder() .addPath(new BezierLine(intake11, launch2)) .setLinearHeadingInterpolation(intake11.getHeading(), launch2.getHeading()) @@ -134,7 +135,7 @@ public void start() { robot.turretServo.setPosition(0); robot.angleServo.setPosition(angleServoClose); robot.flapsServo.setPosition(FLAP_OPEN); - robot.turretServo.setPosition(0.46); + robot.turretServo.setPosition(0.3); robot.intakeMotor.setPower(-1); robot.loaderServo.runContinuous(false, false); } @@ -166,9 +167,10 @@ public void autonomousPathUpdate() { switch (pathState) { case 0: //also start motors to save time - robot.outtake.activeConfig.velocity = RANGE_2_VELOCITY; - robot.outtake.activeConfig.kS = RANGE_2_KS; - robot.outtake.on = true; + robot.leftOuttake.setVelocity(launchSpeedClose); + robot.rightOuttake.setVelocity(launchSpeedClose); + robot.angleServo.setPosition(angleServoClose); +// robot.outtake.on = true; //go to pose robot.follower.followPath(goToLaunch1); setPathState(1); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java index 3876bbb..f2bb14f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java @@ -18,10 +18,10 @@ public Coordinates(double x, double y, double heading) { } } - public static double launchSpeedClose = 1150; - public static double launchSpeedBack = 1400; - public static double angleServoBack = 0.6; - public static double angleServoClose = 0.32; + public static double launchSpeedClose = 1230; + public static double launchSpeedBack = 1600; + public static double angleServoBack = 0.3; + public static double angleServoClose = 0.2; /// RED CLOSE auto movement public static Coordinates StartingPoseCloseRed = new Coordinates(130, 110, 0); @@ -37,9 +37,13 @@ public Coordinates(double x, double y, double heading) { public static Coordinates ParkClose = new Coordinates(110, 110, 0); /// RED FAR auto movement - public static Coordinates StartingPoseBackRed = new Coordinates(80, 8.7, 1.57); - public static Coordinates LaunchZoneBack = new Coordinates(85, 17, 1.17); - public static Coordinates ParkBack = new Coordinates(84, 34.5, 1.57); + public static Coordinates StartingPoseBackRed = new Coordinates(0, -75, 0); + public static Coordinates LaunchZoneBack = new Coordinates(7, -84, -0.13); + public static Coordinates IntakeZoneBack1 = new Coordinates(8, -100, -1.5); + public static Coordinates IntakeZoneBack11 = new Coordinates(5, -144, -1.5); + public static Coordinates ParkBack = new Coordinates(15, -80, 0); + + /// BLUE CLOSE auto movement public static Coordinates StartingPoseCloseBlue = new Coordinates(14, 110, 0); @@ -55,9 +59,11 @@ public Coordinates(double x, double y, double heading) { public static Coordinates ParkCloseBlue = new Coordinates(-6, 110, 0); /// BLUE FAR auto movement - public static Coordinates StartingPoseBackBlue = new Coordinates(63, 8, 1.57); - public static Coordinates LaunchZoneBackBlue = new Coordinates(60, 17, 1.97); - public static Coordinates ParkBackBlue = new Coordinates(61, 32, 1.57); + public static Coordinates StartingPoseBackBlue = new Coordinates(5, -63, 0); + public static Coordinates LaunchZoneBackBlue = new Coordinates(9, -59, 0.13); + public static Coordinates IntakeZoneBack1Blue = new Coordinates(5, -40, 1.5); + public static Coordinates IntakeZoneBack11Blue = new Coordinates(5, -20, 1.5); + public static Coordinates ParkBackBlue = new Coordinates(30, -50, 0); public static Pose coordinates(Coordinates coord) { return new Pose(coord.x, coord.y, coord.heading); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java index 35fd81f..d3ef9e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java @@ -54,15 +54,12 @@ public class MainDrivingOp extends OpMode { boolean rumbled = false; -// Pose idk67; @Override public void init() { lpsCounter = new LpsCounter(); lpsCounter.getLoopTime(); -// idk67 = new Pose(68, 68, 0); robot.initFollower(hardwareMap, true); -// robot.follower.setStartingPose(idk67); robot.init(hardwareMap); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java index 77e64d1..aaef1b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java @@ -52,8 +52,8 @@ public class Constants { public static double ANGLE_SERVO_FAR = 0.72; public static double ANGLE_SERVO_MIN = 0; - public static double FLAP_CLOSED = 0.29; - public static double FLAP_OPEN = 0.59; + public static double FLAP_CLOSED = 0.55; + public static double FLAP_OPEN = 1; public static double INTAKE_DRIVER_POWER = 0.55; public static double INTAKE_DRIVER_REVERSE = -0.55; @@ -61,23 +61,23 @@ public class Constants { public static double minVelocity = 1140; public static double maxVelocity = 1500; - public static double RANGE_1 = 40; + public static double RANGE_1 = 20; public static double RANGE_1_ANGLE = 0; public static double RANGE_1_VELOCITY = 1000; public static double RANGE_1_KS = 0.15; - public static double RANGE_2 = 20; + public static double RANGE_2 = 40; public static double RANGE_2_ANGLE = 0.32; public static double RANGE_2_VELOCITY = 1150; public static double RANGE_2_KS = 0.2; - public static double RANGE_3 = 70; + public static double RANGE_3 = 90; public static double RANGE_3_ANGLE = 0.64; public static double RANGE_3_VELOCITY = 1350; public static double RANGE_3_KS = 0.3; - public static double RANGE_4 = 140; + public static double RANGE_4 = 120; public static double RANGE_4_ANGLE = 0.70; public static double RANGE_4_VELOCITY = 1600; public static double RANGE_4_KS = 0.5; @@ -109,9 +109,9 @@ public class Constants { public static double DELTA_THRESHOLD = 0.01; public static double MAX_ROTATION_POWER = 0.5; - public static double BASKET_Y = 140; - public static double BASKET_BLUE_Y = 20; - public static double BASKET_X = 140; + public static double BASKET_Y = -140; + public static double BASKET_BLUE_Y = -20; + public static double BASKET_X = 130; public static AutonomousConstants.Coordinates RedTowerCoords = new AutonomousConstants.Coordinates(130, 130, 0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index fd68f48..50c994d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -54,8 +54,8 @@ public class Constants { .rightFrontMotorDirection(DcMotorSimple.Direction.REVERSE) .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD) .useBrakeModeInTeleOp(true) - .xVelocity(93.675629) - .yVelocity(77.158601); + .xVelocity(293.675629) + .yVelocity(277.158601); // public static TwoWheelConstants localizerConstants = new TwoWheelConstants() @@ -85,7 +85,7 @@ public class Constants { .hardwareMapName("pinpoint") .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD) - .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED) + .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD) ;