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

implemented loop time efficiencies and added turret tracking to shooter test

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-07 19:02:41 -06:00
parent 50060d3812
commit ef1c7b0e6b
11 changed files with 274 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
View File

@@ -213,8 +213,7 @@ public class Auto_LT_Close extends LinearOpMode {
stamp = System.currentTimeMillis();
}
ticker++;
robot.transferServo.setPosition(transferServo_out);
servos.setTransferPos(transferServo_out);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
@@ -246,8 +245,7 @@ public class Auto_LT_Close 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);
@@ -323,8 +321,7 @@ public class Auto_LT_Close extends LinearOpMode {
// TELE.update();
robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
robot.spin1.setPosition(firstSpindexShootPos);
robot.spin2.setPosition(1 - firstSpindexShootPos);
servos.setSpinPos(firstSpindexShootPos);
return true;
} else {
@@ -390,20 +387,18 @@ public class Auto_LT_Close extends LinearOpMode {
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);
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
@@ -471,27 +466,24 @@ public class Auto_LT_Close extends LinearOpMode {
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);
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
@@ -563,8 +555,7 @@ public class Auto_LT_Close 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;
@@ -627,7 +618,7 @@ public class Auto_LT_Close extends LinearOpMode {
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;
@@ -695,7 +686,7 @@ public class Auto_LT_Close 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);
@@ -766,7 +757,7 @@ public class Auto_LT_Close 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);
@@ -819,14 +810,13 @@ public class Auto_LT_Close extends LinearOpMode {
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);
servos.setTransferPos(transferServo_out);
limelightUsed = false;
@@ -834,8 +824,8 @@ public class Auto_LT_Close extends LinearOpMode {
while (opModeInInit()) {
robot.hood.setPosition(shoot0Hood);
turret.manualSetTurret(turrDefault);
servos.setHoodPos(shoot0Hood);
turret.setTurret(turrDefault);
if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java
View File

@@ -169,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();
@@ -611,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));

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java
View File

@@ -207,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();
@@ -752,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));

56
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java
View File

@@ -97,8 +97,7 @@ public class Auto_LT_Far extends LinearOpMode {
stamp = System.currentTimeMillis();
}
ticker++;
robot.transferServo.setPosition(transferServo_out);
servos.setTransferPos(transferServo_out);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
@@ -130,8 +129,7 @@ public class Auto_LT_Far 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);
@@ -207,8 +205,7 @@ public class Auto_LT_Far extends LinearOpMode {
// TELE.update();
robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
robot.spin1.setPosition(firstSpindexShootPos);
robot.spin2.setPosition(1 - firstSpindexShootPos);
servos.setSpinPos(firstSpindexShootPos);
return true;
} else {
@@ -274,20 +271,18 @@ public class Auto_LT_Far extends LinearOpMode {
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);
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
@@ -355,27 +350,24 @@ public class Auto_LT_Far extends LinearOpMode {
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);
servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer();
spindexer.processIntake();
@@ -447,8 +439,7 @@ public class Auto_LT_Far 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;
@@ -511,7 +502,7 @@ public class Auto_LT_Far extends LinearOpMode {
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;
@@ -579,7 +570,7 @@ public class Auto_LT_Far 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);
@@ -650,7 +641,7 @@ public class Auto_LT_Far 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);
@@ -701,14 +692,13 @@ public class Auto_LT_Far extends LinearOpMode {
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);
servos.setTransferPos(transferServo_out);
robot.light.setPosition(1);
@@ -721,7 +711,7 @@ public class Auto_LT_Far extends LinearOpMode {
gamepad2.rumble(1000);
}
turret.manualSetTurret(turretShootPos);
turret.setTurret(turretShootPos);
robot.hood.setPosition(shoot0Hood);

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

@@ -19,6 +19,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
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.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
@@ -54,6 +55,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;
@@ -92,6 +94,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);
@@ -113,7 +118,7 @@ public class TeleopV3 extends LinearOpMode {
waitForStart();
if (isStopRequested()) return;
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
while (opModeIsActive()) {
@@ -133,7 +138,7 @@ public class TeleopV3 extends LinearOpMode {
if (gamepad1.right_bumper) {
shootAll = false;
robot.transferServo.setPosition(transferServo_out);
servo.setTransferPos(transferServo_out);
}
@@ -193,9 +198,9 @@ public class TeleopV3 extends LinearOpMode {
//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()) {
@@ -265,10 +270,10 @@ public class TeleopV3 extends LinearOpMode {
// 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();
@@ -280,7 +285,7 @@ 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();
@@ -293,6 +298,7 @@ public class TeleopV3 extends LinearOpMode {
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}
loopTimes.loop();
//
// TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
// TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
@@ -328,7 +334,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", robot.voltage.getVoltage()); // returns alleged recorded voltage (not same as driver hub)
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);
}
}

53
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
@@ -37,10 +44,12 @@ public class ShooterTest extends LinearOpMode {
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;
@@ -50,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
@@ -61,17 +71,51 @@ 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) {
@@ -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);
}
}

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

49
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 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;
}
@@ -48,8 +91,4 @@ public class Servos {
public double setTurrPos(double pos) {
return 1.0;
}
public boolean turretEqual(double pos) {
return true;
}
}

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

@@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.utils;
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;
@@ -184,11 +186,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) {
@@ -196,7 +196,7 @@ public class Spindexer {
} else {
ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple
}
}
}
}
// Position 2
// Find which ball position this is in the spindexer
@@ -205,11 +205,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
@@ -234,11 +232,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
@@ -269,8 +265,7 @@ public class Spindexer {
}
// 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){
@@ -281,13 +276,14 @@ public class Spindexer {
// robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05);
// }
// }
// prevPos = currentPos;
// prevPos = pos;
}
public void stopSpindexer() {
}
private double prevLight = 0.0;
public void ballCounterLight(){
int counter = 0;
if (!ballPositions[0].isEmpty){
@@ -299,15 +295,21 @@ public class Spindexer {
if (!ballPositions[2].isEmpty){
counter++;
}
double light;
if (counter == 3){
robot.light.setPosition(Light3);
light = Light3;
} else if (counter == 2){
robot.light.setPosition(Light2);
light = Light2;
} else if (counter == 1){
robot.light.setPosition(Light1);
light = Light1;
} else {
robot.light.setPosition(Light0);
light = Light0;
}
if (light != prevLight){
robot.light.setPosition(light);
}
prevLight = light;
}
public boolean slotIsEmpty(int slot){
@@ -325,7 +327,7 @@ public class Spindexer {
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:
@@ -336,7 +338,7 @@ public class Spindexer {
unknownColorDetect = 0;
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
servos.setSpinPos(intakePositions[commandedIntakePosition]);
}
break;
case UNKNOWN_DETECT:
@@ -355,7 +357,7 @@ public class Spindexer {
} else {
// Maintain Position
spindexerWiggle *= -1.0;
moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
}
break;
case FINDNEXT:
@@ -387,7 +389,7 @@ public class Spindexer {
//commandedIntakePosition = bestFitMotif();
currentIntakeState = Spindexer.IntakeState.FULL;
}
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
servos.setSpinPos(intakePositions[commandedIntakePosition]);
break;
case MOVING:
@@ -403,7 +405,7 @@ public class Spindexer {
//detectBalls(false, false);
} else {
// Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]);
servos.setSpinPos(intakePositions[commandedIntakePosition]);
}
break;
@@ -416,7 +418,7 @@ public class Spindexer {
}
// Maintain Position
spindexerWiggle *= -1.0;
moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
break;
case SHOOT_ALL_PREP:
@@ -425,7 +427,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;
@@ -437,7 +439,7 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
}
// Maintain Position
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
servos.setSpinPos(outakePositions[commandedIntakePosition]);
break;
case SHOOTNEXT:
@@ -458,7 +460,7 @@ public class Spindexer {
// Empty return to intake state
currentIntakeState = IntakeState.FINDNEXT;
}
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
servos.setSpinPos(outakePositions[commandedIntakePosition]);
break;
case SHOOTMOVING:
@@ -467,7 +469,7 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
} else {
// Keep moving the spindexer
moveSpindexerToPos(outakePositions[commandedIntakePosition]);
servos.setSpinPos(outakePositions[commandedIntakePosition]);
}
break;
@@ -492,14 +494,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;
@@ -510,11 +512,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;
@@ -565,7 +567,7 @@ public class Spindexer {
//break;
case NONE:
return 0;
//break;
//break;
}
return 0;
}

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

@@ -71,10 +71,13 @@ public class Turret {
return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault;
}
public void manualSetTurret(double pos) {
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1 - 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) {
@@ -272,8 +275,7 @@ public class Turret {
}
// Set servo positions
robot.turr1.setPosition(turretPos + manualOffset);
robot.turr2.setPosition(1.0 - turretPos - manualOffset);
setTurret(turretPos + manualOffset);
/* ---------------- TELEMETRY ---------------- */