KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Compare commits

base: KeshavAnandCode:fab0d6346d097e3c449805c055f2e1eade57af21
Branches Tags
KeshavAnandCode:master KeshavAnandCode:LimelightPID KeshavAnandCode:SpindexerUpgrades KeshavAnandCode:Targeting KeshavAnandCode:SpindexerPosUpdate KeshavAnandCode:SpindexerRefactor KeshavAnandCode:danielv2 KeshavAnandCode:organize/expand KeshavAnandCode:extracontrolhub KeshavAnandCode:daniel
..
compare: KeshavAnandCode:93ed2208a987489beda2dfbddda71ad3d2011a4e
Branches Tags
KeshavAnandCode:master KeshavAnandCode:LimelightPID KeshavAnandCode:SpindexerUpgrades KeshavAnandCode:Targeting KeshavAnandCode:SpindexerPosUpdate KeshavAnandCode:SpindexerRefactor KeshavAnandCode:danielv2 KeshavAnandCode:organize/expand KeshavAnandCode:extracontrolhub KeshavAnandCode:daniel

24 Commits
fab0d6346d ... 93ed2208a9

Author SHA1 Message Date
KeshavAnandCode
93ed2208a9 Added light? 2026-02-12 20:08:53 -06:00
KeshavAnandCode
b9f169b685 stash 2026-02-12 20:06:00 -06:00
KeshavAnandCode
754cb6c622 Merge branch 'danielv5' 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
 2026-02-12 20:03:07 -06:00
KeshavAnandCode
645267d05f Sloth load works 2026-02-12 19:51:50 -06:00
DanTheMan-byte
6edc2075c0 stash 2026-02-12 19:51:33 -06:00
KeshavAnandCode
0d4189e15d Merge remote-tracking branch 'upstream/master' into sloth-load 2026-02-12 19:45:11 -06:00
KeshavAnandCode
c5ff8d37ab before merge 2026-02-12 19:45:07 -06:00
DanTheMan-byte
a0e98c9f69 loop times at 30ms 2026-02-10 22:16:38 -06:00
DanTheMan-byte
d48185d393 Merge branch 'LoopTimeReduction' into danielv5 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
 2026-02-10 20:47:05 -06:00
DanTheMan-byte
cc83872a95 stash before merge 2026-02-10 20:31:40 -06:00
Matt9150
f5b8b2fd89 Tweak PID for Limelight to improve turret Aiming. 2026-02-10 20:29:33 -06:00
DanTheMan-byte
48b5925b15 added loop time fixes 2026-02-10 18:37:06 -06:00
Matt9150
830c2b1481 Further speed up loop times. We are now running 50% faster but need to retune the turret PID and update the shoot all speed everywhere. 2026-02-10 00:17:26 -06:00
DanTheMan-byte
4488fabecf auton progress with errors 2026-02-08 23:02:42 -06:00
DanTheMan-byte
ef1c7b0e6b implemented loop time efficiencies and added turret tracking to shooter test 2026-02-07 19:02:41 -06:00
DanTheMan-byte
50060d3812 @KeshavAnandCode I need your help tomorrow to edit auto actions so they are more sensor based 2026-02-07 17:29:34 -06:00
Matt9150
9f5dcb4343 Reduce calls to the spindexer, transfer and color sensors. Add new MeasuringLoopTimes class to measure min, max and avg loop times. 2026-02-07 16:06:03 -06:00
DanTheMan-byte
de9ce388c4 made it so each cycle was its own void 2026-02-07 16:00:10 -06:00
DanTheMan-byte
0d483f2a63 Progress update: some changes to auto and voltage added to flywheel (tested briefly, need more tests to verify) 2026-02-07 15:48:04 -06:00
DanTheMan-byte
e4dd2147d6 bunch of minor changes plus major change in running auton with customizable settings 2026-02-06 21:39:23 -06:00
DanTheMan-byte
bf8f9c1129 Merge branch 'master' into danielv5 2026-02-06 19:13:11 -06:00
DanTheMan-byte
26c7c779f9 added calibration to auto init 2026-02-03 21:56:29 -06:00
Cal Kestis
203c2d3737 Merge pull request #1944 from FIRST-Tech-Challenge/20251231-104637-release-candidate 
FtcRobotController v11.1
 2026-01-20 12:17:38 -08:00
Cal Kestis
dc995e3d6f FtcRobotController v11.1 2025-12-31 10:54:21 -08:00
34 changed files with 2772 additions and 1290 deletions
Expand all files Collapse all files

4
FtcRobotController/src/main/AndroidManifest.xml
View File

@@ -1,8 +1,8 @@
<?xml version="1.0" encoding="utf-8"?>
<manifest xmlns:android="http://schemas.android.com/apk/res/android"
xmlns:tools="http://schemas.android.com/tools"
android:versionCode="60"
android:versionName="11.0">
android:versionCode="61"
android:versionName="11.1">
<uses-permission android:name="android.permission.RECEIVE_BOOT_COMPLETED" />

2
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java vendored
View File

@@ -92,7 +92,6 @@ public class ConceptAprilTag extends LinearOpMode {
telemetry.update();
waitForStart();
if (opModeIsActive()) {
while (opModeIsActive()) {
telemetryAprilTag();
@@ -110,7 +109,6 @@ public class ConceptAprilTag extends LinearOpMode {
// Share the CPU.
sleep(20);
}
}
// Save more CPU resources when camera is no longer needed.
visionPortal.close();

2
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java vendored
View File

@@ -88,7 +88,6 @@ public class ConceptAprilTagEasy extends LinearOpMode {
telemetry.update();
waitForStart();
if (opModeIsActive()) {
while (opModeIsActive()) {
telemetryAprilTag();
@@ -106,7 +105,6 @@ public class ConceptAprilTagEasy extends LinearOpMode {
// Share the CPU.
sleep(20);
}
}
// Save more CPU resources when camera is no longer needed.
visionPortal.close();

2
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java vendored
View File

@@ -81,7 +81,6 @@ public class ConceptAprilTagSwitchableCameras extends LinearOpMode {
telemetry.update();
waitForStart();
if (opModeIsActive()) {
while (opModeIsActive()) {
telemetryCameraSwitching();
@@ -102,7 +101,6 @@ public class ConceptAprilTagSwitchableCameras extends LinearOpMode {
// Share the CPU.
sleep(20);
}
}
// Save more CPU resources when camera is no longer needed.
visionPortal.close();

16
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadEdgeDetection.java vendored
View File

@@ -83,6 +83,22 @@ public class ConceptGamepadEdgeDetection extends LinearOpMode {
telemetry.addData("Gamepad 1 Right Bumper Released", gamepad1.rightBumperWasReleased());
telemetry.addData("Gamepad 1 Right Bumper Status", gamepad1.right_bumper);
// Add an empty line to separate the buttons in telemetry
telemetry.addLine();
// Add the status of the Gamepad 1 Left trigger
telemetry.addData("Gamepad 1 Left Trigger Pressed", gamepad1.leftTriggerWasPressed());
telemetry.addData("Gamepad 1 Left Trigger Released", gamepad1.leftTriggerWasReleased());
telemetry.addData("Gamepad 1 Left Trigger Status", gamepad1.left_trigger_pressed);
// Add an empty line to separate the buttons in telemetry
telemetry.addLine();
// Add the status of the Gamepad 1 Right trigger
telemetry.addData("Gamepad 1 Right Trigger Pressed", gamepad1.rightTriggerWasPressed());
telemetry.addData("Gamepad 1 Right Trigger Released", gamepad1.rightTriggerWasReleased());
telemetry.addData("Gamepad 1 Right Trigger Status", gamepad1.right_trigger_pressed);
// Add a note that the telemetry is only updated every 2 seconds
telemetry.addLine("\nTelemetry is updated every 2 seconds.");

2
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/ConceptExternalHardwareClass.java vendored
View File

@@ -27,7 +27,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
package org.firstinspires.ftc.robotcontroller.external.samples.externalhardware;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

2
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/RobotHardware.java vendored
View File

@@ -27,7 +27,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
package org.firstinspires.ftc.robotcontroller.external.samples.externalhardware;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;

15
README.md
View File

@@ -59,6 +59,21 @@ The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc
# Release Information
## Version 11.1 (20251231-104637)
### Enhancements
* Gamepad triggers can now be accessed as booleans and have edge detection supported.
* GoBildaPinpointDriver now supports Pinpoint v2 functionality
* Adds webcam calibrations for goBILDA's USB camera.
### Bug Fixes
* Fixes issue [1654](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1654) in GoBildaPinpointDriver that caused error if resolution was set in other than MM
* Fixes issue [1628](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1628) Blocks editor displays incorrect Java code for gamepad edge detection blocks.
* Fixes possible race condition issue [1884](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1884) on Driver Station startup when Driver Station name doesn't match the Robot Controller name.
* Fixes issue [1863](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1863) - Incorrect package paths in samples.
* Fixes an issue where an OnBotJava filename that begins with a lowercase character would fail to properly rename the file if the user tried to rename it so that it begins with an uppercase character.
## Version 11.0 (20250827-105138)
### Enhancements

43
TeamCode/build.gradle
View File

@@ -12,8 +12,25 @@
// Custom definitions may go here
// Include common definitions from above.
buildscript {
repositories {
mavenCentral()
maven {
url "https://repo.dairy.foundation/releases"
}
}
dependencies {
classpath "dev.frozenmilk:Load:0.2.4"
}
}
apply from: '../build.common.gradle'
apply from: '../build.dependencies.gradle'
// there should be 2 or 3 more lines that start with 'apply plugin:' here
apply plugin: 'dev.frozenmilk.sinister.sloth.load'
android {
namespace = 'org.firstinspires.ftc.teamcode'
@@ -28,14 +45,32 @@ repositories {
maven {
url = 'https://maven.brott.dev/'
}
// Dairy releases repository
maven {
url = "https://repo.dairy.foundation/releases"
}
// Dairy snapshots repository
maven {
url = "https://repo.dairy.foundation/snapshots"
}
}
dependencies {
implementation project(':FtcRobotController')
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("dev.frozenmilk.sinister:Sloth:0.2.4")
implementation 'org.ftclib.ftclib:core:2.1.1' // core
implementation("com.acmerobotics.roadrunner:ftc:0.1.25") {
exclude group: "com.acmerobotics.dashboard"
}
implementation("com.acmerobotics.roadrunner:actions:1.0.1") {
exclude group: "com.acmerobotics.dashboard"
}
implementation("com.acmerobotics.roadrunner:core:1.0.1") {
exclude group: "com.acmerobotics.dashboard"
}
implementation("com.acmerobotics.slothboard:dashboard:0.2.4+0.5.1") //Slothdashboard
}

457
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
View File

@@ -1,62 +1,9 @@
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.Front_Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
@@ -64,6 +11,8 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
import androidx.annotation.NonNull;
@@ -82,7 +31,6 @@ 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;
@@ -95,30 +43,30 @@ import java.util.Objects;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
public class Auto_LT_Close extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015;
public static double shoot0SpinSpeedIncrease = 0.02;
public static double spindexerSpeedIncrease = 0.03;
public static double finalSpindexerSpeedIncrease = 0.025;
public static double finalSpindexerSpeedIncrease = 0.03;
// 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;
public static double redObeliskTurrPos1 = turrDefault + 0.12;
public static double redObeliskTurrPos2 = turrDefault + 0.13;
public static double redObeliskTurrPos3 = turrDefault + 0.14;
public static double blueObeliskTurrPos1 = turrDefault - 0.12;
public static double blueObeliskTurrPos2 = turrDefault - 0.13;
public static double blueObeliskTurrPos3 = turrDefault - 0.14;
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0;
public static double normalIntakeTime = 3.3;
public static double shoot1Turr = 0.57;
public static double shoot0XTolerance = 1.0;
public static double redTurretShootPos = turrDefault + 0.12;
public static double blueTurretShootPos = turrDefault - 0.14;
double turretShootPos = 0.0;
public static double finalShootAllTime = 3.0;
@@ -130,7 +78,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
public static double intake3Time = 4.2;
public static double flywheel0Time = 3.5;
public static double pickup1Speed = 23;
public static double pickup1Speed = 15;
// ---- SECOND SHOT / PICKUP ----
public static double shoot1Vel = 2300;
public static double shootAllVelocity = 2500;
@@ -163,21 +111,22 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
Targeting.Settings targetingSettings;
private double firstSpindexShootPos = autoSpinStartPos;
private boolean shootForward = true;
private double x1, y1, h1;
double x1, y1, h1;
private double x2a, y2a, h2a, t2a;
double x2a, y2a, h2a, t2a;
private double x2b, y2b, h2b, t2b;
private double x2c, y2c, h2c, t2c;
double x2b, y2b, h2b, t2b;
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;
double x3a, y3a, h3a;
double x3b, y3b, h3b;
double x4a, y4a, h4a;
double x4b, y4b, h4b;
private double xShoot, yShoot, hShoot;
private double xGate, yGate, hGate;
private double xPrep, yPrep, hPrep;
double xShoot, yShoot, hShoot;
double xGate, yGate, hGate;
double xPrep, yPrep, hPrep;
double xLeave, yLeave, hLeave;
private double shoot1Tangent;
@@ -186,6 +135,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
private int rearSlotGreen = 0;
private int mostGreenSlot = 0;
int ballCycles = 3;
int prevMotif = 0;
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() {
@@ -202,14 +153,29 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
stamp = System.currentTimeMillis();
}
ticker++;
robot.transferServo.setPosition(transferServo_out);
turret.manualSetTurret(turretShootPos);
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);
@@ -219,8 +185,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
spindexerWiggle *= -1.0;
robot.spin1.setPosition(spindexer_intakePos1 + spindexerWiggle);
robot.spin2.setPosition(1 - spindexer_intakePos1 - spindexerWiggle);
servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
spindexer.detectBalls(true, true);
@@ -231,11 +196,12 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) {
passengerSlotGreen++;
}
if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) {
rearSlotGreen++;
}
robot.intake.setPower(1);
spindexer.setIntakePower(1);
decideGreenSlot = true;
@@ -250,8 +216,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
mostGreenSlot = 1;
}
decideGreenSlot = false;
if (motif_id == 21) {
@@ -293,14 +257,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
return true;
} else if ((System.currentTimeMillis() - stamp) < (time * 1000)) {
// TELE.addData("MostGreenSlot", mostGreenSlot);
// TELE.update();
spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
TELE.addData("MostGreenSlot", mostGreenSlot);
robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
robot.spin1.setPosition(firstSpindexShootPos);
robot.spin2.setPosition(1 - firstSpindexShootPos);
servos.setSpinPos(firstSpindexShootPos);
return true;
} else {
@@ -314,11 +275,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
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
@@ -327,6 +285,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
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();
@@ -334,36 +294,51 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
robot.intake.setPower(0);
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (getRuntime() - stamp < shootTime) {
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
servos.setSpinPos(autoSpinStartPos);
} else {
robot.transferServo.setPosition(transferServo_in);
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = robot.spin1.getPosition();
robot.spin1.setPosition(prevSpinPos + spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
double prevSpinPos = servos.getSpinCmdPos();
servos.setSpinPos(prevSpinPos + spindexSpeed);
}
return true;
} else {
robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1;
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
@@ -398,43 +373,57 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
robot.intake.setPower(0);
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if (getRuntime() - stamp < shootTime) {
if (getRuntime() - stamp < firstShootTime) {
robot.transferServo.setPosition(transferServo_in);
robot.spin1.setPosition(firstSpindexShootPos);
robot.spin2.setPosition(1 - firstSpindexShootPos);
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {
robot.transferServo.setPosition(transferServo_in);
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = robot.spin1.getPosition();
double prevSpinPos = servos.getSpinCmdPos();
if (shootForward) {
robot.spin1.setPosition(prevSpinPos + spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else {
robot.spin1.setPosition(prevSpinPos - spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos + spindexSpeed);
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
}
return true;
} else {
robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1;
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
@@ -459,18 +448,17 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
ticker++;
spindexer.processIntake();
robot.intake.setPower(1);
spindexer.setIntakePower(1);
spindexer.ballCounterLight();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.addData("Full?", spindexer.isFull());
TELE.update();
return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
}
};
}
@@ -507,8 +495,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
ticker++;
motif = turret.detectObelisk();
robot.turr1.setPosition(turrPos);
robot.turr2.setPosition(1 - turrPos);
turret.setTurret(turrPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
@@ -568,8 +555,10 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
ticker++;
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
robot.hood.setPosition(hoodPos);
servos.setHoodPos(hoodPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
@@ -637,8 +626,10 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
turret.trackGoal(deltaPose);
robot.hood.setPosition(targetingSettings.hoodAngle);
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;
@@ -706,8 +697,10 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, false);
robot.hood.setPosition(targetingSettings.hoodAngle);
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;
@@ -728,6 +721,15 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
};
}
// initialize path variables here
TrajectoryActionBuilder shoot0 = null;
TrajectoryActionBuilder pickup1 = null;
TrajectoryActionBuilder shoot1 = null;
TrajectoryActionBuilder pickup2 = null;
TrajectoryActionBuilder shoot2 = null;
TrajectoryActionBuilder pickup3 = null;
TrajectoryActionBuilder shoot3 = null;
@Override
public void runOpMode() throws InterruptedException {
@@ -746,37 +748,24 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
servos = new Servos(hardwareMap);
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(turrDefault);
turret.setTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
servos.setSpinPos(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;
servos.setTransferPos(transferServo_out);
limelightUsed = false;
robot.light.setPosition(1);
while (opModeInInit()) {
robot.hood.setPosition(shoot0Hood);
turret.manualSetTurret(turrDefault);
servos.setHoodPos(shoot0Hood);
turret.setTurret(turrDefault);
if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance;
@@ -790,18 +779,18 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
turrDefault += 0.01;
}
redObeliskTurrPos1 = turrDefault + 0.12;
redObeliskTurrPos2 = turrDefault + 0.13;
redObeliskTurrPos3 = turrDefault + 0.14;
blueObeliskTurrPos1 = turrDefault - 0.12;
blueObeliskTurrPos2 = turrDefault - 0.13;
blueObeliskTurrPos3 = turrDefault - 0.14;
redTurretShootPos = turrDefault + 0.12;
blueTurretShootPos = turrDefault - 0.14;
if (gamepad2.rightBumperWasPressed()){
ballCycles++;
}
if (gamepad2.leftBumperWasPressed()){
ballCycles--;
}
if (gamepad2.squareWasPressed()){
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
gamepad2.rumble(500);
}
if (redAlliance) {
robot.light.setPosition(0.28);
@@ -836,11 +825,14 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
xShoot = rShootX;
yShoot = rShootY;
hShoot = rShootH;
xLeave = rLeaveX;
yLeave = rLeaveY;
hLeave = rLeaveH;
obeliskTurrPos1 = redObeliskTurrPos1;
obeliskTurrPos2 = redObeliskTurrPos2;
obeliskTurrPos3 = redObeliskTurrPos3;
turretShootPos = redTurretShootPos;
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
turretShootPos = turrDefault + redTurretShootPos;
} else {
robot.light.setPosition(0.6);
@@ -876,44 +868,57 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
xShoot = bShootX;
yShoot = bShootY;
hShoot = bShootH;
xLeave = bLeaveX;
yLeave = bLeaveY;
hLeave = bLeaveH;
obeliskTurrPos1 = blueObeliskTurrPos1;
obeliskTurrPos2 = blueObeliskTurrPos2;
obeliskTurrPos3 = blueObeliskTurrPos3;
turretShootPos = blueTurretShootPos;
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
turretShootPos = turrDefault + blueTurretShootPos;
}
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
.strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x2a, y2a), h2a)
.strafeToLinearHeading(new Vector2d(x2b, y2b), h2b,
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
new TranslationalVelConstraint(pickup1Speed));
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
if (ballCycles < 2){
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
} else {
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
}
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
.strafeToLinearHeading(new Vector2d(x3a, y3a), h3a)
.strafeToLinearHeading(new Vector2d(x3b, y3b), h3b,
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
.strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
new TranslationalVelConstraint(pickup1Speed));
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
if (ballCycles < 3){
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
} else {
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hLeave));
}
pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x4a, y4a), h4a)
.strafeToLinearHeading(new Vector2d(x4b, y4b), h4b,
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(x4a, y4a), Math.toRadians(h4a))
.strafeToLinearHeading(new Vector2d(x4b, y4b), Math.toRadians(h4b),
new TranslationalVelConstraint(pickup1Speed));
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, Math.toRadians(h4b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.addData("Ball Cycles", ballCycles);
TELE.update();
}
@@ -926,6 +931,37 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
robot.transfer.setPower(1);
startAuto();
if (ballCycles > 0){
cycleStackClose();
}
if (ballCycles > 1){
cycleStackMiddle();
}
if (ballCycles > 2){
cycleStackFar();
}
while (opModeIsActive()) {
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
flywheel.manageFlywheel(0);
TELE.addLine("finished");
TELE.update();
}
}
}
void startAuto() {
assert shoot0 != null;
Actions.runBlocking(
@@ -947,7 +983,9 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
Actions.runBlocking(
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
);
}
void cycleStackClose(){
Actions.runBlocking(
new ParallelAction(
pickup1.build(),
@@ -955,8 +993,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
shootAllVelocity,
shootAllHood,
intake1Time,
x2b,
y2b,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
),
@@ -975,9 +1013,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
motif = turret.getObeliskID();
if (motif == 0) motif = 22;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
@@ -1009,14 +1046,16 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
)
);
}
void cycleStackMiddle(){
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
manageShooterAuto(
intake2Time,
x2b,
y2b,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
),
@@ -1033,6 +1072,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
)
);
motif = turret.getObeliskID();
if (motif == 0) motif = prevMotif;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
manageFlywheelAuto(
@@ -1060,14 +1104,16 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
)
);
}
void cycleStackFar(){
Actions.runBlocking(
new ParallelAction(
pickup3.build(),
manageShooterAuto(
intake3Time,
x2b,
y2b,
0.501,
0.501,
pickup1XTolerance,
pickup1YTolerance
),
@@ -1084,6 +1130,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
)
);
motif = turret.getObeliskID();
if (motif == 0) motif = prevMotif;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
manageFlywheelAuto(
@@ -1111,17 +1162,5 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addLine("finished");
TELE.update();
sleep(2000);
}
}
}

938
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java Normal file
View File

@@ -0,0 +1,938 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
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.hoodOffset;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ParallelAction;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting;
import org.firstinspires.ftc.teamcode.utils.Turret;
import java.util.Objects;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Far extends LinearOpMode {
public static double shoot0Vel = 3300, shoot0Hood = 0.48 + hoodOffset;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015;
public static double shoot0XTolerance = 1.0;
public static double redTurretShootPos = 0.05;
public static double blueTurretShootPos = -0.05;
public static int fwdTime = 200, strafeTime = 2300;
double xLeave, yLeave, hLeave;
public static int sleepTime = 1300;
public int motif = 0;
double turretShootPos = 0.0;
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
Servos servos;
Spindexer spindexer;
Flywheel flywheel;
Turret turret;
Targeting targeting;
Targeting.Settings targetingSettings;
double firstSpindexShootPos = autoSpinStartPos;
boolean shootForward = true;
double xShoot, yShoot, hShoot;
private int driverSlotGreen = 0;
private int passengerSlotGreen = 0;
int rearSlotGreen = 0;
int mostGreenSlot = 0;
double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
public static double firstShootTime = 0.3;
public static double flywheel0Time = 3.5;
public static double shoot0Time = 2;
boolean gatePickup = false;
boolean stack3 = true;
double xStackPickupA, yStackPickupA, hStackPickupA;
double xStackPickupB, yStackPickupB, hStackPickupB;
public static int pickupStackSpeed = 15;
int prevMotif = 0;
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
double spindexerWiggle = 0.01;
boolean decideGreenSlot = false;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
servos.setTransferPos(transferServo_out);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.addData("motif", motif_id);
TELE.update();
if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
spindexerWiggle *= -1.0;
servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
spindexer.detectBalls(true, true);
if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) {
driverSlotGreen++;
}
if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) {
passengerSlotGreen++;
}
if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) {
rearSlotGreen++;
}
spindexer.setIntakePower(1);
decideGreenSlot = true;
return true;
} else if (decideGreenSlot) {
if (driverSlotGreen >= passengerSlotGreen && driverSlotGreen >= rearSlotGreen) {
mostGreenSlot = 3;
} else if (passengerSlotGreen >= driverSlotGreen && passengerSlotGreen >= rearSlotGreen) {
mostGreenSlot = 2;
} else {
mostGreenSlot = 1;
}
decideGreenSlot = false;
if (motif_id == 21) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
} else {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
}
} else if (motif_id == 22) {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
} else {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = true;
}
} else {
if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
} else if (mostGreenSlot == 2) {
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
} else {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
}
}
return true;
} else if ((System.currentTimeMillis() - stamp) < (time * 1000)) {
// TELE.addData("MostGreenSlot", mostGreenSlot);
// TELE.update();
spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
servos.setSpinPos(firstSpindexShootPos);
return true;
} else {
return false;
}
}
};
}
public Action shootAll(int vel, double shootTime, double spindexSpeed) {
return new Action() {
int ticker = 1;
double stamp = 0.0;
double velo = vel;
int shooterTicker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
servos.setSpinPos(autoSpinStartPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = servos.getSpinCmdPos();
servos.setSpinPos(prevSpinPos + spindexSpeed);
}
return true;
} else {
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
return false;
}
}
};
}
public Action shootAllAuto(double shootTime, double spindexSpeed) {
return new Action() {
int ticker = 1;
double stamp = 0.0;
double velo = 0.0;
int shooterTicker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
spindexer.setIntakePower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
spindexer.setIntakePower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robX, robY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
if (getRuntime() - stamp < shootTime) {
if (getRuntime() - stamp < firstShootTime) {
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
double prevSpinPos = servos.getSpinCmdPos();
if (shootForward) {
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else {
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
}
return true;
} else {
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
return false;
}
}
};
}
public Action intake(double intakeTime) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
spindexer.processIntake();
spindexer.setIntakePower(1);
spindexer.ballCounterLight();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Full?", spindexer.isFull());
TELE.update();
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
}
};
}
public Action detectObelisk(
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance,
double turrPos
) {
boolean timeFallback = (time != 0.501);
boolean posXFallback = (posX != 0.501);
boolean posYFallback = (posY != 0.501);
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
robot.limelight.pipelineSwitch(1);
}
ticker++;
motif = turret.detectObelisk();
turret.setTurret(turrPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
if (shouldFinish){
if (redAlliance){
robot.limelight.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
}
return false;
} else {
return true;
}
}
};
}
public Action manageFlywheel(
double vel,
double hoodPos,
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance
) {
boolean timeFallback = (time != 0.501);
boolean posXFallback = (posX != 0.501);
boolean posYFallback = (posY != 0.501);
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
servos.setHoodPos(hoodPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return !shouldFinish;
}
};
}
public Action manageShooterAuto(
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance
) {
boolean timeFallback = (time != 0.501);
boolean posXFallback = (posX != 0.501);
boolean posYFallback = (posY != 0.501);
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
double robotX = drive.localizer.getPose().position.x;
double robotY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robotX - goalX; // delta x from robot to goal
double dy = robotY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, false);
turret.trackGoal(deltaPose);
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return !shouldFinish;
}
};
}
public Action manageFlywheelAuto(
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance
) {
boolean timeFallback = (time != 0.501);
boolean posXFallback = (posX != 0.501);
boolean posYFallback = (posY != 0.501);
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
double robotX = drive.localizer.getPose().position.x;
double robotY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robotX - goalX; // delta x from robot to goal
double dy = robotY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, false);
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return !shouldFinish;
}
};
}
// initialize path variables here
TrajectoryActionBuilder leave3Ball = null;
TrajectoryActionBuilder leaveFromShoot = null;
TrajectoryActionBuilder pickup3 = null;
TrajectoryActionBuilder shoot3 = null;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
flywheel = new Flywheel(hardwareMap);
targeting = new Targeting();
targetingSettings = new Targeting.Settings(0.0, 0.0);
spindexer = new Spindexer(hardwareMap);
servos = new Servos(hardwareMap);
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
turret = new Turret(robot, TELE, robot.limelight);
turret.setTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, autoStart);
servos.setSpinPos(autoSpinStartPos);
servos.setTransferPos(transferServo_out);
while (opModeInInit()) {
// Recalibration in initialization
drive.updatePoseEstimate();
if (gamepad2.triangle) {
autoStart = drive.localizer.getPose(); // use this position as starting position
gamepad2.rumble(1000);
}
if (gamepad2.squareWasPressed()){
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
gamepad2.rumble(500);
}
if (gamepad2.leftBumperWasPressed()){
gatePickup = !gatePickup;
}
if (gamepad2.rightBumperWasPressed()){
stack3 = !stack3;
}
turret.setTurret(turretShootPos);
robot.hood.setPosition(shoot0Hood);
if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance;
}
if (gamepad2.dpadLeftWasPressed()) {
turrDefault -= 0.01;
}
if (gamepad2.dpadRightWasPressed()) {
turrDefault += 0.01;
}
if (redAlliance) {
robot.light.setPosition(0.28);
xLeave = rLeaveX;
yLeave = rLeaveY;
hLeave = rLeaveH;
xShoot = rShootX;
yShoot = rShootY;
hShoot = rShootH;
xStackPickupA = rStackPickupAX;
yStackPickupA = rStackPickupAY;
hStackPickupA = rStackPickupAH;
xStackPickupB = rStackPickupBX;
yStackPickupB = rStackPickupBY;
hStackPickupB = rStackPickupBH;
turretShootPos = turrDefault + redTurretShootPos;
} else {
robot.light.setPosition(0.6);
xLeave = bLeaveX;
yLeave = bLeaveY;
hLeave = bLeaveH;
xShoot = bShootX;
yShoot = bShootY;
hShoot = bShootH;
xStackPickupA = bStackPickupAX;
yStackPickupA = bStackPickupAY;
hStackPickupA = bStackPickupAH;
xStackPickupB = bStackPickupBX;
yStackPickupB = bStackPickupBY;
hStackPickupB = bStackPickupBH;
turretShootPos = turrDefault + blueTurretShootPos;
}
leave3Ball = drive.actionBuilder(autoStart)
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
leaveFromShoot = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA))
.strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB),
new TranslationalVelConstraint(pickupStackSpeed));
shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.addData("Gate Cycle?", gatePickup);
TELE.addData("Pickup Stack?", stack3);
TELE.addData("Start Position", autoStart);
TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
// Currently only shoots; keep this start and modify times and then add extra paths
if (opModeIsActive()) {
double stamp = getRuntime();
robot.transfer.setPower(1);
startAuto();
if (stack3){
//cycleStackFar();
}
if (gatePickup || stack3){
leave();
} else {
leave3Ball();
}
// Actual way to end autonomous in to find final position
while (opModeIsActive()) {
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
flywheel.manageFlywheel(0);
TELE.addLine("finished");
TELE.update();
}
}
}
void startAuto(){
Actions.runBlocking(
new ParallelAction(
manageFlywheel(
shoot0Vel,
shoot0Hood,
flywheel0Time,
0.501,
0.501,
shoot0XTolerance,
0.501
)
)
);
Actions.runBlocking(
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
);
}
void leave3Ball(){
assert leave3Ball != null;
Actions.runBlocking(leave3Ball.build());
}
void leave(){
assert leaveFromShoot != null;
Actions.runBlocking(leaveFromShoot.build());
}
// void cycleStackFar(){
// Actions.runBlocking(
// new ParallelAction(
// pickup3.build(),
// manageShooterAuto(
// intake3Time,
// 0.501,
// 0.501,
// 0.501,
// 0.501
// ),
// intake(intake3Time),
// detectObelisk(
// intake3Time,
// 0.501,
// 0.501,
// 0.501,
// 0.501,
// obeliskTurrPos3
// )
//
// )
// );
//
// motif = turret.getObeliskID();
//
// if (motif == 0) motif = prevMotif;
// prevMotif = motif;
//
// Actions.runBlocking(
// new ParallelAction(
// manageFlywheelAuto(
// shoot3Time,
// 0.501,
// 0.501,
// 0.501,
// 0.501
// ),
// shoot3.build(),
// prepareShootAll(colorSenseTime, shoot3Time, motif)
// )
// );
//
// Actions.runBlocking(
// new ParallelAction(
// manageShooterAuto(
// finalShootAllTime,
// 0.501,
// 0.501,
// 0.501,
// 0.501
// ),
// shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
// )
//
// );
// }
}

403
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java
View File

@@ -1,403 +0,0 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ParallelAction;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting;
import org.firstinspires.ftc.teamcode.utils.Turret;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Far_3Ball extends LinearOpMode {
public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015;
public static double redObeliskTurrPos1 = turrDefault + 0.12;
public static double redObeliskTurrPos2 = turrDefault + 0.13;
public static double redObeliskTurrPos3 = turrDefault + 0.14;
public static double blueObeliskTurrPos1 = turrDefault - 0.12;
public static double blueObeliskTurrPos2 = turrDefault - 0.13;
public static double blueObeliskTurrPos3 = turrDefault - 0.14;
public static double shoot0XTolerance = 1.0;
public static double redTurretShootPos = turrDefault;
public static double blueTurretShootPos = turrDefault;
public static int fwdTime = 200, strafeTime = 2300;
public static double rPickupGateX = 1, rPickupGateY = 1, rPickupGateH = 1;
public static double rPickupZoneX = 1, rPickupZoneY = 1, rPickupZoneH = 1;
public static double rShootX = 1, rShootY = 1, rShootH = 1;
public static double bPickupZoneX = 1, bPickupZoneY = 1, bPickupZoneH = 1;
public static double bPickupGateX = 1, bPickupGateY = 1, bPickupGateH = 1;
public static double bShootX = 1, bShootY = 1, bShootH = 1;
public static int sleepTime = 1300;
public int motif = 0;
double turretShootPos = 0.0;
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
Servos servos;
Spindexer spindexer;
Flywheel flywheel;
Turret turret;
Targeting targeting;
Targeting.Settings targetingSettings;
private double firstSpindexShootPos = autoSpinStartPos;
private boolean shootForward = true;
private double xShoot, yShoot, hShoot;
private int driverSlotGreen = 0;
private int passengerSlotGreen = 0;
private int rearSlotGreen = 0;
private int mostGreenSlot = 0;
private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
public Action shootAll(int vel, double shootTime, double spindexSpeed) {
return new Action() {
int ticker = 1;
double stamp = 0.0;
double velo = vel;
int shooterTicker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
robot.intake.setPower(-0.3);
if (ticker == 1) {
stamp = getRuntime();
}
ticker++;
robot.intake.setPower(0);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (getRuntime() - stamp < shootTime) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
} else {
robot.transferServo.setPosition(transferServo_in);
shooterTicker++;
double prevSpinPos = robot.spin1.getPosition();
robot.spin1.setPosition(prevSpinPos + spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
}
return true;
} else {
robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1;
spindexer.resetSpindexer();
spindexer.processIntake();
return false;
}
}
};
}
public Action intake(double intakeTime) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
spindexer.processIntake();
robot.intake.setPower(1);
spindexer.ballCounterLight();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
}
};
}
public Action manageFlywheel(
double vel,
double hoodPos,
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance
) {
boolean timeFallback = (time != 0.501);
boolean posXFallback = (posX != 0.501);
boolean posYFallback = (posY != 0.501);
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
}
ticker++;
flywheel.manageFlywheel(vel);
robot.hood.setPosition(hoodPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || xDone || yDone;
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Hood", robot.hood.getPosition());
TELE.update();
return !shouldFinish;
}
};
}
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
flywheel = new Flywheel(hardwareMap);
targeting = new Targeting();
targetingSettings = new Targeting.Settings(0.0, 0.0);
spindexer = new Spindexer(hardwareMap);
servos = new Servos(hardwareMap);
robot.limelight.start();
robot.limelight.pipelineSwitch(1);
turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, teleEnd);
robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
robot.transferServo.setPosition(transferServo_out);
robot.light.setPosition(1);
while (opModeInInit()) {
robot.hood.setPosition(shoot0Hood);
turret.manualSetTurret(turretShootPos);
if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance;
}
if (gamepad2.dpadLeftWasPressed()) {
turrDefault -= 0.01;
}
if (gamepad2.dpadRightWasPressed()) {
turrDefault += 0.01;
}
redObeliskTurrPos1 = turrDefault + 0.12;
redObeliskTurrPos2 = turrDefault + 0.13;
redObeliskTurrPos3 = turrDefault + 0.14;
blueObeliskTurrPos1 = turrDefault - 0.12;
blueObeliskTurrPos2 = turrDefault - 0.13;
blueObeliskTurrPos3 = turrDefault - 0.14;
redTurretShootPos = turrDefault + 0.05;
if (redAlliance) {
robot.light.setPosition(0.28);
pickupGateX = rPickupGateX;
pickupGateY = rPickupGateY;
pickupGateH = rPickupGateH;
pickupZoneX = rPickupZoneX;
pickupZoneY = rPickupZoneY;
pickupZoneH = rPickupZoneH;
xShoot = rShootX;
yShoot = rShootY;
hShoot = rShootH;
turretShootPos = redTurretShootPos;
} else {
robot.light.setPosition(0.6);
pickupGateX = bPickupGateX;
pickupGateY = bPickupGateY;
pickupGateH = bPickupGateH;
pickupZoneX = bPickupZoneX;
pickupZoneY = bPickupZoneY;
pickupZoneH = bPickupZoneH;
xShoot = bShootX;
yShoot = bShootY;
hShoot = bShootH;
turretShootPos = blueTurretShootPos;
}
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()) {
robot.transfer.setPower(1);
Actions.runBlocking(
new ParallelAction(
manageFlywheel(
shoot0Vel,
shoot0Hood,
9,
0.501,
0.501,
shoot0XTolerance,
0.501
)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease)
);
robot.frontLeft.setPower(-0.4);
robot.frontRight.setPower(-0.4);
robot.backLeft.setPower(-0.4);
robot.backRight.setPower(-0.4);
sleep(fwdTime);
robot.frontLeft.setPower(0);
robot.frontRight.setPower(0);
robot.backLeft.setPower(0);
robot.backRight.setPower(0);
sleep(sleepTime);
robot.frontLeft.setPower(-0.4);
robot.frontRight.setPower(0.4);
robot.backLeft.setPower(0.4);
robot.backRight.setPower(-0.4);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
sleep(strafeTime);
robot.frontLeft.setPower(0);
robot.frontRight.setPower(0);
robot.backLeft.setPower(0);
robot.backRight.setPower(0);
while (opModeIsActive()) {
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addLine("finished");
TELE.update();
}
}
}
}

648
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/Actions.java Normal file
View File

@@ -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;
}
};
}
}

117
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java
View File

@@ -1,61 +1,61 @@
package org.firstinspires.ftc.teamcode.autonomous;
package org.firstinspires.ftc.teamcode.autonomous.disabled;
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.Front_Poses.bShootH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
@@ -76,7 +76,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
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;
@@ -170,7 +169,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
robot.transferServo.setPosition(transferServo_out);
turret.manualSetTurret(turretShootPos);
turret.setTurret(turretShootPos);
robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000);
drive.updatePoseEstimate();
@@ -612,7 +611,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(0.4);
turret.setTurret(0.4);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));

118
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java
View File

@@ -1,61 +1,61 @@
package org.firstinspires.ftc.teamcode.autonomous;
package org.firstinspires.ftc.teamcode.autonomous.disabled;
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.bh3aG;
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.bx3aG;
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.by3aG;
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.rh3aG;
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.rx3aG;
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.ry3aG;
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.Front_Poses.bShootH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3aG;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3aG;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3aG;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3aG;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3aG;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3aG;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
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;
@@ -78,6 +78,7 @@ 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.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Poses_V2;
@@ -91,6 +92,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
import java.util.Objects;
@Disabled
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close_GateOpen extends LinearOpMode {
@@ -205,7 +207,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
ticker++;
robot.transferServo.setPosition(transferServo_out);
turret.manualSetTurret(turretShootPos);
turret.setTurret(turretShootPos);
drive.updatePoseEstimate();
@@ -750,7 +752,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(0.4);
turret.setTurret(0.4);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));

86
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java
View File

@@ -1,49 +1,49 @@
package org.firstinspires.ftc.teamcode.autonomous.disabled;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
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.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.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.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.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.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.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java Normal file
View File

@@ -0,0 +1,23 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Back_Poses {
public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50.1;
public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50;
public static double rShootX = 95, rShootY = 85, rShootH = 90;
public static double bShootX = 95, bShootY = -85, bShootH = -90;
public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140;
public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
public static double rStackPickupBX = 50, rStackPickupBY = 78, rStackPickupBH = 140.1;
public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1;
public static Pose2d autoStart = new Pose2d(0, 0, 0); // TODO: find this position
}

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java Normal file
View File

@@ -0,0 +1,46 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Front_Poses {
public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
public static double bx1 = 20, by1 = 0.5, bh1 = 0.1;
public static double rx2a = 41, ry2a = 18, rh2a = 140;
public static double bx2a = 41, by2a = -18, bh2a = -140;
public static double rx2b = 23, ry2b = 36, rh2b = 140.1;
public static double bx2b = 19, by2b = -40, bh2b = -140.1;
public static double rx3a = 55, ry3a = 39, rh3a = 140;
public static double bx3a = 55, by3a = -39, bh3a = -140;
public static double rx3aG = 60, ry3aG = 34, rh3aG = 140;
public static double bx3aG = 55, by3aG = -43, bh3aG = -140;
public static double rx3b = 36, ry3b = 58, rh3b = 140.1;
public static double bx3b = 41, by3b = -59, bh3b = -140.1;
public static double rx4a = 75, ry4a = 53, rh4a = 140;
public static double bx4a = 75, by4a = -53, bh4a = -140;
public static double rx4b = 50, ry4b = 78, rh4b = 140.1;
public static double bx4b = 47, by4b = -85, bh4b = -140.1;
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0;
public static double rShootX = 40, rShootY = 10, rShootH = 50;
public static double bShootX = 40, bShootY = 0, bShootH = -50;
public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
public static double bxPrep = 45, byPrep = -10, bhPrep = -50;
public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50;
public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50;
public static Pose2d teleStart = new Pose2d(0, 0, 0);
}

49
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
View File

@@ -1,49 +0,0 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Poses {
public static double rx1 = 20, ry1 = 0.5, rh1 = Math.toRadians(0.1);
public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140);
public static double rx2b = 19, ry2b = 40, rh2b = Math.toRadians(140.1);
public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
public static double rx3aG = 60, ry3aG = 34, rh3aG = Math.toRadians(140);
public static double rx3b = 36, ry3b = 58, rh3b = Math.toRadians(140.1);
public static double rx4a = 75, ry4a = 53, rh4a = Math.toRadians(140);
public static double rx4b = 45, ry4b = 83, rh4b = Math.toRadians(140.1);
public static double bx1 = 20, by1 = 0.5, bh1 = Math.toRadians(0.1);
public static double bx2a = 41, by2a = -18, bh2a = Math.toRadians(-140);
public static double bx2b = 19, by2b = -40, bh2b = Math.toRadians(-140.1);
public static double bx3a = 55, by3a = -39, bh3a = Math.toRadians(-140);
public static double bx3b = 41, by3b = -59, bh3b = Math.toRadians(-140.1);
public static double bx3aG = 55, by3aG = -43, bh3aG = Math.toRadians(-140);
public static double bx4a = 75, by4a = -53, bh4a = Math.toRadians(-140);
public static double bx4b = 47, by4b = -85, bh4b = Math.toRadians(-140.1);
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 = 40, bShootY = 7, bShootH = Math.toRadians(-50);
public static double bxPrep = 45, byPrep = -10, bhPrep = Math.toRadians(-50);
public static Pose2d teleStart = new Pose2d(0, 0, 0);
public static Pose2d teleEnd = new Pose2d(0, 0, 0);
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java
View File

@@ -1,7 +1,9 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
@Disabled
@Config
public class Poses_V2 {

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -32,4 +32,14 @@ public class ServoPositions {
public static double turret_redClose = 0.42;
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;
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java
View File

@@ -1,7 +1,9 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
@Disabled
@Config
public class ShooterVars {

62
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -1,10 +1,10 @@
package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import com.acmerobotics.dashboard.FtcDashboard;
@@ -22,6 +22,7 @@ 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.Light;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
@@ -58,6 +59,7 @@ public class TeleopV3 extends LinearOpMode {
Targeting targeting;
Targeting.Settings targetingSettings;
Drivetrain drivetrain;
MeasuringLoopTimes loopTimes;
double autoHoodOffset = 0.0;
int shooterTicker = 0;
boolean intake = false;
@@ -73,8 +75,6 @@ public class TeleopV3 extends LinearOpMode {
boolean overrideTurr = false;
int intakeTicker = 0;
boolean turretInterpolate = false;
private boolean shootAll = false;
@Override
@@ -97,6 +97,9 @@ public class TeleopV3 extends LinearOpMode {
drivetrain = new Drivetrain(robot, drive);
loopTimes = new MeasuringLoopTimes();
loopTimes.init();
PIDFController tController = new PIDFController(tp, ti, td, tf);
tController.setTolerance(0.001);
@@ -107,6 +110,9 @@ public class TeleopV3 extends LinearOpMode {
light.init(robot.light, spindexer, turret);
light.setState(StateEnums.LightState.MANUAL);
limelightUsed = true;
robot.light.setPosition(1);
while (opModeInInit()) {
robot.limelight.start();
if (redAlliance) {
@@ -126,7 +132,8 @@ public class TeleopV3 extends LinearOpMode {
waitForStart();
if (isStopRequested()) return;
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
robot.transfer.setPower(1);
while (opModeIsActive()) {
@@ -144,7 +151,7 @@ public class TeleopV3 extends LinearOpMode {
if (gamepad1.right_bumper) {
shootAll = false;
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
light.setState(StateEnums.LightState.BALL_COUNT);
@@ -155,9 +162,7 @@ public class TeleopV3 extends LinearOpMode {
light.setState(StateEnums.LightState.GOAL_LOCK);
}
robot.transfer.setPower(1);
double offset;
//TURRET TRACKING
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
@@ -204,14 +209,16 @@ public class TeleopV3 extends LinearOpMode {
}
//SHOOTER:
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
//HOOD:
if (targetingHood) {
robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset);
servo.setHoodPos(targetingSettings.hoodAngle + autoHoodOffset);
} else {
robot.hood.setPosition(hoodDefaultPos);
servo.setHoodPos(hoodDefaultPos);
}
if (gamepad2.dpadUpWasPressed()) {
@@ -243,6 +250,8 @@ public class TeleopV3 extends LinearOpMode {
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
}
if (enableSpindexerManager) {
//if (!shootAll) {
spindexer.processIntake();
@@ -250,13 +259,12 @@ public class TeleopV3 extends LinearOpMode {
// RIGHT_BUMPER
if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
robot.intake.setPower(1);
spindexer.setIntakePower(1);
} else if (gamepad1.cross) {
robot.intake.setPower(-1);
spindexer.setIntakePower(-1);
} else {
robot.intake.setPower(0);
spindexer.setIntakePower(0);
}
// LEFT_BUMPER
@@ -277,14 +285,14 @@ public class TeleopV3 extends LinearOpMode {
spindexer.prepareShootAllContinous();
//TELE.addLine("preparing to shoot");
// } else if (shooterTicker == 2) {
// //robot.transferServo.setPosition(transferServo_in);
// //servo.setTransferPos(transferServo_in);
// spindexer.shootAll();
// TELE.addLine("starting to shoot");
} else if (!spindexer.shootAllComplete()) {
robot.transferServo.setPosition(transferServo_in);
servo.setTransferPos(transferServo_in);
//TELE.addLine("shoot");
} else {
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
//spindexPos = spindexer_intakePos1;
shootAll = false;
spindexer.resetSpindexer();
@@ -296,28 +304,20 @@ public class TeleopV3 extends LinearOpMode {
}
if (gamepad1.left_stick_button) {
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
//spindexPos = spindexer_intakePos1;
shootAll = false;
spindexer.resetSpindexer();
}
}
if (gamepad2.square) {
drive.updatePoseEstimate();
teleEnd = drive.localizer.getPose();
gamepad2.rumble(1000);
}
//EXTRA STUFFINESS:
drive.updatePoseEstimate();
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}
loopTimes.loop();
//
// TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
// TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
@@ -341,7 +341,7 @@ public class TeleopV3 extends LinearOpMode {
// TELE.addData("spinTestCounter", spindexer.counter);
// TELE.addData("autoSpintake", autoSpintake);
//
TELE.addData("shootall commanded", shootAll);
// TELE.addData("shootall commanded", shootAll);
// Targeting Debug
TELE.addData("robotX", robotX);
TELE.addData("robotY", robotY);
@@ -353,6 +353,10 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.addData("Voltage", voltage); // returns alleged recorded voltage (not same as driver hub)
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.update();

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java
View File

@@ -60,7 +60,7 @@ public class LimelightTest extends LinearOpMode {
if (turretMode){
if (turretPos != 0.501){
turret.manualSetTurret(turretPos);
turret.setTurret(turretPos);
}
}

62
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
View File

@@ -1,23 +1,30 @@
package org.firstinspires.ftc.teamcode.tests;
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.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.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
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
@TeleOp
@@ -26,22 +33,23 @@ public class ShooterTest extends LinearOpMode {
public static double parameter = 0.0;
// --- CONSTANTS YOU TUNE ---
//TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED
public static double Velocity = 0.0;
public static double P = 255.0;
public static double I = 0.0;
public static double D = 0.0;
public static double F = 7.5;
public static double F = 90;
public static double transferPower = 1.0;
public static double hoodPos = 0.501;
public static double turretPos = 0.501;
public static boolean shoot = false;
public static boolean intake = false;
public static boolean turretTrack = true;
Robot robot;
Flywheel flywheel;
Servos servo;
MecanumDrive drive;
Turret turret;
double shootStamp = 0.0;
boolean shootAll = false;
@@ -51,6 +59,7 @@ public class ShooterTest extends LinearOpMode {
public double hoodAdjust = 0.0;
public static double hoodAdjustFactor = 1.0;
private int shooterTicker = 0;
public static double spinSpeed = 0.02;
Spindexer spindexer ;
@Override
@@ -62,22 +71,58 @@ public class ShooterTest extends LinearOpMode {
flywheel = new Flywheel(hardwareMap);
spindexer = new Spindexer(hardwareMap);
servo = new Servos(hardwareMap);
drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
MultipleTelemetry TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
turret = new Turret(robot, TELE, robot.limelight);
Turret.limelightUsed = true;
waitForStart();
robot.limelight.start();
if (isStopRequested()) return;
while (opModeIsActive()) {
if (redAlliance){
robot.limelight.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
}
//TURRET TRACKING
drive.updatePoseEstimate();
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);
if (turretTrack){
turret.trackGoal(deltaPose);
} else if (turretPos != 0.501){
turret.setTurret(turretPos);
}
double voltage = robot.voltage.getVoltage();
if (mode == 0) {
rightShooter.setPower(parameter);
leftShooter.setPower(parameter);
} else if (mode == 1) {
flywheel.setPIDF(P, I, D, F);
flywheel.setPIDF(P, I, D, F / voltage);
flywheel.manageFlywheel((int) Velocity);
}
@@ -109,7 +154,6 @@ public class ShooterTest extends LinearOpMode {
//intake = false;
//reject = false;
// TODO: Change starting position based on desired order to shoot green ball
//spindexPos = spindexer_intakePos1;
if (getRuntime() - shootStamp < 3.5) {
@@ -120,8 +164,11 @@ public class ShooterTest extends LinearOpMode {
robot.transferServo.setPosition(transferServo_in);
shooterTicker++;
double prevSpinPos = robot.spin1.getPosition();
robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease);
robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
if (prevSpinPos < 0.9){
robot.spin1.setPosition(prevSpinPos + spinSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spinSpeed);
}
}
@@ -150,6 +197,7 @@ public class ShooterTest extends LinearOpMode {
TELE.addData("Power", robot.shooter1.getPower());
TELE.addData("Steady?", flywheel.getSteady());
TELE.addData("Position", robot.shooter1.getCurrentPosition());
TELE.addData("Voltage", voltage);
TELE.update();

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
View File

@@ -12,6 +12,7 @@ public class Flywheel {
public double velo1 = 0.0;
public double velo2 = 0.0;
double targetVelocity = 0.0;
double previousTargetVelocity = 0.0;
double powPID = 0.0;
boolean steady = false;
public Flywheel (HardwareMap hardwareMap) {
@@ -37,6 +38,8 @@ public class Flywheel {
}
// Set the robot PIDF for the next cycle.
private double prevF = 0.501;
public static int voltagePIDFDifference = 5;
public void setPIDF(double p, double i, double d, double f) {
shooterPIDF1.p = p;
shooterPIDF1.i = i;
@@ -46,6 +49,10 @@ public class Flywheel {
shooterPIDF2.i = i;
shooterPIDF2.d = d;
shooterPIDF2.f = f;
if (Math.abs(prevF - f) > voltagePIDFDifference){
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
}
}
// Convert from RPM to Ticks per Second
@@ -55,12 +62,12 @@ public class Flywheel {
private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;}
public double manageFlywheel(double commandedVelocity) {
if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
targetVelocity = commandedVelocity;
// Add code here to set PIDF based on desired RPM
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
//robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
//robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
@@ -69,6 +76,7 @@ public class Flywheel {
velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
velo = Math.max(velo1, velo2);
}
// really should be a running average of the last 5
steady = (Math.abs(targetVelocity - velo) < 200.0);

66
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java Normal file
View File

@@ -0,0 +1,66 @@
package org.firstinspires.ftc.teamcode.utils;
import com.qualcomm.robotcore.util.ElapsedTime;
public class MeasuringLoopTimes {
private ElapsedTime elapsedtime;
private double minLoopTime = 999999999999.0;
private double maxLoopTime = 0.0;
private double mainLoopTime = 0.0;
private double MeasurementStart = 0.0;
private double currentTime = 0.0;
private double avgLoopTime = 0.0;
private int avgLoopTimeTicker = 0;
private double avgLoopTimeSum = 0;
private double getTimeSeconds ()
{
return (double) System.currentTimeMillis()/1000.0;
}
public void init() {
elapsedtime = new ElapsedTime();
elapsedtime.reset();
MeasurementStart = getTimeSeconds();
}
public double getAvgLoopTime() {
return avgLoopTime;
}
public double getMaxLoopTimeOneMin() {
return maxLoopTime;
}
public double getMinLoopTimeOneMin() {
return minLoopTime;
}
public void loop() {
currentTime = getTimeSeconds();
if ((MeasurementStart + 15.0) < currentTime)
{
minLoopTime = 9999999.0;
maxLoopTime = 0.0;
MeasurementStart = currentTime;
avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker;
avgLoopTimeSum = 0.0;
avgLoopTimeTicker = 0;
}
mainLoopTime = elapsedtime.milliseconds();
elapsedtime.reset();
avgLoopTimeSum += mainLoopTime;
avgLoopTimeTicker++;
minLoopTime = Math.min(minLoopTime,mainLoopTime);
maxLoopTime = Math.max(maxLoopTime,mainLoopTime);
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java
View File

@@ -35,7 +35,7 @@ public class PositionalServoProgrammer extends LinearOpMode {
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()){
if (spindexPos != 0.501 && !servo.spinEqual(spindexPos)){
if (spindexPos != 0.501){
robot.spin1.setPosition(spindexPos);
robot.spin2.setPosition(1-spindexPos);
}
@@ -52,16 +52,6 @@ public class PositionalServoProgrammer extends LinearOpMode {
if (light !=0.501){
robot.light.setPosition(light);
}
// To check configuration of spindexer:
// Set "mode" to 1 and spindexPow to 0.1
// If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE
// Do the previous test again to confirm
// Set "mode" to 0 but keep spindexPos at 0.501
// Manually turn the spindexer until "spindexer pos" is set close to 0
// Check each spindexer voltage:
// If "spindexer voltage 1" is closer to 0 than "spindexer voltage 2," then you are done!
// If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE
//TODO: @KeshavAnandCode do the above please
TELE.addData("spindexer pos", servo.getSpinPos());
TELE.addData("turret pos", robot.turr1.getPosition());

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
View File

@@ -12,6 +12,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
@@ -33,7 +34,7 @@ public class Robot {
public double shooterPIDF_P = 255.0;
public double shooterPIDF_I = 0.0;
public double shooterPIDF_D = 0.0;
public double shooterPIDF_F = 7.5;
public double shooterPIDF_F = 90;
public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
public DcMotorEx shooter1;
public DcMotorEx shooter2;
@@ -56,11 +57,11 @@ public class Robot {
public RevColorSensorV3 color3;
public Limelight3A limelight;
public Servo light;
public VoltageSensor voltage;
public Robot(HardwareMap hardwareMap) {
//Define components w/ hardware map
//TODO: fix the configuration of these - I trust you to figure it out yourself @KeshavAnandCode
frontLeft = hardwareMap.get(DcMotorEx.class, "fl");
frontRight = hardwareMap.get(DcMotorEx.class, "fr");
backLeft = hardwareMap.get(DcMotorEx.class, "bl");
@@ -78,7 +79,7 @@ public class Robot {
shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
//TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode
shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F);
shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
@@ -96,7 +97,6 @@ public class Robot {
turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
//TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer
spin1 = hardwareMap.get(Servo.class, "spin2");
spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
@@ -128,5 +128,6 @@ public class Robot {
}
light = hardwareMap.get(Servo.class, "light");
voltage = hardwareMap.voltageSensor.iterator().next();
}
}

45
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
View File

@@ -18,6 +18,15 @@ public class Servos {
PIDFController spinPID;
PIDFController turretPID;
private double prevSpinPos = 0.0;
private boolean firstSpinPos = true;
private double prevTransferPos = 0.0;
private boolean firstTransferPos = true;
private double prevHoodPos = 0.0;
private boolean firstHoodPos = true;
public Servos(HardwareMap hardwareMap) {
robot = new Robot(hardwareMap);
spinPID = new PIDFController(spinP, spinI, spinD, spinF);
@@ -32,8 +41,42 @@ public class Servos {
return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3);
}
public double setSpinPos(double pos) {
public double getSpinCmdPos() {
return prevSpinPos;
}
public static boolean servoPosEqual(double pos1, double pos2) {
return (Math.abs(pos1 - pos2) < 0.005);
}
public double setTransferPos(double pos) {
if (firstTransferPos || !servoPosEqual(pos, prevTransferPos)) {
robot.transferServo.setPosition(pos);
firstTransferPos = false;
}
prevTransferPos = pos;
return pos;
}
public double setSpinPos(double pos) {
if (firstSpinPos || !servoPosEqual(pos, prevSpinPos)) {
robot.spin1.setPosition(pos);
robot.spin2.setPosition(1-pos);
firstSpinPos = false;
}
prevSpinPos = pos;
return pos;
}
public double setHoodPos(double pos){
if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) {
robot.hood.setPosition(pos);
firstHoodPos = false;
}
prevHoodPos = pos;
return pos;
}

267
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
View File

@@ -1,13 +1,12 @@
package org.firstinspires.ftc.teamcode.utils;
import static org.firstinspires.ftc.teamcode.constants.Color.Light0;
import static org.firstinspires.ftc.teamcode.constants.Color.Light1;
import static org.firstinspires.ftc.teamcode.constants.Color.Light2;
import static org.firstinspires.ftc.teamcode.constants.Color.Light3;
import static org.firstinspires.ftc.teamcode.constants.Color.LightGreen;
import static org.firstinspires.ftc.teamcode.constants.Color.LightOrange;
import static org.firstinspires.ftc.teamcode.constants.Color.LightPurple;
import static org.firstinspires.ftc.teamcode.constants.Color.colorFilterAlpha;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import static org.firstinspires.ftc.teamcode.constants.Color.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
@@ -22,10 +21,6 @@ import static org.firstinspires.ftc.teamcode.utils.Servos.spinF;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinI;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinP;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.constants.StateEnums;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
@@ -34,39 +29,84 @@ import java.util.Objects;
public class Spindexer {
// Array of commandedIntakePositions with contents
// {RearCenter, FrontDriver, FrontPassenger}
static final int[][] RotatedBallPositions = {{0, 2, 1}, {1, 0, 2}, {2, 1, 0}};
public int commandedIntakePosition = 0;
public double distanceRearCenter = 0.0;
public double distanceFrontDriver = 0.0;
public double distanceFrontPassenger = 0.0;
public double spindexerWiggle = 0.01;
public double spindexerOuttakeWiggle = 0.01;
public double spindexerPosOffset = 0.00;
public StateEnums.Motif desiredMotif = StateEnums.Motif.NONE;
public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
public int unknownColorDetect = 0;
public int counter = 0;
Robot robot;
Servos servos;
Flywheel flywheel;
MecanumDrive drive;
double lastKnownSpinPos = 0.0;
MultipleTelemetry TELE;
PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF);
double spinCurrentPos = 0.0;
int shootWaitCount = 0;
BallPosition[] ballPositions = new BallPosition[3];
double[] outakePositions =
{spindexer_outtakeBall1 + spindexerPosOffset,
spindexer_outtakeBall2 + spindexerPosOffset,
spindexer_outtakeBall3 + spindexerPosOffset};
double[] intakePositions =
{spindexer_intakePos1, spindexer_intakePos2, spindexer_intakePos3};
public int commandedIntakePosition = 0;
public double distanceRearCenter = 0.0;
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;
private double intakeTicker = 0;
public double spindexerPosOffset = 0.00;
public static int shootWaitMax = 4;
public StateEnums.Motif desiredMotif = StateEnums.Motif.NONE;
// For Use
enum RotatedBallPositionNames {
REARCENTER,
FRONTDRIVER,
FRONTPASSENGER
}
// Array of commandedIntakePositions with contents
// {RearCenter, FrontDriver, FrontPassenger}
static final int[][] RotatedBallPositions = {{0,2,1}, {1,0,2}, {2,1,0}};
class spindexerBallRoatation {
int rearCenter = 0; // aka commanded Position
int frontDriver = 0;
int frontPassenger = 0;
}
enum IntakeState {
UNKNOWN_START,
UNKNOWN_MOVE,
UNKNOWN_DETECT,
INTAKE,
FINDNEXT,
MOVING,
FULL,
SHOOTNEXT,
SHOOTMOVING,
SHOOTWAIT,
SHOOT_ALL_PREP,
SHOOT_ALL_READY,
SHOOT_PREP_CONTINOUS,
SHOOT_CONTINOUS
}
int shootWaitCount = 0;
public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
public int unknownColorDetect = 0;
public enum BallColor {
UNKNOWN,
GREEN,
PURPLE
}
class BallPosition {
boolean isEmpty = true;
int foundEmpty = 0;
BallColor ballColor = BallColor.UNKNOWN;
}
BallPosition[] ballPositions = new BallPosition[3];
public boolean init () {
return true;
}
public Spindexer(HardwareMap hardwareMap) {
robot = new Robot(hardwareMap);
@@ -80,11 +120,34 @@ public class Spindexer {
ballPositions[1] = new BallPosition();
ballPositions[2] = new BallPosition();
}
public boolean init() {
return true;
}
double[] outakePositions =
{spindexer_outtakeBall1+spindexerPosOffset,
spindexer_outtakeBall2+spindexerPosOffset,
spindexer_outtakeBall3+spindexerPosOffset};
double[] intakePositions =
{spindexer_intakePos1, spindexer_intakePos2, spindexer_intakePos3};
public int counter = 0;
// private double getTimeSeconds ()
// {
// return (double) System.currentTimeMillis()/1000.0;
// }
// public double getPos() {
// robot.spin1Pos.getVoltage();
// robot.spin1Pos.getMaxVoltage();
// return (robot.spin1Pos.getVoltage()/robot.spin1Pos.getMaxVoltage());
// }
// public void manageSpindexer() {
//
// }
public void resetBallPosition (int pos) {
ballPositions[pos].isEmpty = true;
@@ -126,11 +189,9 @@ public class Spindexer {
if (detectRearColor) {
// Detect which color
double green = robot.color1.getNormalizedColors().green;
double red = robot.color1.getNormalizedColors().red;
double blue = robot.color1.getNormalizedColors().blue;
NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors();
double gP = green / (green + red + blue);
double gP = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue);
// FIXIT - Add filtering to improve accuracy.
if (gP >= 0.38) {
@@ -147,11 +208,9 @@ public class Spindexer {
// reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0;
if (detectFrontColor) {
double green = robot.color2.getNormalizedColors().green;
double red = robot.color2.getNormalizedColors().red;
double blue = robot.color2.getNormalizedColors().blue;
NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors();
double gP = green / (green + red + blue);
double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue);
if (gP >= 0.4) {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
@@ -176,11 +235,9 @@ public class Spindexer {
// reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0;
if (detectFrontColor) {
double green = robot.color3.getNormalizedColors().green;
double red = robot.color3.getNormalizedColors().red;
double blue = robot.color3.getNormalizedColors().blue;
NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors();
double gP = green / (green + red + blue);
double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue);
if (gP >= 0.42) {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
@@ -209,11 +266,9 @@ public class Spindexer {
return newPos1Detection;
}
// Has code to unjam spindexer
private void moveSpindexerToPos(double pos) {
robot.spin1.setPosition(pos);
robot.spin2.setPosition(1 - pos);
servos.setSpinPos(pos);
// double currentPos = servos.getSpinPos();
// if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
// if (currentPos > pos){
@@ -224,24 +279,9 @@ public class Spindexer {
// robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05);
// }
// }
// prevPos = currentPos;
// prevPos = pos;
}
// private double getTimeSeconds ()
// {
// return (double) System.currentTimeMillis()/1000.0;
// }
// public double getPos() {
// robot.spin1Pos.getVoltage();
// robot.spin1Pos.getMaxVoltage();
// return (robot.spin1Pos.getVoltage()/robot.spin1Pos.getMaxVoltage());
// }
// public void manageSpindexer() {
//
// }
public void stopSpindexer() {
}
@@ -302,6 +342,9 @@ public class Spindexer {
}
}
public boolean slotIsEmpty(int slot){
return !ballPositions[slot].isEmpty;
}
@@ -310,13 +353,14 @@ public class Spindexer {
return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty);
}
private double intakeTicker = 0;
public boolean processIntake() {
switch (currentIntakeState) {
case UNKNOWN_START:
// For now just set position ONE if UNKNOWN
commandedIntakePosition = 0;
moveSpindexerToPos(intakePositions[0]);
servos.setSpinPos(intakePositions[0]);
currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE;
break;
case UNKNOWN_MOVE:
@@ -327,7 +371,7 @@ public class Spindexer {
unknownColorDetect = 0;
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
servos.setSpinPos(intakePositions[commandedIntakePosition]);
}
break;
case UNKNOWN_DETECT:
@@ -346,7 +390,7 @@ public class Spindexer {
} else {
// Maintain Position
spindexerWiggle *= -1.0;
moveSpindexerToPos(intakePositions[commandedIntakePosition] + spindexerWiggle);
servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
}
break;
case FINDNEXT:
@@ -378,7 +422,7 @@ public class Spindexer {
//commandedIntakePosition = bestFitMotif();
currentIntakeState = Spindexer.IntakeState.FULL;
}
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
servos.setSpinPos(intakePositions[commandedIntakePosition]);
break;
case MOVING:
@@ -394,7 +438,7 @@ public class Spindexer {
//detectBalls(false, false);
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
servos.setSpinPos(intakePositions[commandedIntakePosition]);
}
break;
@@ -407,7 +451,7 @@ public class Spindexer {
}
// Maintain Position
spindexerWiggle *= -1.0;
moveSpindexerToPos(intakePositions[commandedIntakePosition] + spindexerWiggle);
servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
break;
case SHOOT_ALL_PREP:
@@ -416,7 +460,7 @@ public class Spindexer {
commandedIntakePosition = 0;
if (!servos.spinEqual(outakePositions[commandedIntakePosition])) {
// Keep moving the spindexer
moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
servos.setSpinPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
}
break;
@@ -428,7 +472,7 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
}
// Maintain Position
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
servos.setSpinPos(outakePositions[commandedIntakePosition]);
break;
case SHOOTNEXT:
@@ -449,7 +493,7 @@ public class Spindexer {
// Empty return to intake state
currentIntakeState = IntakeState.FINDNEXT;
}
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
servos.setSpinPos(outakePositions[commandedIntakePosition]);
break;
case SHOOTMOVING:
@@ -458,12 +502,11 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
} else {
// Keep moving the spindexer
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
servos.setSpinPos(outakePositions[commandedIntakePosition]);
}
break;
case SHOOTWAIT:
double shootWaitMax = 4;
// Stopping when we get to the new position
if (prevIntakeState != currentIntakeState) {
if (commandedIntakePosition==2) {
@@ -483,14 +526,14 @@ public class Spindexer {
}
// Keep moving the spindexer
spindexerOuttakeWiggle *= -1.0;
moveSpindexerToPos(outakePositions[commandedIntakePosition] + spindexerOuttakeWiggle);
servos.setSpinPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle);
break;
case SHOOT_PREP_CONTINOUS:
if (servos.spinEqual(spinStartPos)){
currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
} else {
moveSpindexerToPos(spinStartPos);
servos.setSpinPos(spinStartPos);
}
break;
@@ -501,11 +544,11 @@ public class Spindexer {
if (servos.getSpinPos() > spinEndPos){
currentIntakeState = IntakeState.FINDNEXT;
} else {
double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease;
double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease;
if (spinPos > spinEndPos + 0.03){
spinPos = spinEndPos + 0.03;
}
moveSpindexerToPos(spinPos);
servos.setSpinPos(spinPos);
}
break;
@@ -568,18 +611,15 @@ public class Spindexer {
public void prepareShootAll(){
currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP;
}
public void prepareShootAllContinous(){
currentIntakeState = Spindexer.IntakeState.SHOOT_PREP_CONTINOUS;
}
public void shootAll () {
ballPositions[0].isEmpty = false;
ballPositions[1].isEmpty = false;
ballPositions[2].isEmpty = false;
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
}
public void shootAllContinous(){
ballPositions[0].isEmpty = false;
ballPositions[1].isEmpty = false;
@@ -587,7 +627,8 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
}
public boolean shootAllComplete() {
public boolean shootAllComplete ()
{
return ((currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_PREP) &&
(currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) &&
(currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) &&
@@ -601,7 +642,8 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.FINDNEXT;
}
public void update() {
public void update()
{
}
public BallColor GetFrontDriverColor () {
@@ -615,46 +657,11 @@ public class Spindexer {
public BallColor GetRearCenterColor () {
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor;
}
// For Use
enum RotatedBallPositionNames {
REARCENTER,
FRONTDRIVER,
FRONTPASSENGER
private double prevPow = 0.501;
public void setIntakePower(double pow){
if (prevPow != 0.501 && prevPow != pow){
robot.intake.setPower(pow);
}
enum IntakeState {
UNKNOWN_START,
UNKNOWN_MOVE,
UNKNOWN_DETECT,
INTAKE,
FINDNEXT,
MOVING,
FULL,
SHOOTNEXT,
SHOOTMOVING,
SHOOTWAIT,
SHOOT_ALL_PREP,
SHOOT_ALL_READY,
SHOOT_PREP_CONTINOUS,
SHOOT_CONTINOUS
}
public enum BallColor {
UNKNOWN,
GREEN,
PURPLE
}
class spindexerBallRoatation {
int rearCenter = 0; // aka commanded Position
int frontDriver = 0;
int frontPassenger = 0;
}
class BallPosition {
boolean isEmpty = true;
int foundEmpty = 0;
BallColor ballColor = BallColor.UNKNOWN;
prevPow = pow;
}
}

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java
View File

@@ -77,6 +77,7 @@ public class Targeting {
double cancelOffsetY = 0.0; // was 7.0
double unitConversionFactor = 0.95;
int tileSize = 24; //inches
public static boolean turretInterpolate = false;
public Targeting() {
}

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -10,6 +10,7 @@ import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
import org.firstinspires.ftc.teamcode.constants.Color;
@@ -37,7 +38,8 @@ public class Turret {
// TODO: tune these values for limelight
public static double clampTolerance = 0.03;
public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
//public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
public static double B_PID_P = 0.066, B_PID_I = 0.0, B_PID_D = 0.007;
Robot robot;
MultipleTelemetry TELE;
Limelight3A webcam;
@@ -56,6 +58,9 @@ public class Turret {
private double permanentOffset = 0.0;
private PIDController bearingPID;
private double prevTurretPos = 0.0;
private boolean firstTurretPos = true;
public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
this.TELE = tele;
this.robot = rob;
@@ -76,11 +81,14 @@ public class Turret {
return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault;
}
public void manualSetTurret(double pos) {
private double prevTurrPos = 0.501;
public void setTurret(double pos) {
if (prevTurrPos != 0.501 && prevTurrPos != pos){
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1-pos);
}
prevTurrPos = pos;
}
public boolean turretEqual(double pos) {
return Math.abs(pos - this.getTurrPos()) < turretTolerance;
@@ -284,8 +292,7 @@ public class Turret {
}
// Set servo positions
robot.turr1.setPosition(turretPos + manualOffset);
robot.turr2.setPosition(1.0 - turretPos - manualOffset);
setTurret(turretPos + manualOffset);
/* ---------------- TELEMETRY ---------------- */

34
build.dependencies.gradle
View File

@@ -6,30 +6,26 @@ repositories {
maven { url = 'https://maven.brott.dev/' } //RR
maven { url = "https://maven.rowanmcalpin.com/" } //Next FTC
maven { url = "https://repo.dairy.foundation/releases" } //AS
}
dependencies {
implementation 'org.firstinspires.ftc:Inspection:11.0.0'
implementation 'org.firstinspires.ftc:Blocks:11.0.0'
implementation 'org.firstinspires.ftc:RobotCore:11.0.0'
implementation 'org.firstinspires.ftc:RobotServer:11.0.0'
implementation 'org.firstinspires.ftc:OnBotJava:11.0.0'
implementation 'org.firstinspires.ftc:Hardware:11.0.0'
implementation 'org.firstinspires.ftc:FtcCommon:11.0.0'
implementation 'org.firstinspires.ftc:Vision:11.0.0'
implementation 'org.firstinspires.ftc:Inspection:11.1.0'
implementation 'org.firstinspires.ftc:Blocks:11.1.0'
//noinspection Aligned16KB
implementation 'org.firstinspires.ftc:RobotCore:11.1.0'
implementation 'org.firstinspires.ftc:RobotServer:11.1.0'
implementation 'org.firstinspires.ftc:OnBotJava:11.1.0'
implementation 'org.firstinspires.ftc:Hardware:11.1.0'
implementation 'org.firstinspires.ftc:FtcCommon:11.1.0'
implementation 'org.firstinspires.ftc:Vision:11.1.0'
implementation 'androidx.appcompat:appcompat:1.2.0'
implementation 'com.pedropathing:ftc:2.0.1' //PedroCore
implementation 'com.pedropathing:telemetry:0.0.6' //PedroTele
implementation 'com.pedropathing:ftc:2.0.6' //PedroCore
implementation 'com.pedropathing:telemetry:1.0.0' //PedroTele
implementation 'com.bylazar:fullpanels:1.0.2' //Panels
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
@@ -40,11 +36,5 @@ dependencies {
implementation("page.j5155.AdvantageScope:lite:v26.0.0") //AS
}