stash
This commit is contained in:
@@ -0,0 +1,648 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode.autonomous.actions;
|
||||||
|
|
||||||
|
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||||
|
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
|
||||||
|
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
|
||||||
|
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
|
||||||
|
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
|
||||||
|
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
|
||||||
|
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
|
||||||
|
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
|
||||||
|
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
|
||||||
|
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
|
||||||
|
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
|
||||||
|
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
|
||||||
|
|
||||||
|
import androidx.annotation.NonNull;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||||
|
import com.acmerobotics.roadrunner.Action;
|
||||||
|
import com.acmerobotics.roadrunner.Pose2d;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
|
||||||
|
import org.firstinspires.ftc.teamcode.utils.Drivetrain;
|
||||||
|
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;
|
||||||
|
|
||||||
|
public class Actions{
|
||||||
|
Robot robot;
|
||||||
|
MultipleTelemetry TELE;
|
||||||
|
Servos servos;
|
||||||
|
Flywheel flywheel;
|
||||||
|
MecanumDrive drive;
|
||||||
|
Spindexer spindexer;
|
||||||
|
Targeting targeting;
|
||||||
|
Targeting.Settings targetingSettings;
|
||||||
|
Turret turret;
|
||||||
|
private int driverSlotGreen = 0;
|
||||||
|
private int passengerSlotGreen = 0;
|
||||||
|
private int rearSlotGreen = 0;
|
||||||
|
private int mostGreenSlot = 0;
|
||||||
|
private double firstSpindexShootPos = spinStartPos;
|
||||||
|
private boolean shootForward = true;
|
||||||
|
public static double firstShootTime = 0.3;
|
||||||
|
public int motif = 0;
|
||||||
|
|
||||||
|
public Actions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur){
|
||||||
|
this.robot = rob;
|
||||||
|
this.drive = dri;
|
||||||
|
this.TELE = tel;
|
||||||
|
this.servos = ser;
|
||||||
|
this.flywheel = fly;
|
||||||
|
this.spindexer = spi;
|
||||||
|
this.targeting = tar;
|
||||||
|
this.targetingSettings = tS;
|
||||||
|
this.turret = tur;
|
||||||
|
}
|
||||||
|
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
|
||||||
|
return new Action() {
|
||||||
|
double stamp = 0.0;
|
||||||
|
int ticker = 0;
|
||||||
|
|
||||||
|
double spindexerWiggle = 0.01;
|
||||||
|
|
||||||
|
boolean decideGreenSlot = false;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||||
|
if (ticker == 0) {
|
||||||
|
stamp = System.currentTimeMillis();
|
||||||
|
}
|
||||||
|
ticker++;
|
||||||
|
servos.setTransferPos(transferServo_out);
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
|
double robX = drive.localizer.getPose().position.x;
|
||||||
|
double robY = drive.localizer.getPose().position.y;
|
||||||
|
double robotHeading = drive.localizer.getPose().heading.toDouble();
|
||||||
|
|
||||||
|
double goalX = -15;
|
||||||
|
double goalY = 0;
|
||||||
|
|
||||||
|
double dx = robX - goalX; // delta x from robot to goal
|
||||||
|
double dy = robY - goalY; // delta y from robot to goal
|
||||||
|
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
|
||||||
|
|
||||||
|
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
|
||||||
|
|
||||||
|
targetingSettings = targeting.calculateSettings
|
||||||
|
(robX, robY, robotHeading, 0.0, turretInterpolate);
|
||||||
|
|
||||||
|
turret.trackGoal(deltaPose);
|
||||||
|
|
||||||
|
TELE.addData("Velocity", flywheel.getVelo());
|
||||||
|
TELE.addData("Hood", robot.hood.getPosition());
|
||||||
|
TELE.addData("motif", motif_id);
|
||||||
|
TELE.update();
|
||||||
|
|
||||||
|
if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
|
||||||
|
|
||||||
|
spindexerWiggle *= -1.0;
|
||||||
|
|
||||||
|
servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
|
||||||
|
|
||||||
|
spindexer.detectBalls(true, true);
|
||||||
|
|
||||||
|
if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) {
|
||||||
|
driverSlotGreen++;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) {
|
||||||
|
passengerSlotGreen++;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) {
|
||||||
|
rearSlotGreen++;
|
||||||
|
}
|
||||||
|
|
||||||
|
spindexer.setIntakePower(1);
|
||||||
|
|
||||||
|
decideGreenSlot = true;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
} else if (decideGreenSlot) {
|
||||||
|
|
||||||
|
if (driverSlotGreen >= passengerSlotGreen && driverSlotGreen >= rearSlotGreen) {
|
||||||
|
mostGreenSlot = 3;
|
||||||
|
} else if (passengerSlotGreen >= driverSlotGreen && passengerSlotGreen >= rearSlotGreen) {
|
||||||
|
mostGreenSlot = 2;
|
||||||
|
} else {
|
||||||
|
mostGreenSlot = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
decideGreenSlot = false;
|
||||||
|
|
||||||
|
if (motif_id == 21) {
|
||||||
|
if (mostGreenSlot == 1) {
|
||||||
|
firstSpindexShootPos = spindexer_outtakeBall1;
|
||||||
|
shootForward = true;
|
||||||
|
} else if (mostGreenSlot == 2) {
|
||||||
|
firstSpindexShootPos = spindexer_outtakeBall2;
|
||||||
|
shootForward = false;
|
||||||
|
} else {
|
||||||
|
firstSpindexShootPos = spindexer_outtakeBall3;
|
||||||
|
shootForward = false;
|
||||||
|
}
|
||||||
|
} else if (motif_id == 22) {
|
||||||
|
if (mostGreenSlot == 1) {
|
||||||
|
firstSpindexShootPos = spindexer_outtakeBall2;
|
||||||
|
shootForward = false;
|
||||||
|
} else if (mostGreenSlot == 2) {
|
||||||
|
firstSpindexShootPos = spindexer_outtakeBall3;
|
||||||
|
shootForward = false;
|
||||||
|
} else {
|
||||||
|
firstSpindexShootPos = spindexer_outtakeBall2;
|
||||||
|
shootForward = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
if (mostGreenSlot == 1) {
|
||||||
|
firstSpindexShootPos = spindexer_outtakeBall3;
|
||||||
|
shootForward = false;
|
||||||
|
} else if (mostGreenSlot == 2) {
|
||||||
|
firstSpindexShootPos = spindexer_outtakeBall3b;
|
||||||
|
shootForward = true;
|
||||||
|
} else {
|
||||||
|
firstSpindexShootPos = spindexer_outtakeBall1;
|
||||||
|
shootForward = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
} else if ((System.currentTimeMillis() - stamp) < (time * 1000)) {
|
||||||
|
// TELE.addData("MostGreenSlot", mostGreenSlot);
|
||||||
|
// TELE.update();
|
||||||
|
spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
|
||||||
|
|
||||||
|
servos.setSpinPos(firstSpindexShootPos);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
public Action shootAll(int vel, double shootTime, double spindexSpeed) {
|
||||||
|
return new Action() {
|
||||||
|
int ticker = 1;
|
||||||
|
double stamp = 0.0;
|
||||||
|
double velo = vel;
|
||||||
|
int shooterTicker = 0;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||||
|
TELE.addData("Velocity", flywheel.getVelo());
|
||||||
|
TELE.addData("Hood", robot.hood.getPosition());
|
||||||
|
TELE.update();
|
||||||
|
|
||||||
|
double voltage = robot.voltage.getVoltage();
|
||||||
|
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
|
||||||
|
flywheel.manageFlywheel(vel);
|
||||||
|
velo = flywheel.getVelo();
|
||||||
|
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
|
spindexer.setIntakePower(-0.3);
|
||||||
|
|
||||||
|
if (ticker == 1) {
|
||||||
|
stamp = System.currentTimeMillis();
|
||||||
|
}
|
||||||
|
ticker++;
|
||||||
|
|
||||||
|
spindexer.setIntakePower(0);
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
|
double robX = drive.localizer.getPose().position.x;
|
||||||
|
double robY = drive.localizer.getPose().position.y;
|
||||||
|
double robotHeading = drive.localizer.getPose().heading.toDouble();
|
||||||
|
|
||||||
|
double goalX = -15;
|
||||||
|
double goalY = 0;
|
||||||
|
|
||||||
|
double dx = robX - goalX; // delta x from robot to goal
|
||||||
|
double dy = robY - goalY; // delta y from robot to goal
|
||||||
|
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
|
||||||
|
|
||||||
|
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
|
||||||
|
|
||||||
|
targetingSettings = targeting.calculateSettings
|
||||||
|
(robX, robY, robotHeading, 0.0, turretInterpolate);
|
||||||
|
|
||||||
|
turret.trackGoal(deltaPose);
|
||||||
|
|
||||||
|
if ((System.currentTimeMillis() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
|
||||||
|
|
||||||
|
if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) {
|
||||||
|
servos.setSpinPos(spinStartPos);
|
||||||
|
} else {
|
||||||
|
servos.setTransferPos(transferServo_in);
|
||||||
|
shooterTicker++;
|
||||||
|
double prevSpinPos = servos.getSpinCmdPos();
|
||||||
|
servos.setSpinPos(prevSpinPos + spindexSpeed);
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
servos.setTransferPos(transferServo_out);
|
||||||
|
|
||||||
|
spindexer.resetSpindexer();
|
||||||
|
spindexer.processIntake();
|
||||||
|
|
||||||
|
return false;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
public Action shootAllAuto(double shootTime, double spindexSpeed) {
|
||||||
|
return new Action() {
|
||||||
|
int ticker = 1;
|
||||||
|
|
||||||
|
double stamp = 0.0;
|
||||||
|
|
||||||
|
double velo = 0.0;
|
||||||
|
|
||||||
|
int shooterTicker = 0;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||||
|
TELE.addData("Velocity", flywheel.getVelo());
|
||||||
|
TELE.addData("Hood", robot.hood.getPosition());
|
||||||
|
TELE.update();
|
||||||
|
|
||||||
|
velo = flywheel.getVelo();
|
||||||
|
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
|
spindexer.setIntakePower(-0.3);
|
||||||
|
|
||||||
|
if (ticker == 1) {
|
||||||
|
stamp = System.currentTimeMillis();
|
||||||
|
}
|
||||||
|
ticker++;
|
||||||
|
|
||||||
|
spindexer.setIntakePower(0);
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
|
double robX = drive.localizer.getPose().position.x;
|
||||||
|
double robY = drive.localizer.getPose().position.y;
|
||||||
|
double robotHeading = drive.localizer.getPose().heading.toDouble();
|
||||||
|
|
||||||
|
double goalX = -15;
|
||||||
|
double goalY = 0;
|
||||||
|
|
||||||
|
double dx = robX - goalX; // delta x from robot to goal
|
||||||
|
double dy = robY - goalY; // delta y from robot to goal
|
||||||
|
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
|
||||||
|
|
||||||
|
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
|
||||||
|
|
||||||
|
targetingSettings = targeting.calculateSettings
|
||||||
|
(robX, robY, robotHeading, 0.0, turretInterpolate);
|
||||||
|
|
||||||
|
turret.trackGoal(deltaPose);
|
||||||
|
|
||||||
|
if (System.currentTimeMillis() - stamp < shootTime) {
|
||||||
|
|
||||||
|
if (System.currentTimeMillis() - stamp < firstShootTime) {
|
||||||
|
servos.setTransferPos(transferServo_out);
|
||||||
|
servos.setSpinPos(firstSpindexShootPos);
|
||||||
|
} else {
|
||||||
|
servos.setTransferPos(transferServo_in);
|
||||||
|
shooterTicker++;
|
||||||
|
double prevSpinPos = servos.getSpinCmdPos();
|
||||||
|
|
||||||
|
if (shootForward) {
|
||||||
|
servos.setSpinPos(prevSpinPos + spindexSpeed);
|
||||||
|
} else {
|
||||||
|
servos.setSpinPos(prevSpinPos - spindexSpeed);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
servos.setTransferPos(transferServo_out);
|
||||||
|
|
||||||
|
spindexer.resetSpindexer();
|
||||||
|
spindexer.processIntake();
|
||||||
|
|
||||||
|
return false;
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
public Action intake(double intakeTime) {
|
||||||
|
return new Action() {
|
||||||
|
double stamp = 0.0;
|
||||||
|
int ticker = 0;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||||
|
if (ticker == 0) {
|
||||||
|
stamp = System.currentTimeMillis();
|
||||||
|
}
|
||||||
|
ticker++;
|
||||||
|
|
||||||
|
spindexer.processIntake();
|
||||||
|
spindexer.setIntakePower(1);
|
||||||
|
|
||||||
|
spindexer.ballCounterLight();
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
|
TELE.addData("Full?", spindexer.isFull());
|
||||||
|
TELE.update();
|
||||||
|
|
||||||
|
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
public Action detectObelisk(
|
||||||
|
double time,
|
||||||
|
double posX,
|
||||||
|
double posY,
|
||||||
|
double posXTolerance,
|
||||||
|
double posYTolerance,
|
||||||
|
double turrPos
|
||||||
|
) {
|
||||||
|
|
||||||
|
boolean timeFallback = (time != 0.501);
|
||||||
|
boolean posXFallback = (posX != 0.501);
|
||||||
|
boolean posYFallback = (posY != 0.501);
|
||||||
|
|
||||||
|
return new Action() {
|
||||||
|
|
||||||
|
double stamp = 0.0;
|
||||||
|
int ticker = 0;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||||
|
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
Pose2d currentPose = drive.localizer.getPose();
|
||||||
|
|
||||||
|
if (ticker == 0) {
|
||||||
|
stamp = System.currentTimeMillis();
|
||||||
|
robot.limelight.pipelineSwitch(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
ticker++;
|
||||||
|
motif = turret.detectObelisk();
|
||||||
|
|
||||||
|
turret.setTurret(turrPos);
|
||||||
|
|
||||||
|
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
|
||||||
|
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
|
||||||
|
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
|
||||||
|
|
||||||
|
boolean shouldFinish = timeDone || xDone || yDone;
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
teleStart = drive.localizer.getPose();
|
||||||
|
TELE.addData("Velocity", flywheel.getVelo());
|
||||||
|
TELE.addData("Hood", robot.hood.getPosition());
|
||||||
|
TELE.update();
|
||||||
|
|
||||||
|
if (shouldFinish){
|
||||||
|
if (redAlliance){
|
||||||
|
robot.limelight.pipelineSwitch(4);
|
||||||
|
} else {
|
||||||
|
robot.limelight.pipelineSwitch(2);
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
} else {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
public Action manageFlywheel(
|
||||||
|
double vel,
|
||||||
|
double hoodPos,
|
||||||
|
double time,
|
||||||
|
double posX,
|
||||||
|
double posY,
|
||||||
|
double posXTolerance,
|
||||||
|
double posYTolerance
|
||||||
|
) {
|
||||||
|
|
||||||
|
boolean timeFallback = (time != 0.501);
|
||||||
|
boolean posXFallback = (posX != 0.501);
|
||||||
|
boolean posYFallback = (posY != 0.501);
|
||||||
|
|
||||||
|
return new Action() {
|
||||||
|
|
||||||
|
double stamp = 0.0;
|
||||||
|
int ticker = 0;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||||
|
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
Pose2d currentPose = drive.localizer.getPose();
|
||||||
|
|
||||||
|
if (ticker == 0) {
|
||||||
|
stamp = System.currentTimeMillis();
|
||||||
|
}
|
||||||
|
|
||||||
|
ticker++;
|
||||||
|
|
||||||
|
double voltage = robot.voltage.getVoltage();
|
||||||
|
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
|
||||||
|
flywheel.manageFlywheel(vel);
|
||||||
|
servos.setHoodPos(hoodPos);
|
||||||
|
|
||||||
|
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
|
||||||
|
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
|
||||||
|
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
|
||||||
|
|
||||||
|
boolean shouldFinish = timeDone || xDone || yDone;
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
teleStart = drive.localizer.getPose();
|
||||||
|
TELE.addData("Velocity", flywheel.getVelo());
|
||||||
|
TELE.addData("Hood", robot.hood.getPosition());
|
||||||
|
TELE.update();
|
||||||
|
|
||||||
|
return !shouldFinish;
|
||||||
|
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
public Action manageShooterAuto(
|
||||||
|
double time,
|
||||||
|
double posX,
|
||||||
|
double posY,
|
||||||
|
double posXTolerance,
|
||||||
|
double posYTolerance
|
||||||
|
) {
|
||||||
|
|
||||||
|
boolean timeFallback = (time != 0.501);
|
||||||
|
boolean posXFallback = (posX != 0.501);
|
||||||
|
boolean posYFallback = (posY != 0.501);
|
||||||
|
|
||||||
|
return new Action() {
|
||||||
|
|
||||||
|
double stamp = 0.0;
|
||||||
|
int ticker = 0;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||||
|
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
Pose2d currentPose = drive.localizer.getPose();
|
||||||
|
|
||||||
|
if (ticker == 0) {
|
||||||
|
stamp = System.currentTimeMillis();
|
||||||
|
}
|
||||||
|
|
||||||
|
ticker++;
|
||||||
|
|
||||||
|
double robotX = drive.localizer.getPose().position.x;
|
||||||
|
double robotY = drive.localizer.getPose().position.y;
|
||||||
|
|
||||||
|
double robotHeading = drive.localizer.getPose().heading.toDouble();
|
||||||
|
|
||||||
|
double goalX = -15;
|
||||||
|
double goalY = 0;
|
||||||
|
|
||||||
|
double dx = robotX - goalX; // delta x from robot to goal
|
||||||
|
double dy = robotY - goalY; // delta y from robot to goal
|
||||||
|
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
|
||||||
|
|
||||||
|
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
|
||||||
|
|
||||||
|
targetingSettings = targeting.calculateSettings
|
||||||
|
(robotX, robotY, robotHeading, 0.0, false);
|
||||||
|
|
||||||
|
turret.trackGoal(deltaPose);
|
||||||
|
|
||||||
|
servos.setHoodPos(targetingSettings.hoodAngle);
|
||||||
|
|
||||||
|
double voltage = robot.voltage.getVoltage();
|
||||||
|
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
|
||||||
|
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
|
||||||
|
|
||||||
|
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
|
||||||
|
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
|
||||||
|
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
|
||||||
|
|
||||||
|
boolean shouldFinish = timeDone || xDone || yDone;
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
|
TELE.addData("Velocity", flywheel.getVelo());
|
||||||
|
TELE.addData("Hood", robot.hood.getPosition());
|
||||||
|
TELE.update();
|
||||||
|
|
||||||
|
return !shouldFinish;
|
||||||
|
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
public Action manageFlywheelAuto(
|
||||||
|
double time,
|
||||||
|
double posX,
|
||||||
|
double posY,
|
||||||
|
double posXTolerance,
|
||||||
|
double posYTolerance
|
||||||
|
) {
|
||||||
|
|
||||||
|
boolean timeFallback = (time != 0.501);
|
||||||
|
boolean posXFallback = (posX != 0.501);
|
||||||
|
boolean posYFallback = (posY != 0.501);
|
||||||
|
|
||||||
|
return new Action() {
|
||||||
|
|
||||||
|
double stamp = 0.0;
|
||||||
|
int ticker = 0;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||||
|
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
Pose2d currentPose = drive.localizer.getPose();
|
||||||
|
|
||||||
|
if (ticker == 0) {
|
||||||
|
stamp = System.currentTimeMillis();
|
||||||
|
}
|
||||||
|
|
||||||
|
ticker++;
|
||||||
|
|
||||||
|
double robotX = drive.localizer.getPose().position.x;
|
||||||
|
double robotY = drive.localizer.getPose().position.y;
|
||||||
|
|
||||||
|
double robotHeading = drive.localizer.getPose().heading.toDouble();
|
||||||
|
|
||||||
|
double goalX = -15;
|
||||||
|
double goalY = 0;
|
||||||
|
|
||||||
|
double dx = robotX - goalX; // delta x from robot to goal
|
||||||
|
double dy = robotY - goalY; // delta y from robot to goal
|
||||||
|
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
|
||||||
|
|
||||||
|
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
|
||||||
|
|
||||||
|
targetingSettings = targeting.calculateSettings
|
||||||
|
(robotX, robotY, robotHeading, 0.0, false);
|
||||||
|
|
||||||
|
servos.setHoodPos(targetingSettings.hoodAngle);
|
||||||
|
|
||||||
|
double voltage = robot.voltage.getVoltage();
|
||||||
|
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
|
||||||
|
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
|
||||||
|
|
||||||
|
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
|
||||||
|
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
|
||||||
|
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
|
||||||
|
|
||||||
|
boolean shouldFinish = timeDone || xDone || yDone;
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
teleStart = drive.localizer.getPose();
|
||||||
|
TELE.addData("Velocity", flywheel.getVelo());
|
||||||
|
TELE.addData("Hood", robot.hood.getPosition());
|
||||||
|
TELE.update();
|
||||||
|
|
||||||
|
return !shouldFinish;
|
||||||
|
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -32,4 +32,14 @@ public class ServoPositions {
|
|||||||
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;
|
||||||
|
|
||||||
|
// These values are ADDED to turrDefault
|
||||||
|
public static double redObeliskTurrPos1 = 0.12;
|
||||||
|
public static double redObeliskTurrPos2 = 0.13;
|
||||||
|
public static double redObeliskTurrPos3 = 0.14;
|
||||||
|
public static double blueObeliskTurrPos1 = -0.12;
|
||||||
|
public static double blueObeliskTurrPos2 = -0.13;
|
||||||
|
public static double blueObeliskTurrPos3 = -0.14;
|
||||||
|
public static double redTurretShootPos = 0.1;
|
||||||
|
public static double blueTurretShootPos = -0.14;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user