Merge branch 'auto9Ball'
# Conflicts: # TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_12Ball.java
This commit is contained in:
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,921 @@
|
||||
package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
|
||||
|
||||
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.constants.Poses_V2;
|
||||
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 ProtoAutoClose_V4 extends LinearOpMode {
|
||||
public static double shoot0Vel = 2300, shoot0Hood = 0.93;
|
||||
public static double autoSpinStartPos = 0.2;
|
||||
public static double shoot0SpinSpeedIncrease = 0.014;
|
||||
|
||||
public static double spindexerSpeedIncrease = 0.02;
|
||||
|
||||
public static double obeliskTurrPos = 0.53;
|
||||
public static double normalIntakeTime = 3.0;
|
||||
public static double shoot1Turr = 0.57;
|
||||
public static double shoot0XTolerance = 1.0;
|
||||
public static double turretShootPos = 0.53;
|
||||
public static double shootAllTime = 1.8;
|
||||
public static double shoot0Time = 1.6;
|
||||
public static double intake1Time = 3.0;
|
||||
public static double flywheel0Time = 3.5;
|
||||
public static double pickup1Speed = 25;
|
||||
// ---- SECOND SHOT / PICKUP ----
|
||||
public static double shoot1Vel = 2300;
|
||||
public static double shoot1Hood = 0.93;
|
||||
|
||||
public static double shootAllVelocity = 2500;
|
||||
public static double shootAllHood = 0.78;
|
||||
// ---- PICKUP TIMING ----
|
||||
public static double pickup1Time = 3.0;
|
||||
// ---- 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.0;
|
||||
public static double shootCycleTime = 2.5;
|
||||
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 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;
|
||||
|
||||
|
||||
public Action prepareShootAll(double time) {
|
||||
return new Action() {
|
||||
double stamp = 0.0;
|
||||
int ticker = 0;
|
||||
|
||||
@Override
|
||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||
if (ticker == 0) {
|
||||
stamp = System.currentTimeMillis();
|
||||
}
|
||||
ticker++;
|
||||
|
||||
robot.spin1.setPosition(autoSpinStartPos);
|
||||
robot.spin2.setPosition(1 - autoSpinStartPos);
|
||||
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
|
||||
turret.manualSetTurret(turretShootPos);
|
||||
|
||||
robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000);
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
TELE.addData("Velocity", flywheel.getVelo());
|
||||
TELE.addData("Hood", robot.hood.getPosition());
|
||||
TELE.update();
|
||||
|
||||
return (System.currentTimeMillis() - stamp) < (time * 1000);
|
||||
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
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();
|
||||
|
||||
Poses_V2.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();
|
||||
|
||||
Poses_V2.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);
|
||||
|
||||
motif = turret.detectObelisk();
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
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();
|
||||
|
||||
return !shouldFinish;
|
||||
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
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.pipelineSwitch(1);
|
||||
|
||||
turret = new Turret(robot, TELE, robot.limelight);
|
||||
|
||||
turret.manualSetTurret(0.4);
|
||||
|
||||
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
|
||||
|
||||
robot.spin1.setPosition(autoSpinStartPos);
|
||||
robot.spin2.setPosition(1 - autoSpinStartPos);
|
||||
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
|
||||
TrajectoryActionBuilder shoot0 = null;
|
||||
TrajectoryActionBuilder pickup1 = null;
|
||||
TrajectoryActionBuilder shoot1 = null;
|
||||
TrajectoryActionBuilder pickup2 = null;
|
||||
TrajectoryActionBuilder shoot2 = null;
|
||||
TrajectoryActionBuilder pickup3 = null;
|
||||
TrajectoryActionBuilder shoot3 = null;
|
||||
|
||||
robot.limelight.start();
|
||||
|
||||
robot.light.setPosition(1);
|
||||
|
||||
while (opModeInInit()) {
|
||||
|
||||
robot.hood.setPosition(shoot0Hood);
|
||||
|
||||
if (gamepad2.crossWasPressed()) {
|
||||
redAlliance = !redAlliance;
|
||||
}
|
||||
|
||||
if (redAlliance) {
|
||||
robot.light.setPosition(0.28);
|
||||
|
||||
// ---- FIRST SHOT ----
|
||||
x1 = rx1;
|
||||
y1 = ry1;
|
||||
h1 = rh1;
|
||||
|
||||
// ---- PICKUP PATH ----
|
||||
x2a = rx2a;
|
||||
y2a = ry2a;
|
||||
h2a = rh2a;
|
||||
x2b = rx2b;
|
||||
y2b = ry2b;
|
||||
h2b = rh2b;
|
||||
x3a = rx3a;
|
||||
y3a = ry3a;
|
||||
h3a = rh3a;
|
||||
x3b = rx3b;
|
||||
y3b = ry3b;
|
||||
h3b = rh3b;
|
||||
x4a = rx4a;
|
||||
y4a = ry4a;
|
||||
h4a = rh4a;
|
||||
x4b = rx4b;
|
||||
y4b = ry4b;
|
||||
h4b = rh4b;
|
||||
xPrep = rxPrep;
|
||||
yPrep = ryPrep;
|
||||
hPrep = rhPrep;
|
||||
xShoot = rShootX;
|
||||
yShoot = rShootY;
|
||||
hShoot = rShootH;
|
||||
|
||||
|
||||
} else {
|
||||
robot.light.setPosition(0.6);
|
||||
|
||||
// ---- FIRST SHOT ----
|
||||
x1 = bx1;
|
||||
y1 = by1;
|
||||
h1 = bh1;
|
||||
|
||||
// ---- PICKUP PATH ----
|
||||
x2a = bx2a;
|
||||
y2a = by2a;
|
||||
h2a = bh2a;
|
||||
x2b = bx2b;
|
||||
y2b = by2b;
|
||||
h2b = bh2b;
|
||||
x3a = bx3a;
|
||||
y3a = by3a;
|
||||
h3a = bh3a;
|
||||
x3b = bx3b;
|
||||
y3b = by3b;
|
||||
h3b = bh3b;
|
||||
x4a = bx4a;
|
||||
y4a = by4a;
|
||||
h4a = bh4a;
|
||||
x4b = bx4b;
|
||||
y4b = by4b;
|
||||
h4b = bh4b;
|
||||
|
||||
xPrep = bxPrep;
|
||||
yPrep = byPrep;
|
||||
hPrep = bhPrep;
|
||||
xShoot = bShootX;
|
||||
yShoot = bShootY;
|
||||
hShoot = bShootH;
|
||||
|
||||
}
|
||||
|
||||
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
|
||||
|
||||
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
.strafeToLinearHeading(new Vector2d(x2a, y2a), h2a)
|
||||
.strafeToLinearHeading(new Vector2d(x2b, y2b), h2b,
|
||||
new TranslationalVelConstraint(pickup1Speed));
|
||||
|
||||
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
|
||||
.strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
|
||||
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
|
||||
|
||||
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
|
||||
.strafeToLinearHeading(new Vector2d(x3a, y3a), h3a)
|
||||
.strafeToLinearHeading(new Vector2d(x3b, y3b), h3b,
|
||||
new TranslationalVelConstraint(pickup1Speed));
|
||||
|
||||
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b))
|
||||
.strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
|
||||
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
|
||||
|
||||
pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
.strafeToLinearHeading(new Vector2d(x4a, y4a), h4a)
|
||||
.strafeToLinearHeading(new Vector2d(x4b, y4b), h4b,
|
||||
new TranslationalVelConstraint(pickup1Speed));
|
||||
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b))
|
||||
.strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
|
||||
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
|
||||
|
||||
|
||||
|
||||
TELE.addData("Red?", redAlliance);
|
||||
TELE.update();
|
||||
}
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
if (opModeIsActive()) {
|
||||
|
||||
robot.transfer.setPower(1);
|
||||
|
||||
assert shoot0 != null;
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
shoot0.build(),
|
||||
manageFlywheel(
|
||||
shoot0Vel,
|
||||
shoot0Hood,
|
||||
flywheel0Time,
|
||||
x1,
|
||||
0.501,
|
||||
shoot0XTolerance,
|
||||
0.501
|
||||
)
|
||||
|
||||
)
|
||||
);
|
||||
|
||||
Actions.runBlocking(
|
||||
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
|
||||
);
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
pickup1.build(),
|
||||
manageFlywheel(
|
||||
shootAllVelocity,
|
||||
shootAllHood,
|
||||
pickup1Time,
|
||||
x2b,
|
||||
y2b,
|
||||
pickup1XTolerance,
|
||||
pickup1YTolerance
|
||||
),
|
||||
intake(intake1Time)
|
||||
|
||||
)
|
||||
);
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
manageFlywheel(
|
||||
shootAllVelocity,
|
||||
shootAllHood,
|
||||
shoot1Time,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501
|
||||
),
|
||||
shoot1.build(),
|
||||
prepareShootAll(shoot1Time)
|
||||
)
|
||||
);
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
manageShooterAuto(
|
||||
shootAllTime,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501
|
||||
),
|
||||
shootAllAuto(shootAllTime, spindexerSpeedIncrease)
|
||||
)
|
||||
|
||||
);
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
pickup2.build(),
|
||||
manageShooterAuto(
|
||||
normalIntakeTime,
|
||||
x2b,
|
||||
y2b,
|
||||
pickup1XTolerance,
|
||||
pickup1YTolerance
|
||||
),
|
||||
intake(normalIntakeTime)
|
||||
|
||||
)
|
||||
);
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
manageFlywheelAuto(
|
||||
shootCycleTime,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501
|
||||
),
|
||||
shoot2.build(),
|
||||
prepareShootAll(shootCycleTime)
|
||||
)
|
||||
);
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
manageShooterAuto(
|
||||
shootAllTime,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501
|
||||
),
|
||||
shootAllAuto(shootAllTime, spindexerSpeedIncrease)
|
||||
)
|
||||
|
||||
);
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
pickup3.build(),
|
||||
manageShooterAuto(
|
||||
normalIntakeTime,
|
||||
x2b,
|
||||
y2b,
|
||||
pickup1XTolerance,
|
||||
pickup1YTolerance
|
||||
),
|
||||
intake(normalIntakeTime)
|
||||
|
||||
)
|
||||
);
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
manageFlywheelAuto(
|
||||
shootCycleTime,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501
|
||||
),
|
||||
shoot3.build(),
|
||||
prepareShootAll(shootCycleTime)
|
||||
)
|
||||
);
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
manageShooterAuto(
|
||||
shootAllTime,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501,
|
||||
0.501
|
||||
),
|
||||
shootAllAuto(shootAllTime, spindexerSpeedIncrease)
|
||||
)
|
||||
|
||||
);
|
||||
|
||||
|
||||
TELE.addLine("finished");
|
||||
TELE.update();
|
||||
|
||||
sleep(2000);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,6 +1,11 @@
|
||||
package org.firstinspires.ftc.teamcode.constants;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
|
||||
@Config
|
||||
public class Color {
|
||||
public static boolean redAlliance = true;
|
||||
public static double Light0 = 0.28, Light1 = 0.67, Light2 = 0.36, Light3 = 0.5;
|
||||
|
||||
public static double colorFilterAlpha = 0.15;
|
||||
}
|
||||
|
||||
@@ -14,17 +14,17 @@ public class Poses {
|
||||
|
||||
public static Pose2d goalPose = new Pose2d(-10, 0, 0);
|
||||
|
||||
public static double rx1 = 40, ry1 = -7, rh1 = 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 = 23, ry2b = 36, rh2b = Math.toRadians(140);
|
||||
public static double rx2b = 23, ry2b = 36, 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 rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140);
|
||||
public static double rx3b = 38, ry3b = 56, rh3b = Math.toRadians(140.1);
|
||||
|
||||
public static double rx4a = 72, ry4a = 55, rh4a = Math.toRadians(140);
|
||||
public static double rx4b = 48, ry4b = 79, rh4b = Math.toRadians(140);
|
||||
public static double rx4a = 72, ry4a = 65, rh4a = Math.toRadians(145);
|
||||
public static double rx4b = 37, ry4b = 85, rh4b = Math.toRadians(145.1);
|
||||
|
||||
public static double bx1 = 40, by1 = 7, bh1 = 0;
|
||||
public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140);
|
||||
@@ -38,6 +38,13 @@ public class Poses {
|
||||
public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140);
|
||||
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
|
||||
|
||||
public static double rShootX = 40, rShootY = -7, rShootH = Math.toRadians(50);
|
||||
public static double rxPrep = 45, ryPrep = 10, rhPrep = Math.toRadians(50);
|
||||
|
||||
public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140);
|
||||
public static double bxPrep = 50, byPrep = -10, bhPrep = Math.toRadians(140);
|
||||
|
||||
|
||||
public static Pose2d teleStart = new Pose2d(0, 0, 0);
|
||||
|
||||
}
|
||||
|
||||
@@ -5,18 +5,21 @@ import com.acmerobotics.dashboard.config.Config;
|
||||
@Config
|
||||
public class ServoPositions {
|
||||
|
||||
public static double spindexer_intakePos1 = 0;
|
||||
public static double spindexer_intakePos1 = 0.05; //0.13;
|
||||
|
||||
public static double spindexer_intakePos2 = 0.19;//0.5;
|
||||
public static double spindexer_intakePos2 = 0.24; //0.33;//0.5;
|
||||
|
||||
public static double spindexer_intakePos3 = 0.38;//0.66;
|
||||
public static double spindexer_intakePos3 = 0.43; //0.53;//0.66;
|
||||
|
||||
public static double spindexer_outtakeBall3 = 0.65;
|
||||
public static double spindexer_outtakeBall3 = 0.71; //0.65; //0.24;
|
||||
|
||||
public static double spindexer_outtakeBall2 = 0.46;
|
||||
public static double spindexer_outtakeBall1 = 0.27;
|
||||
public static double spinStartPos = spindexer_outtakeBall1 - 0.08;
|
||||
public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6;
|
||||
public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4;
|
||||
public static double spinStartPos = spindexer_outtakeBall3 - 0.1;
|
||||
|
||||
public static double shootAllAutoSpinStartPos = 0.2;
|
||||
public static double shootAllSpindexerSpeedIncrease = 0.02;
|
||||
public static double shootAllTime = 1.8;
|
||||
|
||||
public static double transferServo_out = 0.15;
|
||||
|
||||
|
||||
@@ -5,9 +5,7 @@ import androidx.annotation.NonNull;
|
||||
import com.acmerobotics.dashboard.canvas.Canvas;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||
import com.acmerobotics.roadrunner.AccelConstraint;
|
||||
import com.acmerobotics.roadrunner.Action;
|
||||
import com.acmerobotics.roadrunner.Actions;
|
||||
import com.acmerobotics.roadrunner.*;
|
||||
import com.acmerobotics.roadrunner.AngularVelConstraint;
|
||||
import com.acmerobotics.roadrunner.DualNum;
|
||||
import com.acmerobotics.roadrunner.HolonomicController;
|
||||
@@ -16,20 +14,12 @@ import com.acmerobotics.roadrunner.MinVelConstraint;
|
||||
import com.acmerobotics.roadrunner.MotorFeedforward;
|
||||
import com.acmerobotics.roadrunner.Pose2d;
|
||||
import com.acmerobotics.roadrunner.Pose2dDual;
|
||||
import com.acmerobotics.roadrunner.PoseVelocity2d;
|
||||
import com.acmerobotics.roadrunner.PoseVelocity2dDual;
|
||||
import com.acmerobotics.roadrunner.ProfileAccelConstraint;
|
||||
import com.acmerobotics.roadrunner.ProfileParams;
|
||||
import com.acmerobotics.roadrunner.Rotation2d;
|
||||
import com.acmerobotics.roadrunner.Time;
|
||||
import com.acmerobotics.roadrunner.TimeTrajectory;
|
||||
import com.acmerobotics.roadrunner.TimeTurn;
|
||||
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
|
||||
import com.acmerobotics.roadrunner.TrajectoryBuilderParams;
|
||||
import com.acmerobotics.roadrunner.TurnConstraints;
|
||||
import com.acmerobotics.roadrunner.Twist2d;
|
||||
import com.acmerobotics.roadrunner.Twist2dDual;
|
||||
import com.acmerobotics.roadrunner.Vector2d;
|
||||
import com.acmerobotics.roadrunner.VelConstraint;
|
||||
import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
|
||||
import com.acmerobotics.roadrunner.ftc.Encoder;
|
||||
@@ -56,131 +46,13 @@ import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumCommandMessage;
|
||||
import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumLocalizerInputsMessage;
|
||||
import org.firstinspires.ftc.teamcode.libs.RR.messages.PoseMessage;
|
||||
|
||||
import java.lang.Math;
|
||||
import java.util.Arrays;
|
||||
import java.util.LinkedList;
|
||||
import java.util.List;
|
||||
|
||||
@Config
|
||||
public final class MecanumDrive {
|
||||
public static Params PARAMS = new Params();
|
||||
public final MecanumKinematics kinematics = new MecanumKinematics(
|
||||
PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick);
|
||||
public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
|
||||
PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
|
||||
public final VelConstraint defaultVelConstraint =
|
||||
new MinVelConstraint(Arrays.asList(
|
||||
kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
|
||||
new AngularVelConstraint(PARAMS.maxAngVel)
|
||||
));
|
||||
public final AccelConstraint defaultAccelConstraint =
|
||||
new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);
|
||||
public final DcMotorEx leftFront, leftBack, rightBack, rightFront;
|
||||
public final VoltageSensor voltageSensor;
|
||||
public final LazyImu lazyImu;
|
||||
public final Localizer localizer;
|
||||
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
|
||||
private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
|
||||
private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
|
||||
private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
|
||||
private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000);
|
||||
public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
|
||||
LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
|
||||
|
||||
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
|
||||
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
|
||||
}
|
||||
|
||||
// TODO: make sure your config has motors with these names (or change them)
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
||||
leftFront = hardwareMap.get(DcMotorEx.class, "fl");
|
||||
leftBack = hardwareMap.get(DcMotorEx.class, "bl");
|
||||
rightBack = hardwareMap.get(DcMotorEx.class, "br");
|
||||
rightFront = hardwareMap.get(DcMotorEx.class, "fr");
|
||||
|
||||
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
// TODO: reverse motor directions if needed
|
||||
//
|
||||
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
// TODO: make sure your config has an IMU with this name (can be BNO or BHI)
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
||||
lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
|
||||
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
|
||||
|
||||
voltageSensor = hardwareMap.voltageSensor.iterator().next();
|
||||
|
||||
localizer = new PinpointLocalizer(hardwareMap, PARAMS.inPerTick, pose);
|
||||
|
||||
FlightRecorder.write("MECANUM_PARAMS", PARAMS);
|
||||
}
|
||||
|
||||
public void setDrivePowers(PoseVelocity2d powers) {
|
||||
MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(
|
||||
PoseVelocity2dDual.constant(powers, 1));
|
||||
|
||||
double maxPowerMag = 1;
|
||||
for (DualNum<Time> power : wheelVels.all()) {
|
||||
maxPowerMag = Math.max(maxPowerMag, power.value());
|
||||
}
|
||||
|
||||
leftFront.setPower(wheelVels.leftFront.get(0) / maxPowerMag);
|
||||
leftBack.setPower(wheelVels.leftBack.get(0) / maxPowerMag);
|
||||
rightBack.setPower(wheelVels.rightBack.get(0) / maxPowerMag);
|
||||
rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag);
|
||||
}
|
||||
|
||||
public PoseVelocity2d updatePoseEstimate() {
|
||||
PoseVelocity2d vel = localizer.update();
|
||||
poseHistory.add(localizer.getPose());
|
||||
|
||||
while (poseHistory.size() > 100) {
|
||||
poseHistory.removeFirst();
|
||||
}
|
||||
|
||||
estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
|
||||
|
||||
return vel;
|
||||
}
|
||||
|
||||
private void drawPoseHistory(Canvas c) {
|
||||
double[] xPoints = new double[poseHistory.size()];
|
||||
double[] yPoints = new double[poseHistory.size()];
|
||||
|
||||
int i = 0;
|
||||
for (Pose2d t : poseHistory) {
|
||||
xPoints[i] = t.position.x;
|
||||
yPoints[i] = t.position.y;
|
||||
|
||||
i++;
|
||||
}
|
||||
|
||||
c.setStrokeWidth(1);
|
||||
c.setStroke("#3F51B5");
|
||||
c.strokePolyline(xPoints, yPoints);
|
||||
}
|
||||
|
||||
public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
|
||||
return new TrajectoryActionBuilder(
|
||||
TurnAction::new,
|
||||
FollowTrajectoryAction::new,
|
||||
new TrajectoryBuilderParams(
|
||||
1e-6,
|
||||
new ProfileParams(
|
||||
0.25, 0.1, 1e-2
|
||||
)
|
||||
),
|
||||
beginPose, 0.0,
|
||||
defaultTurnConstraints,
|
||||
defaultVelConstraint, defaultAccelConstraint
|
||||
);
|
||||
}
|
||||
|
||||
public static class Params {
|
||||
// IMU orientation
|
||||
// TODO: fill in these values based on
|
||||
@@ -201,9 +73,9 @@ public final class MecanumDrive {
|
||||
public double kA = 0.000046;
|
||||
|
||||
// path profile parameters (in inches)
|
||||
public double maxWheelVel = 250;
|
||||
public double maxWheelVel = 180;
|
||||
public double minProfileAccel = -40;
|
||||
public double maxProfileAccel = 250;
|
||||
public double maxProfileAccel = 180;
|
||||
|
||||
// turn profile parameters (in radians)
|
||||
public double maxAngVel = 4* Math.PI; // shared with path
|
||||
@@ -219,6 +91,35 @@ public final class MecanumDrive {
|
||||
public double headingVelGain = 0.0; // shared with turn
|
||||
}
|
||||
|
||||
public static Params PARAMS = new Params();
|
||||
|
||||
public final MecanumKinematics kinematics = new MecanumKinematics(
|
||||
PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick);
|
||||
|
||||
public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
|
||||
PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
|
||||
public final VelConstraint defaultVelConstraint =
|
||||
new MinVelConstraint(Arrays.asList(
|
||||
kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
|
||||
new AngularVelConstraint(PARAMS.maxAngVel)
|
||||
));
|
||||
public final AccelConstraint defaultAccelConstraint =
|
||||
new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);
|
||||
|
||||
public final DcMotorEx leftFront, leftBack, rightBack, rightFront;
|
||||
|
||||
public final VoltageSensor voltageSensor;
|
||||
|
||||
public final LazyImu lazyImu;
|
||||
|
||||
public final Localizer localizer;
|
||||
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
|
||||
|
||||
private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
|
||||
private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
|
||||
private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
|
||||
private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000);
|
||||
|
||||
public class DriveLocalizer implements Localizer {
|
||||
public final Encoder leftFront, leftBack, rightBack, rightFront;
|
||||
public final IMU imu;
|
||||
@@ -243,13 +144,13 @@ public final class MecanumDrive {
|
||||
}
|
||||
|
||||
@Override
|
||||
public Pose2d getPose() {
|
||||
return pose;
|
||||
public void setPose(Pose2d pose) {
|
||||
this.pose = pose;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setPose(Pose2d pose) {
|
||||
this.pose = pose;
|
||||
public Pose2d getPose() {
|
||||
return pose;
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -315,11 +216,64 @@ public final class MecanumDrive {
|
||||
}
|
||||
}
|
||||
|
||||
public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
|
||||
LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
|
||||
|
||||
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
|
||||
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
|
||||
}
|
||||
|
||||
// TODO: make sure your config has motors with these names (or change them)
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
||||
leftFront = hardwareMap.get(DcMotorEx.class, "fl");
|
||||
leftBack = hardwareMap.get(DcMotorEx.class, "bl");
|
||||
rightBack = hardwareMap.get(DcMotorEx.class, "br");
|
||||
rightFront = hardwareMap.get(DcMotorEx.class, "fr");
|
||||
|
||||
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
// TODO: reverse motor directions if needed
|
||||
//
|
||||
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
// TODO: make sure your config has an IMU with this name (can be BNO or BHI)
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
||||
lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
|
||||
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
|
||||
|
||||
voltageSensor = hardwareMap.voltageSensor.iterator().next();
|
||||
|
||||
localizer = new PinpointLocalizer(hardwareMap, PARAMS.inPerTick, pose);
|
||||
|
||||
FlightRecorder.write("MECANUM_PARAMS", PARAMS);
|
||||
}
|
||||
|
||||
public void setDrivePowers(PoseVelocity2d powers) {
|
||||
MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(
|
||||
PoseVelocity2dDual.constant(powers, 1));
|
||||
|
||||
double maxPowerMag = 1;
|
||||
for (DualNum<Time> power : wheelVels.all()) {
|
||||
maxPowerMag = Math.max(maxPowerMag, power.value());
|
||||
}
|
||||
|
||||
leftFront.setPower(wheelVels.leftFront.get(0) / maxPowerMag);
|
||||
leftBack.setPower(wheelVels.leftBack.get(0) / maxPowerMag);
|
||||
rightBack.setPower(wheelVels.rightBack.get(0) / maxPowerMag);
|
||||
rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag);
|
||||
}
|
||||
|
||||
public final class FollowTrajectoryAction implements Action {
|
||||
public final TimeTrajectory timeTrajectory;
|
||||
private final double[] xPoints, yPoints;
|
||||
private double beginTs = -1;
|
||||
|
||||
private final double[] xPoints, yPoints;
|
||||
|
||||
public FollowTrajectoryAction(TimeTrajectory t) {
|
||||
timeTrajectory = t;
|
||||
|
||||
@@ -496,4 +450,51 @@ public final class MecanumDrive {
|
||||
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
|
||||
}
|
||||
}
|
||||
|
||||
public PoseVelocity2d updatePoseEstimate() {
|
||||
PoseVelocity2d vel = localizer.update();
|
||||
poseHistory.add(localizer.getPose());
|
||||
|
||||
while (poseHistory.size() > 100) {
|
||||
poseHistory.removeFirst();
|
||||
}
|
||||
|
||||
estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
|
||||
|
||||
|
||||
return vel;
|
||||
}
|
||||
|
||||
private void drawPoseHistory(Canvas c) {
|
||||
double[] xPoints = new double[poseHistory.size()];
|
||||
double[] yPoints = new double[poseHistory.size()];
|
||||
|
||||
int i = 0;
|
||||
for (Pose2d t : poseHistory) {
|
||||
xPoints[i] = t.position.x;
|
||||
yPoints[i] = t.position.y;
|
||||
|
||||
i++;
|
||||
}
|
||||
|
||||
c.setStrokeWidth(1);
|
||||
c.setStroke("#3F51B5");
|
||||
c.strokePolyline(xPoints, yPoints);
|
||||
}
|
||||
|
||||
public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
|
||||
return new TrajectoryActionBuilder(
|
||||
TurnAction::new,
|
||||
FollowTrajectoryAction::new,
|
||||
new TrajectoryBuilderParams(
|
||||
1e-6,
|
||||
new ProfileParams(
|
||||
0.25, 0.1, 1e-2
|
||||
)
|
||||
),
|
||||
beginPose, 0.0,
|
||||
defaultTurnConstraints,
|
||||
defaultVelConstraint, defaultAccelConstraint
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,8 +1,13 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
|
||||
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_intakePos2;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos3;
|
||||
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.transferServo_in;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
|
||||
import static org.firstinspires.ftc.teamcode.utils.Servos.spinD;
|
||||
@@ -27,6 +32,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
|
||||
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.utils.Flywheel;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
@@ -122,7 +128,8 @@ public class TeleopV3 extends LinearOpMode {
|
||||
private int tickerA = 1;
|
||||
private boolean transferIn = false;
|
||||
boolean turretInterpolate = false;
|
||||
public static double spinSpeedIncrease = 0.04;
|
||||
public static double spinSpeedIncrease = 0.03;
|
||||
public static int resetSpinTicks = 4;
|
||||
|
||||
public static double velPrediction(double distance) {
|
||||
if (distance < 30) {
|
||||
@@ -172,12 +179,24 @@ public class TeleopV3 extends LinearOpMode {
|
||||
|
||||
Turret turret = new Turret(robot, TELE, robot.limelight);
|
||||
robot.light.setPosition(1);
|
||||
while (opModeInInit()){
|
||||
robot.limelight.start();
|
||||
if (redAlliance) {
|
||||
robot.limelight.pipelineSwitch(4);
|
||||
} else {
|
||||
robot.limelight.pipelineSwitch(2);
|
||||
}
|
||||
}
|
||||
|
||||
waitForStart();
|
||||
if (isStopRequested()) return;
|
||||
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
|
||||
while (opModeIsActive()) {
|
||||
//LIMELIGHT START
|
||||
TELE.addData("Is limelight on?", robot.limelight.getStatus());
|
||||
|
||||
// LIGHT COLORS
|
||||
spindexer.ballCounterLight();
|
||||
|
||||
@@ -245,56 +264,56 @@ public class TeleopV3 extends LinearOpMode {
|
||||
|
||||
}
|
||||
|
||||
if (autoSpintake) {
|
||||
|
||||
if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
|
||||
|
||||
|
||||
robot.spin1.setPosition(spindexPos);
|
||||
robot.spin2.setPosition(1-spindexPos);
|
||||
|
||||
}
|
||||
|
||||
if (gamepad1.right_bumper) {
|
||||
|
||||
shootAll = false;
|
||||
|
||||
intakeTicker++;
|
||||
|
||||
// if (intakeTicker % 20 < 2) {
|
||||
// if (autoSpintake) {
|
||||
//
|
||||
// robot.spin1.setPower(-1);
|
||||
// robot.spin2.setPower(1);
|
||||
// if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
|
||||
//
|
||||
//
|
||||
// robot.spin1.setPosition(spindexPos);
|
||||
// robot.spin2.setPosition(1-spindexPos);
|
||||
//
|
||||
// } else if (intakeTicker % 20 < 10) {
|
||||
// robot.spin1.setPower(-0.5);
|
||||
// robot.spin2.setPower(0.5);
|
||||
// } else if (intakeTicker % 20 < 12) {
|
||||
// robot.spin1.setPower(1);
|
||||
// robot.spin2.setPower(-1);
|
||||
// } else {
|
||||
// robot.spin1.setPower(0.5);
|
||||
// robot.spin2.setPower(-0.5);
|
||||
// }
|
||||
|
||||
robot.intake.setPower(1);
|
||||
intakeStamp = getRuntime();
|
||||
TELE.addData("Reverse?", reverse);
|
||||
TELE.update();
|
||||
} else {
|
||||
if (!servo.spinEqual(spindexPos)) {
|
||||
robot.spin1.setPosition(spindexPos);
|
||||
robot.spin2.setPosition(1-spindexPos);
|
||||
|
||||
}
|
||||
|
||||
spindexPos = spindexer_intakePos1;
|
||||
|
||||
robot.intake.setPower(0);
|
||||
|
||||
intakeTicker = 0;
|
||||
}
|
||||
}
|
||||
//
|
||||
// if (gamepad1.right_bumper) {
|
||||
//
|
||||
// shootAll = false;
|
||||
//
|
||||
// intakeTicker++;
|
||||
//
|
||||
//// if (intakeTicker % 20 < 2) {
|
||||
////
|
||||
//// robot.spin1.setPower(-1);
|
||||
//// robot.spin2.setPower(1);
|
||||
////
|
||||
//// } else if (intakeTicker % 20 < 10) {
|
||||
//// robot.spin1.setPower(-0.5);
|
||||
//// robot.spin2.setPower(0.5);
|
||||
//// } else if (intakeTicker % 20 < 12) {
|
||||
//// robot.spin1.setPower(1);
|
||||
//// robot.spin2.setPower(-1);
|
||||
//// } else {
|
||||
//// robot.spin1.setPower(0.5);
|
||||
//// robot.spin2.setPower(-0.5);
|
||||
//// }
|
||||
//
|
||||
// robot.intake.setPower(1);
|
||||
// intakeStamp = getRuntime();
|
||||
// TELE.addData("Reverse?", reverse);
|
||||
// TELE.update();
|
||||
// } else {
|
||||
// if (!servo.spinEqual(spindexPos)) {
|
||||
// robot.spin1.setPosition(spindexPos);
|
||||
// robot.spin2.setPosition(1-spindexPos);
|
||||
//
|
||||
// }
|
||||
//
|
||||
// spindexPos = spindexer_intakePos1;
|
||||
//
|
||||
// robot.intake.setPower(0);
|
||||
//
|
||||
// intakeTicker = 0;
|
||||
// }
|
||||
// }
|
||||
|
||||
//COLOR:
|
||||
|
||||
@@ -582,58 +601,96 @@ public class TeleopV3 extends LinearOpMode {
|
||||
// }
|
||||
|
||||
if (enableSpindexerManager) {
|
||||
if (!shootAll) {
|
||||
//if (!shootAll) {
|
||||
spindexer.processIntake();
|
||||
}
|
||||
//}
|
||||
|
||||
// RIGHT_BUMPER
|
||||
if (gamepad1.right_bumper) {
|
||||
if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
|
||||
robot.intake.setPower(1);
|
||||
|
||||
} else {
|
||||
robot.intake.setPower(0);
|
||||
|
||||
}
|
||||
|
||||
// LEFT_BUMPER
|
||||
if (!shootAll &&
|
||||
(gamepad1.leftBumperWasReleased() ||
|
||||
gamepad1.leftBumperWasPressed() ||
|
||||
gamepad1.left_bumper)) {
|
||||
if (!shootAll && gamepad1.leftBumperWasReleased()) {
|
||||
shootStamp = getRuntime();
|
||||
shootAll = true;
|
||||
|
||||
shooterTicker = 0;
|
||||
spindexPos = spinStartPos;// TODO: Change starting position based on desired order to shoot green ball
|
||||
|
||||
}
|
||||
|
||||
intakeTicker++;
|
||||
if (shootAll) {
|
||||
|
||||
intakeTicker = 0;
|
||||
intake = false;
|
||||
reject = false;
|
||||
|
||||
if (getRuntime() - shootStamp < 3.5) {
|
||||
// if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) {
|
||||
//
|
||||
// if (shooterTicker == 0){
|
||||
// robot.transferServo.setPosition(transferServo_out);
|
||||
// robot.spin1.setPosition(spinStartPos);
|
||||
// robot.spin2.setPosition(1-spinStartPos);
|
||||
// if (servo.spinEqual(spinStartPos)){
|
||||
// shooterTicker++;
|
||||
// }
|
||||
// TELE.addLine("starting to shoot");
|
||||
// } else {
|
||||
// robot.transferServo.setPosition(transferServo_in);
|
||||
// shooterTicker++;
|
||||
// double prevSpinPos = servo.getSpinPos();
|
||||
// robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease);
|
||||
// robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
|
||||
// TELE.addLine("shooting");
|
||||
// }
|
||||
|
||||
if (shooterTicker == 0 && !servo.spinEqual(spindexPos)){
|
||||
robot.spin1.setPosition(spindexPos);
|
||||
robot.spin2.setPosition(1-spindexPos);
|
||||
} else {
|
||||
// //robot.intake.setPower(-0.3);
|
||||
// if (getRuntime() - shootStamp < 3.0) {
|
||||
//
|
||||
// if (shooterTicker == 0 && !servo.spinEqual(ServoPositions.shootAllAutoSpinStartPos)) {
|
||||
// robot.spin1.setPosition(ServoPositions.shootAllAutoSpinStartPos);
|
||||
// robot.spin2.setPosition(1 - ServoPositions.shootAllAutoSpinStartPos);
|
||||
// } else {
|
||||
// shooterTicker++;
|
||||
// //robot.intake.setPower(0.0);
|
||||
// robot.transferServo.setPosition(transferServo_in);
|
||||
// double prevSpinPos = robot.spin1.getPosition();
|
||||
// robot.spin1.setPosition(prevSpinPos + ServoPositions.shootAllSpindexerSpeedIncrease);
|
||||
// robot.spin2.setPosition(1 - prevSpinPos - ServoPositions.shootAllAutoSpinStartPos);
|
||||
// }
|
||||
//
|
||||
// } else {
|
||||
// robot.transferServo.setPosition(transferServo_out);
|
||||
// //spindexPos = spindexer_intakePos1;
|
||||
// shootAll = false;
|
||||
// spindexer.resetSpindexer();
|
||||
// spindexer.processIntake();
|
||||
//
|
||||
// }
|
||||
|
||||
|
||||
if (shooterTicker == 0) {
|
||||
spindexer.prepareShootAll();
|
||||
TELE.addLine("preparing to shoot");
|
||||
} else if (shooterTicker == 2) {
|
||||
//robot.transferServo.setPosition(transferServo_in);
|
||||
spindexer.shootAll();
|
||||
TELE.addLine("starting to shoot");
|
||||
} else if (!spindexer.shootAllComplete()) {
|
||||
robot.transferServo.setPosition(transferServo_in);
|
||||
shooterTicker++;
|
||||
double prevSpinPos = robot.spin1.getPosition();
|
||||
robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease);
|
||||
robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
|
||||
}
|
||||
|
||||
TELE.addLine("shoot");
|
||||
} else {
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
//spindexPos = spindexer_intakePos1;
|
||||
|
||||
shootAll = false;
|
||||
|
||||
spindexer.resetSpindexer();
|
||||
spindexer.processIntake();
|
||||
//spindexer.processIntake();
|
||||
TELE.addLine("stop shooting");
|
||||
}
|
||||
shooterTicker++;
|
||||
//spindexer.processIntake();
|
||||
}
|
||||
|
||||
if (gamepad1.left_stick_button){
|
||||
@@ -818,42 +875,40 @@ public class TeleopV3 extends LinearOpMode {
|
||||
hub.clearBulkCache();
|
||||
}
|
||||
//
|
||||
TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
|
||||
TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
|
||||
TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
|
||||
|
||||
TELE.addData("pose", drive.localizer.getPose());
|
||||
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
|
||||
TELE.addData("distanceToGoal", distanceToGoal);
|
||||
TELE.addData("hood", robot.hood.getPosition());
|
||||
TELE.addData("targetVel", vel);
|
||||
TELE.addData("Velocity", flywheel.getVelo());
|
||||
TELE.addData("Velo1", flywheel.velo1);
|
||||
TELE.addData("Velo2", flywheel.velo2);
|
||||
TELE.addData("shootOrder", shootOrder);
|
||||
TELE.addData("oddColor", oddBallColor);
|
||||
|
||||
// Spindexer Debug
|
||||
// TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
|
||||
// TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
|
||||
// TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
|
||||
//
|
||||
// TELE.addData("pose", drive.localizer.getPose());
|
||||
// TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
|
||||
// TELE.addData("distanceToGoal", distanceToGoal);
|
||||
// TELE.addData("hood", robot.hood.getPosition());
|
||||
// TELE.addData("targetVel", vel);
|
||||
// TELE.addData("Velocity", flywheel.getVelo());
|
||||
// TELE.addData("Velo1", flywheel.velo1);
|
||||
// TELE.addData("Velo2", flywheel.velo2);
|
||||
// TELE.addData("shootOrder", shootOrder);
|
||||
// TELE.addData("oddColor", oddBallColor);
|
||||
//
|
||||
// // Spindexer Debug
|
||||
TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1));
|
||||
TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition);
|
||||
TELE.addData("spinIntakeState", spindexer.currentIntakeState);
|
||||
TELE.addData("spinTestCounter", spindexer.counter);
|
||||
TELE.addData("autoSpintake", autoSpintake);
|
||||
//TELE.addData("distanceRearCenter", spindexer.distanceRearCenter);
|
||||
//TELE.addData("distanceFrontDriver", spindexer.distanceFrontDriver);
|
||||
//TELE.addData("distanceFrontPassenger", spindexer.distanceFrontPassenger);
|
||||
TELE.addData("shootall commanded", shootAll);
|
||||
// Targeting Debug
|
||||
TELE.addData("robotX", robotX);
|
||||
TELE.addData("robotY", robotY);
|
||||
TELE.addData("robotInchesX", targeting.robotInchesX);
|
||||
TELE.addData( "robotInchesY", targeting.robotInchesY);
|
||||
TELE.addData("Targeting Interpolate", turretInterpolate);
|
||||
//
|
||||
// TELE.addData("shootall commanded", shootAll);
|
||||
// // Targeting Debug
|
||||
// TELE.addData("robotX", robotX);
|
||||
// TELE.addData("robotY", robotY);
|
||||
// TELE.addData("robotInchesX", targeting.robotInchesX);
|
||||
// TELE.addData( "robotInchesY", targeting.robotInchesY);
|
||||
// TELE.addData("Targeting Interpolate", turretInterpolate);
|
||||
TELE.addData("Targeting GridX", targeting.robotGridX);
|
||||
TELE.addData("Targeting GridY", targeting.robotGridY);
|
||||
TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
|
||||
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
|
||||
TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
|
||||
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
|
||||
|
||||
TELE.update();
|
||||
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
package org.firstinspires.ftc.teamcode.tests;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Color.colorFilterAlpha;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
@@ -18,6 +20,9 @@ public class ColorTest extends LinearOpMode {
|
||||
public void runOpMode() throws InterruptedException {
|
||||
robot = new Robot(hardwareMap);
|
||||
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
double color1Distance = 0;
|
||||
double color2Distance = 0;
|
||||
double color3Distance = 0;
|
||||
|
||||
waitForStart();
|
||||
if (isStopRequested()) return;
|
||||
@@ -26,28 +31,34 @@ public class ColorTest extends LinearOpMode {
|
||||
double green1 = robot.color1.getNormalizedColors().green;
|
||||
double blue1 = robot.color1.getNormalizedColors().blue;
|
||||
double red1 = robot.color1.getNormalizedColors().red;
|
||||
double dist1 = robot.color1.getDistance(DistanceUnit.MM);
|
||||
color1Distance = (colorFilterAlpha * dist1) + ((1-colorFilterAlpha) * color1Distance);
|
||||
|
||||
TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
|
||||
TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
|
||||
TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM));
|
||||
TELE.addData("Color1 distance (mm)", color1Distance);
|
||||
|
||||
// ----- COLOR 2 -----
|
||||
double green2 = robot.color2.getNormalizedColors().green;
|
||||
double blue2 = robot.color2.getNormalizedColors().blue;
|
||||
double red2 = robot.color2.getNormalizedColors().red;
|
||||
double dist2 = robot.color2.getDistance(DistanceUnit.MM);
|
||||
color2Distance = (colorFilterAlpha * dist2) + ((1-colorFilterAlpha) * color2Distance);
|
||||
|
||||
TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor());
|
||||
TELE.addData("Color2 green", green2 / (green2 + blue2 + red2));
|
||||
TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM));
|
||||
TELE.addData("Color2 distance (mm)", color2Distance);
|
||||
|
||||
// ----- COLOR 3 -----
|
||||
double green3 = robot.color3.getNormalizedColors().green;
|
||||
double blue3 = robot.color3.getNormalizedColors().blue;
|
||||
double red3 = robot.color3.getNormalizedColors().red;
|
||||
double dist3 = robot.color3.getDistance(DistanceUnit.MM);
|
||||
color3Distance = (colorFilterAlpha * dist3) + ((1-colorFilterAlpha) * color3Distance);
|
||||
|
||||
TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor());
|
||||
TELE.addData("Color3 green", green3 / (green3 + blue3 + red3));
|
||||
TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM));
|
||||
TELE.addData("Color3 distance (mm)", color3Distance);
|
||||
|
||||
TELE.update();
|
||||
}
|
||||
|
||||
@@ -39,6 +39,11 @@ public class Spindexer {
|
||||
public double distanceFrontDriver = 0.0;
|
||||
public double distanceFrontPassenger = 0.0;
|
||||
|
||||
public double spindexerWiggle = 0.01;
|
||||
|
||||
public double spindexerOuttakeWiggle = 0.01;
|
||||
private double prevPos = 0.0;
|
||||
public double spindexerPosOffset = 0.00;
|
||||
public Types.Motif desiredMotif = Types.Motif.NONE;
|
||||
// For Use
|
||||
enum RotatedBallPositionNames {
|
||||
@@ -70,9 +75,12 @@ public class Spindexer {
|
||||
SHOOT_ALL_READY
|
||||
}
|
||||
|
||||
int shootWaitCount = 0;
|
||||
|
||||
public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
|
||||
public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
|
||||
public int unknownColorDetect = 0;
|
||||
enum BallColor {
|
||||
public enum BallColor {
|
||||
UNKNOWN,
|
||||
GREEN,
|
||||
PURPLE
|
||||
@@ -107,7 +115,9 @@ public class Spindexer {
|
||||
|
||||
|
||||
double[] outakePositions =
|
||||
{spindexer_outtakeBall1, spindexer_outtakeBall2, spindexer_outtakeBall3};
|
||||
{spindexer_outtakeBall1+spindexerPosOffset,
|
||||
spindexer_outtakeBall2+spindexerPosOffset,
|
||||
spindexer_outtakeBall3+spindexerPosOffset};
|
||||
|
||||
double[] intakePositions =
|
||||
{spindexer_intakePos1, spindexer_intakePos2, spindexer_intakePos3};
|
||||
@@ -151,12 +161,15 @@ public class Spindexer {
|
||||
int spindexerBallPos = 0;
|
||||
|
||||
// Read Distances
|
||||
distanceRearCenter = robot.color1.getDistance(DistanceUnit.MM);
|
||||
distanceFrontDriver = robot.color2.getDistance(DistanceUnit.MM);
|
||||
distanceFrontPassenger = robot.color3.getDistance(DistanceUnit.MM);
|
||||
double dRearCenter = robot.color1.getDistance(DistanceUnit.MM);
|
||||
distanceRearCenter = (colorFilterAlpha * dRearCenter) + ((1-colorFilterAlpha) * distanceRearCenter);
|
||||
double dFrontDriver = robot.color2.getDistance(DistanceUnit.MM);
|
||||
distanceFrontDriver = (colorFilterAlpha * dFrontDriver) + ((1-colorFilterAlpha) * distanceFrontDriver);
|
||||
double dFrontPassenger = robot.color3.getDistance(DistanceUnit.MM);
|
||||
distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
|
||||
|
||||
// Position 1
|
||||
if (distanceRearCenter < 43) {
|
||||
if (distanceRearCenter < 60) {
|
||||
|
||||
// Mark Ball Found
|
||||
newPos1Detection = true;
|
||||
@@ -170,17 +183,17 @@ public class Spindexer {
|
||||
double gP = green / (green + red + blue);
|
||||
|
||||
// FIXIT - Add filtering to improve accuracy.
|
||||
if (gP >= 0.4) {
|
||||
ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple
|
||||
if (gP >= 0.38) {
|
||||
ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // green
|
||||
} else {
|
||||
ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // purple
|
||||
ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple
|
||||
}
|
||||
}
|
||||
}
|
||||
// Position 2
|
||||
// Find which ball position this is in the spindexer
|
||||
spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()];
|
||||
if (distanceFrontDriver < 60) {
|
||||
if (distanceFrontDriver < 50) {
|
||||
// reset FoundEmpty because looking for 3 in a row before reset
|
||||
ballPositions[spindexerBallPos].foundEmpty = 0;
|
||||
if (detectFrontColor) {
|
||||
@@ -191,9 +204,9 @@ public class Spindexer {
|
||||
double gP = green / (green + red + blue);
|
||||
|
||||
if (gP >= 0.4) {
|
||||
ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
|
||||
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
|
||||
} else {
|
||||
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // purple
|
||||
ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
|
||||
}
|
||||
}
|
||||
} else {
|
||||
@@ -208,7 +221,7 @@ public class Spindexer {
|
||||
|
||||
// Position 3
|
||||
spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()];
|
||||
if (distanceFrontPassenger < 33) {
|
||||
if (distanceFrontPassenger < 29) {
|
||||
|
||||
// reset FoundEmpty because looking for 3 in a row before reset
|
||||
ballPositions[spindexerBallPos].foundEmpty = 0;
|
||||
@@ -219,10 +232,10 @@ public class Spindexer {
|
||||
|
||||
double gP = green / (green + red + blue);
|
||||
|
||||
if (gP >= 0.4) {
|
||||
ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
|
||||
if (gP >= 0.42) {
|
||||
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
|
||||
} else {
|
||||
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // purple
|
||||
ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
|
||||
}
|
||||
}
|
||||
} else {
|
||||
@@ -246,10 +259,21 @@ public class Spindexer {
|
||||
|
||||
return newPos1Detection;
|
||||
}
|
||||
|
||||
public void moveSpindexerToPos(double pos) {
|
||||
// Has code to unjam spindexer
|
||||
private void moveSpindexerToPos(double pos) {
|
||||
robot.spin1.setPosition(pos);
|
||||
robot.spin2.setPosition(1-pos);
|
||||
double currentPos = servos.getSpinPos();
|
||||
if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
|
||||
// if (currentPos > pos){
|
||||
// robot.spin1.setPosition(servos.getSpinPos() + 0.05);
|
||||
// robot.spin2.setPosition(1 - servos.getSpinPos() - 0.05);
|
||||
// } else {
|
||||
// robot.spin1.setPosition(servos.getSpinPos() - 0.05);
|
||||
// robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05);
|
||||
// }
|
||||
}
|
||||
prevPos = currentPos;
|
||||
}
|
||||
|
||||
public void stopSpindexer() {
|
||||
@@ -278,6 +302,10 @@ public class Spindexer {
|
||||
}
|
||||
}
|
||||
|
||||
public boolean slotIsEmpty(int slot){
|
||||
return !ballPositions[slot].isEmpty;
|
||||
}
|
||||
|
||||
public boolean isFull () {
|
||||
return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty);
|
||||
}
|
||||
@@ -287,7 +315,7 @@ public class Spindexer {
|
||||
case UNKNOWN_START:
|
||||
// For now just set position ONE if UNKNOWN
|
||||
commandedIntakePosition = 0;
|
||||
servos.setSpinPos(intakePositions[0]);
|
||||
moveSpindexerToPos(intakePositions[0]);
|
||||
currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE;
|
||||
break;
|
||||
case UNKNOWN_MOVE:
|
||||
@@ -316,34 +344,32 @@ public class Spindexer {
|
||||
currentIntakeState = Spindexer.IntakeState.FINDNEXT;
|
||||
} else {
|
||||
// Maintain Position
|
||||
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
|
||||
spindexerWiggle *= -1.0;
|
||||
moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
|
||||
}
|
||||
break;
|
||||
case FINDNEXT:
|
||||
// Find Next Open Position and start movement
|
||||
double currentSpindexerPos = servos.getSpinPos();
|
||||
double commandedtravelDistance = 2.0;
|
||||
//double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos);
|
||||
double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos);
|
||||
//if (ballPositions[0].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
|
||||
if (ballPositions[0].isEmpty) {
|
||||
// Position 1
|
||||
commandedIntakePosition = 0;
|
||||
servos.setSpinPos(intakePositions[commandedIntakePosition]);
|
||||
currentIntakeState = Spindexer.IntakeState.MOVING;
|
||||
}
|
||||
//proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
|
||||
proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
|
||||
//if (ballPositions[1].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
|
||||
if (ballPositions[1].isEmpty) {
|
||||
// Position 2
|
||||
commandedIntakePosition = 1;
|
||||
servos.setSpinPos(intakePositions[commandedIntakePosition]);
|
||||
currentIntakeState = Spindexer.IntakeState.MOVING;
|
||||
}
|
||||
//proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
|
||||
proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
|
||||
if (ballPositions[2].isEmpty) {
|
||||
// Position 3
|
||||
commandedIntakePosition = 2;
|
||||
servos.setSpinPos(intakePositions[commandedIntakePosition]);
|
||||
currentIntakeState = Spindexer.IntakeState.MOVING;
|
||||
}
|
||||
if (currentIntakeState != Spindexer.IntakeState.MOVING) {
|
||||
@@ -374,29 +400,29 @@ public class Spindexer {
|
||||
currentIntakeState = Spindexer.IntakeState.FINDNEXT;
|
||||
}
|
||||
// Maintain Position
|
||||
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
|
||||
spindexerWiggle *= -1.0;
|
||||
moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
|
||||
break;
|
||||
|
||||
case SHOOT_ALL_PREP:
|
||||
// We get here with function call to prepareToShootMotif
|
||||
// Stopping when we get to the new position
|
||||
if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
|
||||
currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_READY;
|
||||
} else {
|
||||
commandedIntakePosition = 0;
|
||||
if (!servos.spinEqual(outakePositions[commandedIntakePosition])) {
|
||||
// Keep moving the spindexer
|
||||
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
|
||||
moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
|
||||
}
|
||||
break;
|
||||
|
||||
case SHOOT_ALL_READY:
|
||||
case SHOOT_ALL_READY: // Not used
|
||||
// Double Check Colors
|
||||
//detectBalls(false, false); // Minimize hardware calls
|
||||
if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) {
|
||||
// All ball shot move to intake state
|
||||
currentIntakeState = Spindexer.IntakeState.FINDNEXT;
|
||||
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
|
||||
}
|
||||
// Maintain Position
|
||||
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
|
||||
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
|
||||
break;
|
||||
|
||||
case SHOOTNEXT:
|
||||
@@ -404,23 +430,20 @@ public class Spindexer {
|
||||
if (!ballPositions[0].isEmpty) {
|
||||
// Position 1
|
||||
commandedIntakePosition = 0;
|
||||
servos.setSpinPos(outakePositions[commandedIntakePosition]);
|
||||
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
|
||||
} else if (ballPositions[1].isEmpty) { // Possible error: should it be !ballPosition[1].isEmpty?
|
||||
} else if (!ballPositions[1].isEmpty) {
|
||||
// Position 2
|
||||
commandedIntakePosition = 1;
|
||||
servos.setSpinPos(outakePositions[commandedIntakePosition]);
|
||||
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
|
||||
} else if (ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty?
|
||||
} else if (!ballPositions[2].isEmpty) {
|
||||
// Position 3
|
||||
commandedIntakePosition = 2;
|
||||
servos.setSpinPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
|
||||
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
|
||||
} else {
|
||||
// Empty return to intake state
|
||||
currentIntakeState = IntakeState.FINDNEXT;
|
||||
}
|
||||
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
|
||||
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
|
||||
break;
|
||||
|
||||
case SHOOTMOVING:
|
||||
@@ -429,25 +452,39 @@ public class Spindexer {
|
||||
currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
|
||||
} else {
|
||||
// Keep moving the spindexer
|
||||
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
|
||||
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
|
||||
}
|
||||
break;
|
||||
|
||||
case SHOOTWAIT:
|
||||
double shootWaitMax = 4;
|
||||
// Stopping when we get to the new position
|
||||
if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
|
||||
currentIntakeState = Spindexer.IntakeState.INTAKE;
|
||||
stopSpindexer();
|
||||
//detectBalls(true, false);
|
||||
} else {
|
||||
// Keep moving the spindexer
|
||||
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
|
||||
if (prevIntakeState != currentIntakeState) {
|
||||
if (commandedIntakePosition==2) {
|
||||
shootWaitMax = 5;
|
||||
}
|
||||
shootWaitCount = 0;
|
||||
} else {
|
||||
shootWaitCount++;
|
||||
}
|
||||
// wait 10 cycles
|
||||
if (shootWaitCount > shootWaitMax) {
|
||||
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
|
||||
ballPositions[commandedIntakePosition].isEmpty = true;
|
||||
shootWaitCount = 0;
|
||||
//stopSpindexer();
|
||||
//detectBalls(true, false);
|
||||
}
|
||||
// Keep moving the spindexer
|
||||
spindexerOuttakeWiggle *= -1.01;
|
||||
moveSpindexerToPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle);
|
||||
break;
|
||||
|
||||
default:
|
||||
// Statements to execute if no case matches
|
||||
}
|
||||
|
||||
prevIntakeState = currentIntakeState;
|
||||
//TELE.addData("commandedIntakePosition", commandedIntakePosition);
|
||||
//TELE.update();
|
||||
// Signal a successful intake
|
||||
@@ -476,7 +513,7 @@ public class Spindexer {
|
||||
} else if (ballPositions[1].ballColor == BallColor.GREEN) {
|
||||
return 1;
|
||||
} else {
|
||||
return 3;
|
||||
return 2;
|
||||
}
|
||||
//break;
|
||||
case PPG:
|
||||
@@ -499,6 +536,25 @@ public class Spindexer {
|
||||
commandedIntakePosition = bestFitMotif();
|
||||
}
|
||||
|
||||
public void prepareShootAll(){
|
||||
currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP;
|
||||
}
|
||||
public void shootAll () {
|
||||
ballPositions[0].isEmpty = false;
|
||||
ballPositions[1].isEmpty = false;
|
||||
ballPositions[2].isEmpty = false;
|
||||
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
|
||||
}
|
||||
|
||||
public boolean shootAllComplete ()
|
||||
{
|
||||
return ((currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_PREP) &&
|
||||
(currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) &&
|
||||
(currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) &&
|
||||
(currentIntakeState != Spindexer.IntakeState.SHOOTNEXT) &&
|
||||
(currentIntakeState != Spindexer.IntakeState.SHOOTWAIT));
|
||||
}
|
||||
|
||||
void shootAllToIntake () {
|
||||
currentIntakeState = Spindexer.IntakeState.FINDNEXT;
|
||||
}
|
||||
@@ -506,4 +562,16 @@ public class Spindexer {
|
||||
public void update()
|
||||
{
|
||||
}
|
||||
|
||||
public BallColor GetFrontDriverColor () {
|
||||
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]].ballColor;
|
||||
}
|
||||
|
||||
public BallColor GetFrontPassengerColor () {
|
||||
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]].ballColor;
|
||||
}
|
||||
|
||||
public BallColor GetRearCenterColor () {
|
||||
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -67,19 +67,19 @@ public class Targeting {
|
||||
KNOWNTARGETING[3][4] = new Settings (3100.0, 0.47);
|
||||
KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47);
|
||||
// ROW 4
|
||||
KNOWNTARGETING[4][0] = new Settings (4540.0, 0.1);
|
||||
KNOWNTARGETING[4][1] = new Settings (4541.0, 0.1);
|
||||
KNOWNTARGETING[4][2] = new Settings (4542.0, 0.1);
|
||||
KNOWNTARGETING[4][3] = new Settings (4543.0, 0.1);
|
||||
KNOWNTARGETING[4][4] = new Settings (4544.0, 0.1);
|
||||
KNOWNTARGETING[4][5] = new Settings (4545.0, 0.1);
|
||||
// ROW 1
|
||||
KNOWNTARGETING[5][0] = new Settings (4550.0, 0.1);
|
||||
KNOWNTARGETING[5][1] = new Settings (4551.0, 0.1);
|
||||
KNOWNTARGETING[5][2] = new Settings (4552.0, 0.1);
|
||||
KNOWNTARGETING[5][3] = new Settings (4553.0, 0.1);
|
||||
KNOWNTARGETING[5][4] = new Settings (4554.0, 0.1);
|
||||
KNOWNTARGETING[5][5] = new Settings (4555.0, 0.1);
|
||||
KNOWNTARGETING[4][0] = new Settings (3100, 0.49);
|
||||
KNOWNTARGETING[4][1] = new Settings (3100, 0.49);
|
||||
KNOWNTARGETING[4][2] = new Settings (3100, 0.5);
|
||||
KNOWNTARGETING[4][3] = new Settings (3200, 0.5);
|
||||
KNOWNTARGETING[4][4] = new Settings (3250, 0.49);
|
||||
KNOWNTARGETING[4][5] = new Settings (3300, 0.49);
|
||||
// ROW 5
|
||||
KNOWNTARGETING[5][0] = new Settings (3200, 0.48);
|
||||
KNOWNTARGETING[5][1] = new Settings (3200, 0.48);
|
||||
KNOWNTARGETING[5][2] = new Settings (3300, 0.48);
|
||||
KNOWNTARGETING[5][3] = new Settings (3350, 0.48);
|
||||
KNOWNTARGETING[5][4] = new Settings (3350, 0.48);
|
||||
KNOWNTARGETING[5][5] = new Settings (3350, 0.48);
|
||||
}
|
||||
|
||||
public Targeting()
|
||||
@@ -107,61 +107,81 @@ public class Targeting {
|
||||
|
||||
// Determine if we need to interpolate based on tile position.
|
||||
// if near upper or lower quarter or tile interpolate with next tile.
|
||||
int x0 = 0;
|
||||
int y0 = 0;
|
||||
int x1 = 0;
|
||||
int y1 = 0;
|
||||
// interpolate = false;
|
||||
// if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
|
||||
// (robotGridX < 5) && (robotGridY <5)) {
|
||||
// // +X, +Y
|
||||
// interpolate = true;
|
||||
// x1 = robotGridX + 1;
|
||||
// y1 = robotGridY + 1;
|
||||
// } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
|
||||
// (robotGridX > 0) && (robotGridY > 0)) {
|
||||
// // -X, -Y
|
||||
// interpolate = true;
|
||||
// x1 = robotGridX - 1;
|
||||
// y1 = robotGridY - 1;
|
||||
// } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
|
||||
// (robotGridX < 5) && (robotGridY > 0)) {
|
||||
// // +X, -Y
|
||||
// interpolate = true;
|
||||
// x1 = robotGridX + 1;
|
||||
// y1 = robotGridY - 1;
|
||||
// } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
|
||||
// (robotGridX > 0) && (robotGridY < 5)) {
|
||||
// // -X, +Y
|
||||
// interpolate = true;
|
||||
// x1 = robotGridX - 1;
|
||||
// y1 = robotGridY + 1;
|
||||
// } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) {
|
||||
// // -X, Y
|
||||
// interpolate = true;
|
||||
// x1 = robotGridX - 1;
|
||||
// y1 = robotGridY;
|
||||
// } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) {
|
||||
// // X, -Y
|
||||
// interpolate = true;
|
||||
// x1 = robotGridX;
|
||||
// y1 = robotGridY - 1;
|
||||
// } else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) {
|
||||
// // +X, Y
|
||||
// interpolate = true;
|
||||
// x1 = robotGridX + 1;
|
||||
// y1 = robotGridY;
|
||||
// } else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) {
|
||||
// // X, +Y
|
||||
// interpolate = true;
|
||||
// x1 = robotGridX;
|
||||
// y1 = robotGridY + 1;
|
||||
// }
|
||||
interpolate = false;
|
||||
if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
|
||||
(robotGridX < 5) && (robotGridY <5)) {
|
||||
// +X, +Y
|
||||
interpolate = true;
|
||||
x0 = robotGridX;
|
||||
x1 = robotGridX + 1;
|
||||
y0 = robotGridY;
|
||||
y1 = robotGridY + 1;
|
||||
} else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
|
||||
(robotGridX > 0) && (robotGridY > 0)) {
|
||||
// -X, -Y
|
||||
interpolate = true;
|
||||
x0 = robotGridX - 1;
|
||||
x1 = robotGridX;
|
||||
y0 = robotGridY - 1;
|
||||
y1 = robotGridY;
|
||||
} else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
|
||||
(robotGridX < 5) && (robotGridY > 0)) {
|
||||
// +X, -Y
|
||||
interpolate = true;
|
||||
x0 = robotGridX;
|
||||
x1 = robotGridX + 1;
|
||||
y0 = robotGridY - 1;
|
||||
y1 = robotGridY;
|
||||
} else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
|
||||
(robotGridX > 0) && (robotGridY < 5)) {
|
||||
// -X, +Y
|
||||
interpolate = true;
|
||||
x0 = robotGridX - 1;
|
||||
x1 = robotGridX;
|
||||
y0 = robotGridY;
|
||||
y1 = robotGridY + 1;
|
||||
} else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) {
|
||||
// -X, Y
|
||||
interpolate = true;
|
||||
x0 = robotGridX - 1;
|
||||
x1 = robotGridX;
|
||||
y0 = robotGridY;
|
||||
y1 = robotGridY;
|
||||
} else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) {
|
||||
// X, -Y
|
||||
interpolate = true;
|
||||
x0 = robotGridX;
|
||||
x1 = robotGridX;
|
||||
y0 = robotGridY - 1;
|
||||
y1 = robotGridY;
|
||||
} else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) {
|
||||
// +X, Y
|
||||
interpolate = true;
|
||||
x0 = robotGridX;
|
||||
x1 = robotGridX + 1;
|
||||
y0 = robotGridY;
|
||||
y1 = robotGridY;
|
||||
} else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) {
|
||||
// X, +Y
|
||||
interpolate = true;
|
||||
x0 = robotGridX;
|
||||
x1 = robotGridX;
|
||||
y0 = robotGridY;
|
||||
y1 = robotGridY + 1;
|
||||
} else {
|
||||
interpolate = false;
|
||||
}
|
||||
|
||||
//clamp
|
||||
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
|
||||
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
|
||||
|
||||
// basic search
|
||||
if(!interpolate) {
|
||||
if(true) { //!interpolate) {
|
||||
if ((robotGridY < 6) && (robotGridX <6)) {
|
||||
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
|
||||
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
|
||||
@@ -170,27 +190,29 @@ public class Targeting {
|
||||
} else {
|
||||
|
||||
// bilinear interpolation
|
||||
int x0 = robotGridX;
|
||||
//int x0 = robotGridX;
|
||||
//int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1);
|
||||
int y0 = robotGridY;
|
||||
//int y0 = robotGridY;
|
||||
//int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1);
|
||||
|
||||
double x = (robotInchesX - (x0 * tileSize)) / tileSize;
|
||||
double y = (robotInchesY - (y0 * tileSize)) / tileSize;
|
||||
// double x = (robotInchesX - (x0 * tileSize)) / tileSize;
|
||||
// double y = (robotInchesY - (y0 * tileSize)) / tileSize;
|
||||
|
||||
double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM;
|
||||
double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM;
|
||||
double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM;
|
||||
double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM;
|
||||
|
||||
double angle00 = KNOWNTARGETING[y0][x0].hoodAngle;
|
||||
double angle10 = KNOWNTARGETING[y0][x1].hoodAngle;
|
||||
double angle01 = KNOWNTARGETING[y1][x0].hoodAngle;
|
||||
double angle11 = KNOWNTARGETING[y1][x1].hoodAngle;
|
||||
|
||||
recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11;
|
||||
recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11;
|
||||
// double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM;
|
||||
// double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM;
|
||||
// double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM;
|
||||
// double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM;
|
||||
//
|
||||
// double angle00 = KNOWNTARGETING[y0][x0].hoodAngle;
|
||||
// double angle10 = KNOWNTARGETING[y0][x1].hoodAngle;
|
||||
// double angle01 = KNOWNTARGETING[y1][x0].hoodAngle;
|
||||
// double angle11 = KNOWNTARGETING[y1][x1].hoodAngle;
|
||||
|
||||
// recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11;
|
||||
// recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11;
|
||||
// Average target tiles
|
||||
recommendedSettings.flywheelRPM = (KNOWNTARGETING[x0][y0].flywheelRPM + KNOWNTARGETING[x1][y1].flywheelRPM)/2.0;
|
||||
recommendedSettings.hoodAngle = (KNOWNTARGETING[x0][y0].hoodAngle + KNOWNTARGETING[x1][y1].hoodAngle)/2.0;
|
||||
return recommendedSettings;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -61,12 +61,6 @@ public class Turret {
|
||||
this.TELE = tele;
|
||||
this.robot = rob;
|
||||
this.webcam = cam;
|
||||
webcam.start();
|
||||
if (redAlliance) {
|
||||
webcam.pipelineSwitch(4);
|
||||
} else {
|
||||
webcam.pipelineSwitch(2);
|
||||
}
|
||||
bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D);
|
||||
}
|
||||
|
||||
@@ -119,17 +113,14 @@ public class Turret {
|
||||
}
|
||||
|
||||
public double getTy() {
|
||||
limelightRead();
|
||||
return ty;
|
||||
}
|
||||
|
||||
public double getLimelightX() {
|
||||
limelightRead();
|
||||
return limelightPosX;
|
||||
}
|
||||
|
||||
public double getLimelightY() {
|
||||
limelightRead();
|
||||
return limelightPosY;
|
||||
}
|
||||
|
||||
|
||||
@@ -26,10 +26,10 @@ dependencies {
|
||||
|
||||
implementation 'com.bylazar:fullpanels:1.0.2' //Panels
|
||||
|
||||
implementation "com.acmerobotics.roadrunner:ftc:0.1.25"
|
||||
implementation "com.acmerobotics.roadrunner:core:1.0.1"
|
||||
implementation "com.acmerobotics.roadrunner:actions:1.0.1"
|
||||
implementation "com.acmerobotics.dashboard:dashboard:0.5.1"
|
||||
implementation "com.acmerobotics.roadrunner:ftc:0.1.25" //RR
|
||||
implementation "com.acmerobotics.roadrunner:core:1.0.1" //RR
|
||||
implementation "com.acmerobotics.roadrunner:actions:1.0.1" //RR
|
||||
implementation "com.acmerobotics.dashboard:dashboard:0.5.1" //FTC Dash
|
||||
|
||||
implementation 'org.ftclib.ftclib:core:2.1.1' // FTC LIB
|
||||
|
||||
|
||||
Reference in New Issue
Block a user