Silghtly cleaned code after LT...no lgic changes
This commit is contained in:
@@ -0,0 +1,403 @@
|
||||
package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
|
||||
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.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.ftc.Actions;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.utils.Flywheel;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
import org.firstinspires.ftc.teamcode.utils.Servos;
|
||||
import org.firstinspires.ftc.teamcode.utils.Spindexer;
|
||||
import org.firstinspires.ftc.teamcode.utils.Targeting;
|
||||
import org.firstinspires.ftc.teamcode.utils.Turret;
|
||||
|
||||
@Config
|
||||
@Autonomous(preselectTeleOp = "TeleopV3")
|
||||
public class Auto_LT_Far_3Ball extends LinearOpMode {
|
||||
public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset;
|
||||
public static double autoSpinStartPos = 0.2;
|
||||
public static double shoot0SpinSpeedIncrease = 0.015;
|
||||
|
||||
public static double redObeliskTurrPos1 = turrDefault + 0.12;
|
||||
public static double redObeliskTurrPos2 = turrDefault + 0.13;
|
||||
public static double redObeliskTurrPos3 = turrDefault + 0.14;
|
||||
|
||||
public static double blueObeliskTurrPos1 = turrDefault - 0.12;
|
||||
public static double blueObeliskTurrPos2 = turrDefault - 0.13;
|
||||
public static double blueObeliskTurrPos3 = turrDefault - 0.14;
|
||||
|
||||
public static double shoot0XTolerance = 1.0;
|
||||
public static double redTurretShootPos = turrDefault;
|
||||
public static double blueTurretShootPos = turrDefault;
|
||||
public static int fwdTime = 200, strafeTime = 2300;
|
||||
public static double rPickupGateX = 1, rPickupGateY = 1, rPickupGateH = 1;
|
||||
public static double rPickupZoneX = 1, rPickupZoneY = 1, rPickupZoneH = 1;
|
||||
public static double rShootX = 1, rShootY = 1, rShootH = 1;
|
||||
public static double bPickupZoneX = 1, bPickupZoneY = 1, bPickupZoneH = 1;
|
||||
public static double bPickupGateX = 1, bPickupGateY = 1, bPickupGateH = 1;
|
||||
public static double bShootX = 1, bShootY = 1, bShootH = 1;
|
||||
public static int sleepTime = 1300;
|
||||
public int motif = 0;
|
||||
double turretShootPos = 0.0;
|
||||
Robot robot;
|
||||
MultipleTelemetry TELE;
|
||||
MecanumDrive drive;
|
||||
Servos servos;
|
||||
Spindexer spindexer;
|
||||
Flywheel flywheel;
|
||||
Turret turret;
|
||||
Targeting targeting;
|
||||
Targeting.Settings targetingSettings;
|
||||
private double firstSpindexShootPos = autoSpinStartPos;
|
||||
private boolean shootForward = true;
|
||||
private double xShoot, yShoot, hShoot;
|
||||
private int driverSlotGreen = 0;
|
||||
private int passengerSlotGreen = 0;
|
||||
private int rearSlotGreen = 0;
|
||||
private int mostGreenSlot = 0;
|
||||
private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
|
||||
private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
|
||||
|
||||
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();
|
||||
|
||||
flywheel.manageFlywheel(vel);
|
||||
velo = flywheel.getVelo();
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
robot.intake.setPower(-0.3);
|
||||
|
||||
if (ticker == 1) {
|
||||
stamp = getRuntime();
|
||||
}
|
||||
ticker++;
|
||||
|
||||
robot.intake.setPower(0);
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
if (getRuntime() - stamp < shootTime) {
|
||||
|
||||
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
|
||||
robot.spin1.setPosition(autoSpinStartPos);
|
||||
robot.spin2.setPosition(1 - autoSpinStartPos);
|
||||
} else {
|
||||
robot.transferServo.setPosition(transferServo_in);
|
||||
shooterTicker++;
|
||||
double prevSpinPos = robot.spin1.getPosition();
|
||||
robot.spin1.setPosition(prevSpinPos + spindexSpeed);
|
||||
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
} else {
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
//spindexPos = spindexer_intakePos1;
|
||||
|
||||
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();
|
||||
robot.intake.setPower(1);
|
||||
|
||||
spindexer.ballCounterLight();
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
TELE.addData("Velocity", flywheel.getVelo());
|
||||
TELE.addData("Hood", robot.hood.getPosition());
|
||||
TELE.update();
|
||||
|
||||
return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
|
||||
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
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++;
|
||||
|
||||
flywheel.manageFlywheel(vel);
|
||||
robot.hood.setPosition(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;
|
||||
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
|
||||
TELE = new MultipleTelemetry(
|
||||
telemetry, FtcDashboard.getInstance().getTelemetry()
|
||||
);
|
||||
|
||||
flywheel = new Flywheel(hardwareMap);
|
||||
|
||||
targeting = new Targeting();
|
||||
targetingSettings = new Targeting.Settings(0.0, 0.0);
|
||||
|
||||
spindexer = new Spindexer(hardwareMap);
|
||||
|
||||
servos = new Servos(hardwareMap);
|
||||
|
||||
robot.limelight.start();
|
||||
|
||||
robot.limelight.pipelineSwitch(1);
|
||||
|
||||
turret = new Turret(robot, TELE, robot.limelight);
|
||||
|
||||
turret.manualSetTurret(turrDefault);
|
||||
|
||||
drive = new MecanumDrive(hardwareMap, teleEnd);
|
||||
|
||||
robot.spin1.setPosition(autoSpinStartPos);
|
||||
robot.spin2.setPosition(1 - autoSpinStartPos);
|
||||
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
|
||||
robot.light.setPosition(1);
|
||||
|
||||
while (opModeInInit()) {
|
||||
|
||||
robot.hood.setPosition(shoot0Hood);
|
||||
turret.manualSetTurret(turretShootPos);
|
||||
|
||||
if (gamepad2.crossWasPressed()) {
|
||||
redAlliance = !redAlliance;
|
||||
}
|
||||
|
||||
if (gamepad2.dpadLeftWasPressed()) {
|
||||
turrDefault -= 0.01;
|
||||
}
|
||||
|
||||
if (gamepad2.dpadRightWasPressed()) {
|
||||
turrDefault += 0.01;
|
||||
}
|
||||
|
||||
redObeliskTurrPos1 = turrDefault + 0.12;
|
||||
redObeliskTurrPos2 = turrDefault + 0.13;
|
||||
redObeliskTurrPos3 = turrDefault + 0.14;
|
||||
|
||||
blueObeliskTurrPos1 = turrDefault - 0.12;
|
||||
blueObeliskTurrPos2 = turrDefault - 0.13;
|
||||
blueObeliskTurrPos3 = turrDefault - 0.14;
|
||||
|
||||
redTurretShootPos = turrDefault + 0.05;
|
||||
|
||||
if (redAlliance) {
|
||||
robot.light.setPosition(0.28);
|
||||
|
||||
pickupGateX = rPickupGateX;
|
||||
pickupGateY = rPickupGateY;
|
||||
pickupGateH = rPickupGateH;
|
||||
|
||||
pickupZoneX = rPickupZoneX;
|
||||
pickupZoneY = rPickupZoneY;
|
||||
pickupZoneH = rPickupZoneH;
|
||||
|
||||
xShoot = rShootX;
|
||||
yShoot = rShootY;
|
||||
hShoot = rShootH;
|
||||
|
||||
turretShootPos = redTurretShootPos;
|
||||
|
||||
} else {
|
||||
robot.light.setPosition(0.6);
|
||||
|
||||
pickupGateX = bPickupGateX;
|
||||
pickupGateY = bPickupGateY;
|
||||
pickupGateH = bPickupGateH;
|
||||
|
||||
pickupZoneX = bPickupZoneX;
|
||||
pickupZoneY = bPickupZoneY;
|
||||
pickupZoneH = bPickupZoneH;
|
||||
|
||||
xShoot = bShootX;
|
||||
yShoot = bShootY;
|
||||
hShoot = bShootH;
|
||||
|
||||
turretShootPos = blueTurretShootPos;
|
||||
|
||||
}
|
||||
|
||||
TELE.addData("Red?", redAlliance);
|
||||
TELE.addData("Turret Default", turrDefault);
|
||||
|
||||
TELE.update();
|
||||
}
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
if (opModeIsActive()) {
|
||||
|
||||
robot.transfer.setPower(1);
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
manageFlywheel(
|
||||
shoot0Vel,
|
||||
shoot0Hood,
|
||||
9,
|
||||
0.501,
|
||||
0.501,
|
||||
shoot0XTolerance,
|
||||
0.501
|
||||
)
|
||||
|
||||
)
|
||||
);
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
Actions.runBlocking(
|
||||
shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease)
|
||||
);
|
||||
|
||||
robot.frontLeft.setPower(-0.4);
|
||||
robot.frontRight.setPower(-0.4);
|
||||
robot.backLeft.setPower(-0.4);
|
||||
robot.backRight.setPower(-0.4);
|
||||
|
||||
sleep(fwdTime);
|
||||
|
||||
robot.frontLeft.setPower(0);
|
||||
robot.frontRight.setPower(0);
|
||||
robot.backLeft.setPower(0);
|
||||
robot.backRight.setPower(0);
|
||||
|
||||
sleep(sleepTime);
|
||||
|
||||
robot.frontLeft.setPower(-0.4);
|
||||
robot.frontRight.setPower(0.4);
|
||||
robot.backLeft.setPower(0.4);
|
||||
robot.backRight.setPower(-0.4);
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
sleep(strafeTime);
|
||||
|
||||
robot.frontLeft.setPower(0);
|
||||
robot.frontRight.setPower(0);
|
||||
robot.backLeft.setPower(0);
|
||||
robot.backRight.setPower(0);
|
||||
|
||||
while (opModeIsActive()) {
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
TELE.addLine("finished");
|
||||
TELE.update();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,881 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
|
||||
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.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;
|
||||
import com.acmerobotics.roadrunner.TranslationalVelConstraint;
|
||||
import com.acmerobotics.roadrunner.Vector2d;
|
||||
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.libs.RR.MecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.utils.Flywheel;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
import org.firstinspires.ftc.teamcode.utils.Servos;
|
||||
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_Far_Indexed extends LinearOpMode {
|
||||
public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset;
|
||||
public static double autoSpinStartPos = 0.2;
|
||||
public static double shoot0SpinSpeedIncrease = 0.015;
|
||||
|
||||
public static double spindexerSpeedIncrease = 0.03;
|
||||
public static double finalSpindexerSpeedIncrease = 0.025;
|
||||
|
||||
|
||||
public static double redObeliskTurrPos1 = turrDefault + 0.12;
|
||||
public static double redObeliskTurrPos2 = turrDefault + 0.13;
|
||||
public static double redObeliskTurrPos3 = turrDefault + 0.14;
|
||||
|
||||
public static double blueObeliskTurrPos1 = turrDefault - 0.12;
|
||||
public static double blueObeliskTurrPos2 = turrDefault - 0.13;
|
||||
public static double blueObeliskTurrPos3 = turrDefault - 0.14;
|
||||
double obeliskTurrPos1 = 0.0;
|
||||
double obeliskTurrPos2 = 0.0;
|
||||
double obeliskTurrPos3 = 0.0;
|
||||
public static double normalIntakeTime = 3.3;
|
||||
public static double shoot1Turr = 0.57;
|
||||
public static double shoot0XTolerance = 1.0;
|
||||
public static double redTurretShootPos = turrDefault ;
|
||||
public static double blueTurretShootPos = turrDefault ;
|
||||
double turretShootPos = 0.0;
|
||||
|
||||
public static double finalShootAllTime = 3.0;
|
||||
public static double shootAllTime = 1.8;
|
||||
public static double shoot0Time = 1.6;
|
||||
public static double intake1Time = 3.3;
|
||||
public static double intake2Time = 3.8;
|
||||
|
||||
public static double intake3Time = 4.2;
|
||||
|
||||
public static double flywheel0Time = 3.5;
|
||||
public static double pickup1Speed = 23;
|
||||
// ---- SECOND SHOT / PICKUP ----
|
||||
public static double shoot1Vel = 2300;
|
||||
public static double shootAllVelocity = 2500;
|
||||
public static double shootAllHood = 0.78 + hoodOffset;
|
||||
// ---- PICKUP POSITION TOLERANCES ----
|
||||
public static double pickup1XTolerance = 2.0;
|
||||
public static double pickup1YTolerance = 2.0;
|
||||
// ---- OBELISK DETECTION ----
|
||||
public static double obelisk1Time = 1.5;
|
||||
public static double obelisk1XTolerance = 2.0;
|
||||
public static double obelisk1YTolerance = 2.0;
|
||||
public static double shoot1ToleranceX = 2.0;
|
||||
public static double shoot1ToleranceY = 2.0;
|
||||
public static double shoot1Time = 2;
|
||||
public static double shoot2Time = 2;
|
||||
public static double shoot3Time = 2;
|
||||
public static double colorSenseTime = 1.2;
|
||||
|
||||
public static double firstShootTime = 0.3;
|
||||
|
||||
public static int fwdTime = 200, strafeTime = 2300;
|
||||
public int motif = 0;
|
||||
|
||||
Robot robot;
|
||||
MultipleTelemetry TELE;
|
||||
MecanumDrive drive;
|
||||
Servos servos;
|
||||
Spindexer spindexer;
|
||||
Flywheel flywheel;
|
||||
Turret turret;
|
||||
Targeting targeting;
|
||||
Targeting.Settings targetingSettings;
|
||||
private double firstSpindexShootPos = autoSpinStartPos;
|
||||
private boolean shootForward = true;
|
||||
private double x1, y1, h1;
|
||||
|
||||
private double x2a, y2a, h2a, t2a;
|
||||
|
||||
private double x2b, y2b, h2b, t2b;
|
||||
private double x2c, y2c, h2c, t2c;
|
||||
|
||||
private double x3a, y3a, h3a;
|
||||
private double x3b, y3b, h3b;
|
||||
private double x4a, y4a, h4a;
|
||||
private double x4b, y4b, h4b;
|
||||
|
||||
private double xShoot, yShoot, hShoot;
|
||||
private double xGate, yGate, hGate;
|
||||
private double xPrep, yPrep, hPrep;
|
||||
|
||||
private double shoot1Tangent;
|
||||
|
||||
private int driverSlotGreen = 0;
|
||||
private int passengerSlotGreen = 0;
|
||||
|
||||
private int rearSlotGreen = 0;
|
||||
private int mostGreenSlot = 0;
|
||||
|
||||
private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
|
||||
private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
|
||||
public static double rPickupGateX = 1, rPickupGateY = 1, rPickupGateH = 1;
|
||||
public static double rPickupZoneX = 1, rPickupZoneY = 1, rPickupZoneH = 1;
|
||||
public static double rShootX = 1, rShootY = 1, rShootH = 1;
|
||||
public static double bPickupZoneX = 1, bPickupZoneY = 1, bPickupZoneH = 1;
|
||||
public static double bPickupGateX = 1, bPickupGateY = 1, bPickupGateH = 1;
|
||||
public static double bShootX = 1, bShootY = 1, bShootH = 1;
|
||||
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++;
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
|
||||
turret.manualSetTurret(turretShootPos);
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
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;
|
||||
|
||||
robot.spin1.setPosition(spindexer_intakePos1 + spindexerWiggle);
|
||||
robot.spin2.setPosition(1 - 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++;
|
||||
}
|
||||
|
||||
robot.intake.setPower(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);
|
||||
|
||||
|
||||
robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
|
||||
|
||||
robot.spin1.setPosition(firstSpindexShootPos);
|
||||
robot.spin2.setPosition(1 - firstSpindexShootPos);
|
||||
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
public static int sleepTime = 1300;
|
||||
|
||||
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();
|
||||
|
||||
flywheel.manageFlywheel(vel);
|
||||
velo = flywheel.getVelo();
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
robot.intake.setPower(-0.3);
|
||||
|
||||
if (ticker == 1) {
|
||||
stamp = getRuntime();
|
||||
}
|
||||
ticker++;
|
||||
|
||||
robot.intake.setPower(0);
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
if (getRuntime() - stamp < shootTime) {
|
||||
|
||||
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
|
||||
robot.spin1.setPosition(autoSpinStartPos);
|
||||
robot.spin2.setPosition(1 - autoSpinStartPos);
|
||||
} else {
|
||||
robot.transferServo.setPosition(transferServo_in);
|
||||
shooterTicker++;
|
||||
double prevSpinPos = robot.spin1.getPosition();
|
||||
robot.spin1.setPosition(prevSpinPos + spindexSpeed);
|
||||
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
} else {
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
//spindexPos = spindexer_intakePos1;
|
||||
|
||||
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();
|
||||
|
||||
robot.intake.setPower(-0.3);
|
||||
|
||||
if (ticker == 1) {
|
||||
stamp = getRuntime();
|
||||
}
|
||||
ticker++;
|
||||
|
||||
robot.intake.setPower(0);
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
if (getRuntime() - stamp < shootTime) {
|
||||
|
||||
if (getRuntime() - stamp < firstShootTime) {
|
||||
robot.transferServo.setPosition(transferServo_in);
|
||||
robot.spin1.setPosition(firstSpindexShootPos);
|
||||
robot.spin2.setPosition(1 - firstSpindexShootPos);
|
||||
} else {
|
||||
robot.transferServo.setPosition(transferServo_in);
|
||||
shooterTicker++;
|
||||
double prevSpinPos = robot.spin1.getPosition();
|
||||
|
||||
if (shootForward) {
|
||||
robot.spin1.setPosition(prevSpinPos + spindexSpeed);
|
||||
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
|
||||
} else {
|
||||
robot.spin1.setPosition(prevSpinPos - spindexSpeed);
|
||||
robot.spin2.setPosition(1 - prevSpinPos + spindexSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
} else {
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
//spindexPos = spindexer_intakePos1;
|
||||
|
||||
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();
|
||||
robot.intake.setPower(1);
|
||||
|
||||
spindexer.ballCounterLight();
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
TELE.addData("Velocity", flywheel.getVelo());
|
||||
TELE.addData("Hood", robot.hood.getPosition());
|
||||
TELE.update();
|
||||
|
||||
return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
|
||||
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
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();
|
||||
|
||||
robot.turr1.setPosition(turrPos);
|
||||
robot.turr2.setPosition(1 - 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++;
|
||||
|
||||
flywheel.manageFlywheel(vel);
|
||||
robot.hood.setPosition(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);
|
||||
|
||||
robot.hood.setPosition(targetingSettings.hoodAngle);
|
||||
|
||||
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);
|
||||
|
||||
robot.hood.setPosition(targetingSettings.hoodAngle);
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
|
||||
TELE = new MultipleTelemetry(
|
||||
telemetry, FtcDashboard.getInstance().getTelemetry()
|
||||
);
|
||||
|
||||
flywheel = new Flywheel(hardwareMap);
|
||||
|
||||
targeting = new Targeting();
|
||||
targetingSettings = new Targeting.Settings(0.0, 0.0);
|
||||
|
||||
spindexer = new Spindexer(hardwareMap);
|
||||
|
||||
servos = new Servos(hardwareMap);
|
||||
|
||||
robot.limelight.start();
|
||||
|
||||
robot.limelight.pipelineSwitch(1);
|
||||
|
||||
|
||||
turret = new Turret(robot, TELE, robot.limelight);
|
||||
|
||||
turret.manualSetTurret(turrDefault);
|
||||
|
||||
drive = new MecanumDrive(hardwareMap, teleEnd);
|
||||
|
||||
robot.spin1.setPosition(autoSpinStartPos);
|
||||
robot.spin2.setPosition(1 - autoSpinStartPos);
|
||||
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
|
||||
TrajectoryActionBuilder shoot0 = null;
|
||||
TrajectoryActionBuilder pickupGate = null;
|
||||
TrajectoryActionBuilder pickupLoadingZone = null;
|
||||
TrajectoryActionBuilder shootFromGate = null;
|
||||
TrajectoryActionBuilder shootFromZone = null;
|
||||
|
||||
robot.light.setPosition(1);
|
||||
|
||||
while (opModeInInit()) {
|
||||
|
||||
robot.hood.setPosition(shoot0Hood);
|
||||
turret.manualSetTurret(turretShootPos);
|
||||
|
||||
if (gamepad2.crossWasPressed()) {
|
||||
redAlliance = !redAlliance;
|
||||
}
|
||||
|
||||
if (gamepad2.dpadLeftWasPressed()) {
|
||||
turrDefault -=0.01;
|
||||
}
|
||||
|
||||
if (gamepad2.dpadRightWasPressed()) {
|
||||
turrDefault +=0.01;
|
||||
}
|
||||
|
||||
redObeliskTurrPos1 = turrDefault + 0.12;
|
||||
redObeliskTurrPos2 = turrDefault + 0.13;
|
||||
redObeliskTurrPos3 = turrDefault + 0.14;
|
||||
|
||||
blueObeliskTurrPos1 = turrDefault - 0.12;
|
||||
blueObeliskTurrPos2 = turrDefault - 0.13;
|
||||
blueObeliskTurrPos3 = turrDefault - 0.14;
|
||||
|
||||
redTurretShootPos = turrDefault + 0.05;
|
||||
|
||||
|
||||
|
||||
if (redAlliance) {
|
||||
robot.light.setPosition(0.28);
|
||||
|
||||
pickupGateX = rPickupGateX;
|
||||
pickupGateY = rPickupGateY;
|
||||
pickupGateH = rPickupGateH;
|
||||
|
||||
pickupZoneX = rPickupZoneX;
|
||||
pickupZoneY = rPickupZoneY;
|
||||
pickupZoneH = rPickupZoneH;
|
||||
|
||||
xShoot = rShootX;
|
||||
yShoot = rShootY;
|
||||
hShoot = rShootH;
|
||||
|
||||
turretShootPos = redTurretShootPos;
|
||||
|
||||
} else {
|
||||
robot.light.setPosition(0.6);
|
||||
|
||||
pickupGateX = bPickupGateX;
|
||||
pickupGateY = bPickupGateY;
|
||||
pickupGateH = bPickupGateH;
|
||||
|
||||
pickupZoneX = bPickupZoneX;
|
||||
pickupZoneY = bPickupZoneY;
|
||||
pickupZoneH = bPickupZoneH;
|
||||
|
||||
xShoot = bShootX;
|
||||
yShoot = bShootY;
|
||||
hShoot = bShootH;
|
||||
|
||||
turretShootPos = blueTurretShootPos;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
TELE.addData("Red?", redAlliance);
|
||||
TELE.addData("Turret Default", turrDefault);
|
||||
|
||||
TELE.update();
|
||||
}
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
if (opModeIsActive()) {
|
||||
|
||||
robot.transfer.setPower(1);
|
||||
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
manageFlywheel(
|
||||
shoot0Vel,
|
||||
shoot0Hood,
|
||||
9,
|
||||
0.501,
|
||||
0.501,
|
||||
shoot0XTolerance,
|
||||
0.501
|
||||
)
|
||||
|
||||
)
|
||||
);
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
Actions.runBlocking(
|
||||
shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease)
|
||||
);
|
||||
|
||||
robot.frontLeft.setPower(-0.4);
|
||||
robot.frontRight.setPower(-0.4);
|
||||
robot.backLeft.setPower(-0.4);
|
||||
robot.backRight.setPower(-0.4);
|
||||
|
||||
sleep (fwdTime);
|
||||
|
||||
robot.frontLeft.setPower(0);
|
||||
robot.frontRight.setPower(0);
|
||||
robot.backLeft.setPower(0);
|
||||
robot.backRight.setPower(0);
|
||||
|
||||
sleep (sleepTime);
|
||||
|
||||
robot.frontLeft.setPower(-0.4);
|
||||
robot.frontRight.setPower(0.4);
|
||||
robot.backLeft.setPower(0.4);
|
||||
robot.backRight.setPower(-0.4);
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
sleep (strafeTime);
|
||||
|
||||
robot.frontLeft.setPower(0);
|
||||
robot.frontRight.setPower(0);
|
||||
robot.backLeft.setPower(0);
|
||||
robot.backRight.setPower(0);
|
||||
|
||||
while(opModeIsActive()) {
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
TELE.addLine("finished");
|
||||
TELE.update();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -6,19 +6,11 @@ import com.acmerobotics.roadrunner.Pose2d;
|
||||
@Config
|
||||
public class Poses {
|
||||
|
||||
public static double goalHeight = 42; //in inches
|
||||
|
||||
public static double turretHeight = 12;
|
||||
|
||||
public static double relativeGoalHeight = goalHeight - turretHeight;
|
||||
|
||||
public static Pose2d goalPose = new Pose2d(-10, 0, 0);
|
||||
|
||||
public static double rx1 = 20, ry1 = 0.5, rh1 = Math.toRadians(0.1);
|
||||
public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140);
|
||||
public static double rx2b = 19, ry2b = 40, rh2b = Math.toRadians(140.1);
|
||||
|
||||
public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140);
|
||||
|
||||
public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
|
||||
|
||||
@@ -32,7 +24,6 @@ public class Poses {
|
||||
public static double bx1 = 20, by1 = 0.5, bh1 = Math.toRadians(0.1);
|
||||
public static double bx2a = 41, by2a = -18, bh2a = Math.toRadians(-140);
|
||||
public static double bx2b = 19, by2b = -40, bh2b = Math.toRadians(-140.1);
|
||||
public static double bx2c = 34, by2c = -50, bh2c = Math.toRadians(-140);
|
||||
|
||||
public static double bx3a = 55, by3a = -39, bh3a = Math.toRadians(-140);
|
||||
public static double bx3b = 41, by3b = -59, bh3b = Math.toRadians(-140.1);
|
||||
|
||||
@@ -1,15 +1,10 @@
|
||||
package org.firstinspires.ftc.teamcode.constants;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.Pose2d;
|
||||
|
||||
@Config
|
||||
public class Poses_V2 {
|
||||
|
||||
public static double goalHeight = 42; //in inches
|
||||
|
||||
public static double turretHeight = 12;
|
||||
|
||||
public static double rXGateA = 35.5, rYGateA = 46.5, rHGateA = 2.7;
|
||||
|
||||
public static double rXGateB = 56, rYGateB = 37, rHGateB = 2.7750735106709836;
|
||||
@@ -21,5 +16,4 @@ public class Poses_V2 {
|
||||
|
||||
public static double bXGate = 28, bYGate = -65, bHGate = Math.toRadians(-179);
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -19,25 +19,17 @@ public class ServoPositions {
|
||||
public static double spinStartPos = 0.22;
|
||||
public static double spinEndPos = 0.85;
|
||||
|
||||
public static double shootAllAutoSpinStartPos = 0.2;
|
||||
public static double shootAllSpindexerSpeedIncrease = 0.014;
|
||||
public static double shootAllTime = 1.8;
|
||||
|
||||
public static double transferServo_out = 0.15;
|
||||
|
||||
public static double transferServo_in = 0.38;
|
||||
|
||||
public static double turret_range = 0.9;
|
||||
|
||||
public static double hoodDefault = 0.6;
|
||||
|
||||
public static double hoodAuto = 0.27;
|
||||
|
||||
public static double hoodOffset = -0.05;
|
||||
public static double hoodOffset = -0.05;
|
||||
|
||||
public static double turret_redClose = 0.42;
|
||||
public static double turret_blueClose = 0.38;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -4,23 +4,8 @@ import com.acmerobotics.dashboard.config.Config;
|
||||
|
||||
@Config
|
||||
public class ShooterVars {
|
||||
public static double turret_GearRatio = 0.9974;
|
||||
|
||||
public static double turret_Range = 355;
|
||||
|
||||
public static int velTolerance = 300;
|
||||
|
||||
public static int initTolerance = 1000;
|
||||
|
||||
public static int maxVel = 4500;
|
||||
public static double waitTransferOut = 0.3;
|
||||
public static double waitTransfer = 0.4;
|
||||
public static double kP = 0.001; // small proportional gain (tune this)
|
||||
public static double maxStep = 0.06; // prevents sudden jumps
|
||||
|
||||
// VELOCITY CONSTANTS
|
||||
public static int AUTO_CLOSE_VEL = 3175; //3300;
|
||||
public static int AUTO_FAR_VEL = 4000; //TODO: test this
|
||||
|
||||
public static Types.Motif currentMotif = Types.Motif.NONE;
|
||||
}
|
||||
@@ -1,15 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
@Config
|
||||
@TeleOp
|
||||
public class Test extends LinearOpMode {
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
}
|
||||
}
|
||||
@@ -9,6 +9,7 @@ import com.arcrobotics.ftclib.controller.PIDController;
|
||||
import com.qualcomm.hardware.limelightvision.LLResult;
|
||||
import com.qualcomm.hardware.limelightvision.LLResultTypes;
|
||||
import com.qualcomm.hardware.limelightvision.Limelight3A;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
|
||||
@@ -16,6 +17,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
|
||||
import java.util.List;
|
||||
|
||||
@Config
|
||||
@TeleOp
|
||||
public class Turret {
|
||||
|
||||
public static double turretTolerance = 0.02;
|
||||
|
||||
Reference in New Issue
Block a user