diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 8f63e2c..d6520b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -3,25 +3,13 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; -import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; -import androidx.annotation.NonNull; - import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.ParallelAction; import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; @@ -40,17 +28,15 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Targeting; import org.firstinspires.ftc.teamcode.utils.Turret; -import java.util.Objects; - @Config @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Close extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset; public static double autoSpinStartPos = 0.2; - public static double shoot0SpinSpeedIncrease = 0.02; + public static double shoot0SpinSpeedIncrease = 0.005; - public static double spindexerSpeedIncrease = 0.03; - public static double finalSpindexerSpeedIncrease = 0.03; + public static double spindexerSpeedIncrease = 0.005; + public static double finalSpindexerSpeedIncrease = 0.005; // These values are ADDED to turrDefault public static double redObeliskTurrPos1 = 0.12; @@ -111,8 +97,6 @@ public class Auto_LT_Close extends LinearOpMode { Targeting targeting; Targeting.Settings targetingSettings; AutoActions autoActions; - private double firstSpindexShootPos = autoSpinStartPos; - private boolean shootForward = true; double x1, y1, h1; double x2a, y2a, h2a, t2a; @@ -132,11 +116,6 @@ public class Auto_LT_Close extends LinearOpMode { private double shoot1Tangent; - private int driverSlotGreen = 0; - private int passengerSlotGreen = 0; - - private int rearSlotGreen = 0; - private int mostGreenSlot = 0; int ballCycles = 3; int prevMotif = 0; @@ -170,8 +149,6 @@ public class Auto_LT_Close extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.setTurret(turrDefault); - drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index c6eebf2..d521854 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -31,6 +31,7 @@ import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.autonomous.actions.AutoActions; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; @@ -64,6 +65,7 @@ public class Auto_LT_Far extends LinearOpMode { Turret turret; Targeting targeting; Targeting.Settings targetingSettings; + AutoActions autoActions; double firstSpindexShootPos = autoSpinStartPos; boolean shootForward = true; double xShoot, yShoot, hShoot; @@ -83,588 +85,7 @@ public class Auto_LT_Far extends LinearOpMode { public static int pickupStackSpeed = 15; int prevMotif = 0; - public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { - return new Action() { - double stamp = 0.0; - int ticker = 0; - double spindexerWiggle = 0.01; - - boolean decideGreenSlot = false; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - ticker++; - servos.setTransferPos(transferServo_out); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - double robX = drive.localizer.getPose().position.x; - double robY = drive.localizer.getPose().position.y; - double robotHeading = drive.localizer.getPose().heading.toDouble(); - - double goalX = -15; - double goalY = 0; - - double dx = robX - goalX; // delta x from robot to goal - double dy = robY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); - - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - - targetingSettings = targeting.calculateSettings - (robX, robY, robotHeading, 0.0, turretInterpolate); - - turret.trackGoal(deltaPose); - - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.addData("motif", motif_id); - TELE.update(); - - if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) { - - spindexerWiggle *= -1.0; - - servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle); - - spindexer.detectBalls(true, true); - - if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) { - driverSlotGreen++; - } - - if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) { - passengerSlotGreen++; - } - - if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) { - rearSlotGreen++; - } - - spindexer.setIntakePower(1); - - decideGreenSlot = true; - - return true; - } else if (decideGreenSlot) { - - if (driverSlotGreen >= passengerSlotGreen && driverSlotGreen >= rearSlotGreen) { - mostGreenSlot = 3; - } else if (passengerSlotGreen >= driverSlotGreen && passengerSlotGreen >= rearSlotGreen) { - mostGreenSlot = 2; - } else { - mostGreenSlot = 1; - } - - decideGreenSlot = false; - - if (motif_id == 21) { - if (mostGreenSlot == 1) { - firstSpindexShootPos = spindexer_outtakeBall1; - shootForward = true; - } else if (mostGreenSlot == 2) { - firstSpindexShootPos = spindexer_outtakeBall2; - shootForward = false; - } else { - firstSpindexShootPos = spindexer_outtakeBall3; - shootForward = false; - } - } else if (motif_id == 22) { - if (mostGreenSlot == 1) { - firstSpindexShootPos = spindexer_outtakeBall2; - shootForward = false; - } else if (mostGreenSlot == 2) { - firstSpindexShootPos = spindexer_outtakeBall3; - shootForward = false; - } else { - firstSpindexShootPos = spindexer_outtakeBall2; - shootForward = true; - } - - } else { - if (mostGreenSlot == 1) { - firstSpindexShootPos = spindexer_outtakeBall3; - shootForward = false; - } else if (mostGreenSlot == 2) { - firstSpindexShootPos = spindexer_outtakeBall3b; - shootForward = true; - } else { - firstSpindexShootPos = spindexer_outtakeBall1; - shootForward = true; - } - - } - - return true; - } else if ((System.currentTimeMillis() - stamp) < (time * 1000)) { -// TELE.addData("MostGreenSlot", mostGreenSlot); -// TELE.update(); - spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); - - servos.setSpinPos(firstSpindexShootPos); - - return true; - } else { - return false; - } - - } - }; - } - - public Action shootAll(int vel, double shootTime, double spindexSpeed) { - return new Action() { - int ticker = 1; - double stamp = 0.0; - double velo = vel; - int shooterTicker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); - flywheel.manageFlywheel(vel); - velo = flywheel.getVelo(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - spindexer.setIntakePower(-0.3); - - if (ticker == 1) { - stamp = getRuntime(); - } - ticker++; - - spindexer.setIntakePower(0); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - double robX = drive.localizer.getPose().position.x; - double robY = drive.localizer.getPose().position.y; - double robotHeading = drive.localizer.getPose().heading.toDouble(); - - double goalX = -15; - double goalY = 0; - - double dx = robX - goalX; // delta x from robot to goal - double dy = robY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); - - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - - targetingSettings = targeting.calculateSettings - (robX, robY, robotHeading, 0.0, turretInterpolate); - - turret.trackGoal(deltaPose); - - if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { - - if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - servos.setSpinPos(autoSpinStartPos); - } else { - servos.setTransferPos(transferServo_in); - shooterTicker++; - double prevSpinPos = servos.getSpinCmdPos(); - servos.setSpinPos(prevSpinPos + spindexSpeed); - } - - return true; - - } else { - servos.setTransferPos(transferServo_out); - - spindexer.resetSpindexer(); - spindexer.processIntake(); - - return false; - - } - - } - }; - } - - public Action shootAllAuto(double shootTime, double spindexSpeed) { - return new Action() { - int ticker = 1; - - double stamp = 0.0; - - double velo = 0.0; - - int shooterTicker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - velo = flywheel.getVelo(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - spindexer.setIntakePower(-0.3); - - if (ticker == 1) { - stamp = getRuntime(); - } - ticker++; - - spindexer.setIntakePower(0); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - double robX = drive.localizer.getPose().position.x; - double robY = drive.localizer.getPose().position.y; - double robotHeading = drive.localizer.getPose().heading.toDouble(); - - double goalX = -15; - double goalY = 0; - - double dx = robX - goalX; // delta x from robot to goal - double dy = robY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); - - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - - targetingSettings = targeting.calculateSettings - (robX, robY, robotHeading, 0.0, turretInterpolate); - - turret.trackGoal(deltaPose); - - if (getRuntime() - stamp < shootTime) { - - if (getRuntime() - stamp < firstShootTime) { - servos.setTransferPos(transferServo_out); - servos.setSpinPos(firstSpindexShootPos); - } else { - servos.setTransferPos(transferServo_in); - shooterTicker++; - double prevSpinPos = servos.getSpinCmdPos(); - - if (shootForward) { - servos.setSpinPos(prevSpinPos + spindexSpeed); - } else { - servos.setSpinPos(prevSpinPos - spindexSpeed); - } - } - - return true; - - } else { - servos.setTransferPos(transferServo_out); - - spindexer.resetSpindexer(); - spindexer.processIntake(); - - return false; - - } - } - }; - } - - public Action intake(double intakeTime) { - return new Action() { - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - ticker++; - - spindexer.processIntake(); - spindexer.setIntakePower(1); - - spindexer.ballCounterLight(); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addData("Full?", spindexer.isFull()); - TELE.update(); - - return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); - } - }; - } - - public Action detectObelisk( - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance, - double turrPos - ) { - - boolean timeFallback = (time != 0.501); - boolean posXFallback = (posX != 0.501); - boolean posYFallback = (posY != 0.501); - - return new Action() { - - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); - - if (ticker == 0) { - stamp = System.currentTimeMillis(); - robot.limelight.pipelineSwitch(1); - } - - ticker++; - motif = turret.detectObelisk(); - - turret.setTurret(turrPos); - - boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; - boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; - - boolean shouldFinish = timeDone || xDone || yDone; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - if (shouldFinish){ - if (redAlliance){ - robot.limelight.pipelineSwitch(4); - } else { - robot.limelight.pipelineSwitch(2); - } - return false; - } else { - return true; - } - - } - }; - } - - public Action manageFlywheel( - double vel, - double hoodPos, - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance - ) { - - boolean timeFallback = (time != 0.501); - boolean posXFallback = (posX != 0.501); - boolean posYFallback = (posY != 0.501); - - return new Action() { - - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); - - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - - ticker++; - - double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); - flywheel.manageFlywheel(vel); - servos.setHoodPos(hoodPos); - - boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; - boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; - - boolean shouldFinish = timeDone || xDone || yDone; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return !shouldFinish; - - } - }; - } - - public Action manageShooterAuto( - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance - ) { - - boolean timeFallback = (time != 0.501); - boolean posXFallback = (posX != 0.501); - boolean posYFallback = (posY != 0.501); - - return new Action() { - - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); - - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - - ticker++; - - double robotX = drive.localizer.getPose().position.x; - double robotY = drive.localizer.getPose().position.y; - - double robotHeading = drive.localizer.getPose().heading.toDouble(); - - double goalX = -15; - double goalY = 0; - - double dx = robotX - goalX; // delta x from robot to goal - double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); - - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - - targetingSettings = targeting.calculateSettings - (robotX, robotY, robotHeading, 0.0, false); - - turret.trackGoal(deltaPose); - - servos.setHoodPos(targetingSettings.hoodAngle); - - double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); - flywheel.manageFlywheel(targetingSettings.flywheelRPM); - - boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; - boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; - - boolean shouldFinish = timeDone || xDone || yDone; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return !shouldFinish; - - } - }; - } - - public Action manageFlywheelAuto( - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance - ) { - - boolean timeFallback = (time != 0.501); - boolean posXFallback = (posX != 0.501); - boolean posYFallback = (posY != 0.501); - - return new Action() { - - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); - - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - - ticker++; - - double robotX = drive.localizer.getPose().position.x; - double robotY = drive.localizer.getPose().position.y; - - double robotHeading = drive.localizer.getPose().heading.toDouble(); - - double goalX = -15; - double goalY = 0; - - double dx = robotX - goalX; // delta x from robot to goal - double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); - - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - - targetingSettings = targeting.calculateSettings - (robotX, robotY, robotHeading, 0.0, false); - - servos.setHoodPos(targetingSettings.hoodAngle); - - double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); - flywheel.manageFlywheel(targetingSettings.flywheelRPM); - - boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; - boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; - - boolean shouldFinish = timeDone || xDone || yDone; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return !shouldFinish; - - } - }; - } // initialize path variables here TrajectoryActionBuilder leave3Ball = null; @@ -700,6 +121,8 @@ public class Auto_LT_Far extends LinearOpMode { drive = new MecanumDrive(hardwareMap, autoStart); + autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret); + servos.setSpinPos(autoSpinStartPos); servos.setTransferPos(transferServo_out); @@ -850,7 +273,7 @@ public class Auto_LT_Far extends LinearOpMode { void startAuto(){ Actions.runBlocking( new ParallelAction( - manageFlywheel( + autoActions.manageFlywheel( shoot0Vel, shoot0Hood, flywheel0Time, @@ -864,7 +287,7 @@ public class Auto_LT_Far extends LinearOpMode { ); Actions.runBlocking( - shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) + autoActions.shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) ); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index f03e6ce..57611a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -15,6 +15,7 @@ import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import androidx.annotation.NonNull; +import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; @@ -30,6 +31,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret; import java.util.Objects; +@Config public class AutoActions{ Robot robot; MultipleTelemetry TELE; @@ -246,7 +248,7 @@ public class AutoActions{ turret.trackGoal(deltaPose); - if ((System.currentTimeMillis() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { + if ((System.currentTimeMillis() - stamp < shootTime*1000 && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) { servos.setSpinPos(spinStartPos); @@ -325,9 +327,9 @@ public class AutoActions{ turret.trackGoal(deltaPose); - if (System.currentTimeMillis() - stamp < shootTime) { + if (System.currentTimeMillis() - stamp < shootTime*1000) { - if (System.currentTimeMillis() - stamp < firstShootTime) { + if (System.currentTimeMillis() - stamp < firstShootTime*1000) { servos.setTransferPos(transferServo_out); servos.setSpinPos(firstSpindexShootPos); } else { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index b1f0d8b..61ac3d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -81,12 +81,16 @@ public class Turret { return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault; } - private double prevTurrPos = 0.501; + private double prevTurrPos = 0; + private boolean isFirstTurretPos = true; public void setTurret(double pos) { - if (prevTurrPos != 0.501 && prevTurrPos != pos){ + if (isFirstTurretPos || prevTurrPos != pos){ robot.turr1.setPosition(pos); robot.turr2.setPosition(1-pos); + isFirstTurretPos = false; } + TELE.addLine("Moved Turret"); + TELE.update(); prevTurrPos = pos; }