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
|
@Config
|
||||||
public class Poses {
|
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 rx1 = 20, ry1 = 0.5, rh1 = Math.toRadians(0.1);
|
||||||
public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140);
|
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 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);
|
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 bx1 = 20, by1 = 0.5, bh1 = Math.toRadians(0.1);
|
||||||
public static double bx2a = 41, by2a = -18, bh2a = Math.toRadians(-140);
|
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 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 bx3a = 55, by3a = -39, bh3a = Math.toRadians(-140);
|
||||||
public static double bx3b = 41, by3b = -59, bh3b = Math.toRadians(-140.1);
|
public static double bx3b = 41, by3b = -59, bh3b = Math.toRadians(-140.1);
|
||||||
|
|||||||
@@ -1,15 +1,10 @@
|
|||||||
package org.firstinspires.ftc.teamcode.constants;
|
package org.firstinspires.ftc.teamcode.constants;
|
||||||
|
|
||||||
import com.acmerobotics.dashboard.config.Config;
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
import com.acmerobotics.roadrunner.Pose2d;
|
|
||||||
|
|
||||||
@Config
|
@Config
|
||||||
public class Poses_V2 {
|
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 rXGateA = 35.5, rYGateA = 46.5, rHGateA = 2.7;
|
||||||
|
|
||||||
public static double rXGateB = 56, rYGateB = 37, rHGateB = 2.7750735106709836;
|
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);
|
public static double bXGate = 28, bYGate = -65, bHGate = Math.toRadians(-179);
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -19,18 +19,12 @@ public class ServoPositions {
|
|||||||
public static double spinStartPos = 0.22;
|
public static double spinStartPos = 0.22;
|
||||||
public static double spinEndPos = 0.85;
|
public static double spinEndPos = 0.85;
|
||||||
|
|
||||||
public static double shootAllAutoSpinStartPos = 0.2;
|
|
||||||
public static double shootAllSpindexerSpeedIncrease = 0.014;
|
public static double shootAllSpindexerSpeedIncrease = 0.014;
|
||||||
public static double shootAllTime = 1.8;
|
|
||||||
|
|
||||||
public static double transferServo_out = 0.15;
|
public static double transferServo_out = 0.15;
|
||||||
|
|
||||||
public static double transferServo_in = 0.38;
|
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 hoodAuto = 0.27;
|
||||||
|
|
||||||
public static double hoodOffset = -0.05;
|
public static double hoodOffset = -0.05;
|
||||||
@@ -38,6 +32,4 @@ public static double hoodOffset = -0.05;
|
|||||||
public static double turret_redClose = 0.42;
|
public static double turret_redClose = 0.42;
|
||||||
public static double turret_blueClose = 0.38;
|
public static double turret_blueClose = 0.38;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -4,23 +4,8 @@ import com.acmerobotics.dashboard.config.Config;
|
|||||||
|
|
||||||
@Config
|
@Config
|
||||||
public class ShooterVars {
|
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
|
// VELOCITY CONSTANTS
|
||||||
public static int AUTO_CLOSE_VEL = 3175; //3300;
|
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.LLResult;
|
||||||
import com.qualcomm.hardware.limelightvision.LLResultTypes;
|
import com.qualcomm.hardware.limelightvision.LLResultTypes;
|
||||||
import com.qualcomm.hardware.limelightvision.Limelight3A;
|
import com.qualcomm.hardware.limelightvision.Limelight3A;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
|
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
|
||||||
@@ -16,6 +17,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
|
|||||||
import java.util.List;
|
import java.util.List;
|
||||||
|
|
||||||
@Config
|
@Config
|
||||||
|
@TeleOp
|
||||||
public class Turret {
|
public class Turret {
|
||||||
|
|
||||||
public static double turretTolerance = 0.02;
|
public static double turretTolerance = 0.02;
|
||||||
|
|||||||
Reference in New Issue
Block a user