Compare commits
13 Commits
daniel
...
c368552724
| Author | SHA1 | Date | |
|---|---|---|---|
| c368552724 | |||
| 934cabafc5 | |||
| 05107ab828 | |||
| 7ef9d3a556 | |||
| a84891bc06 | |||
| 21b4b0b4f5 | |||
| 5faa373cad | |||
| 32d481c031 | |||
| 3e8e2c2b4b | |||
| 458b6f53a2 | |||
| aea3f0b2d4 | |||
| bcf59ff458 | |||
| 065bcfa40b |
@@ -1,478 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.h1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.h2;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.h2_b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.h3;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.x1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.x2;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.x2_b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.x3;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.y1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.y2;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.y2_b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.y3;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
|
||||
|
||||
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.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.subsystems.AprilTag;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Intake;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Transfer;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
|
||||
@Config
|
||||
@Autonomous (preselectTeleOp = "TeleopV1")
|
||||
public class Blue extends LinearOpMode {
|
||||
|
||||
Robot robot;
|
||||
|
||||
MultipleTelemetry TELE;
|
||||
|
||||
MecanumDrive drive;
|
||||
|
||||
AprilTag aprilTag;
|
||||
|
||||
Shooter shooter;
|
||||
|
||||
|
||||
|
||||
public static double angle = 0.1;
|
||||
|
||||
Spindexer spindexer;
|
||||
|
||||
Transfer transfer;
|
||||
|
||||
public class shooterOn implements Action {
|
||||
|
||||
int ticker = 1;
|
||||
|
||||
double stamp = 0.0;
|
||||
|
||||
|
||||
@Override
|
||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||
|
||||
if (ticker ==1){
|
||||
stamp = getRuntime();
|
||||
|
||||
}
|
||||
|
||||
ticker ++;
|
||||
|
||||
if (getRuntime() - stamp < 0.2){
|
||||
return true;
|
||||
} else if (getRuntime() - stamp < 0.4) {
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
shooter.update();
|
||||
|
||||
return true;
|
||||
} else {
|
||||
|
||||
return false;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
|
||||
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
TELE = new MultipleTelemetry(
|
||||
telemetry, FtcDashboard.getInstance().getTelemetry()
|
||||
);
|
||||
|
||||
drive = new MecanumDrive(hardwareMap, new Pose2d(
|
||||
0,0,0
|
||||
));
|
||||
|
||||
aprilTag = new AprilTag(robot, TELE);
|
||||
|
||||
shooter = new Shooter(robot, TELE);
|
||||
|
||||
spindexer = new Spindexer(robot, TELE);
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
shooter.setShooterMode("MANUAL");
|
||||
|
||||
shooter.sethoodPosition(0.53);
|
||||
|
||||
transfer = new Transfer(robot);
|
||||
|
||||
transfer.setTransferPosition(transferServo_out);
|
||||
|
||||
Intake intake = new Intake(robot);
|
||||
|
||||
robot.hood.setPosition(hoodDefault);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
|
||||
|
||||
|
||||
TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
.turnTo(Math.toRadians(-135))
|
||||
.strafeToLinearHeading(new Vector2d(x2, -y2), -h2 );
|
||||
|
||||
|
||||
TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, -y2, -h2))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
|
||||
|
||||
TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
|
||||
.strafeToLinearHeading(new Vector2d(x2_b, -y2_b), -h2_b )
|
||||
|
||||
.strafeToLinearHeading(new Vector2d(x3, -y3), -h3 );
|
||||
|
||||
|
||||
TrajectoryActionBuilder traj5 = drive.actionBuilder(new Pose2d(x3, -y3, -h3))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
|
||||
|
||||
|
||||
TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1-30), h1 );
|
||||
|
||||
while(opModeInInit()) {
|
||||
|
||||
if (gamepad2.dpadUpWasPressed()){
|
||||
hoodDefault -= 0.01;
|
||||
}
|
||||
|
||||
if (gamepad2.dpadDownWasPressed()){
|
||||
hoodDefault += 0.01;
|
||||
}
|
||||
|
||||
robot.hood.setPosition(hoodDefault);
|
||||
|
||||
shooter.setTurretPosition(turret_blue);
|
||||
|
||||
aprilTag.initTelemetry();
|
||||
|
||||
aprilTag.update();
|
||||
shooter.update();
|
||||
|
||||
TELE.addData("hood", hoodDefault);
|
||||
|
||||
TELE.update();
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
if (opModeIsActive()){
|
||||
|
||||
robot.hood.setPosition(hoodDefault);
|
||||
|
||||
transfer.setTransferPower(1);
|
||||
|
||||
transfer.update();
|
||||
|
||||
|
||||
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
traj1.build(),
|
||||
new shooterOn()
|
||||
)
|
||||
);
|
||||
|
||||
|
||||
transfer.setTransferPower(1);
|
||||
|
||||
transfer.update();
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
double stamp = getRuntime();
|
||||
|
||||
|
||||
stamp = getRuntime();
|
||||
|
||||
while (getRuntime()-stamp<4.5) {
|
||||
|
||||
|
||||
|
||||
shooter.moveTurret(0.3);
|
||||
|
||||
double time = getRuntime()-stamp;
|
||||
|
||||
|
||||
if (time < 0.3) {
|
||||
|
||||
|
||||
transfer.transferOut();
|
||||
transfer.setTransferPower(1);
|
||||
} else if (time < 1) {
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
|
||||
} else if (time < 1.4) {
|
||||
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 2.6) {
|
||||
transfer.transferOut();
|
||||
|
||||
spindexer.outtake2();
|
||||
|
||||
|
||||
|
||||
} else if (time < 3.0) {
|
||||
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 4.0) {
|
||||
|
||||
transfer.transferOut();
|
||||
|
||||
spindexer.outtake1();
|
||||
|
||||
} else if (time < 4.4) {
|
||||
transfer.transferIn();
|
||||
} else {
|
||||
|
||||
transfer.transferOut();
|
||||
spindexer.outtake3();
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
}
|
||||
|
||||
transfer.update();
|
||||
|
||||
shooter.update();
|
||||
|
||||
spindexer.update();
|
||||
|
||||
}
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
robot.intake.setPower(1);
|
||||
|
||||
Actions.runBlocking(
|
||||
traj2.build()
|
||||
);
|
||||
|
||||
|
||||
Actions.runBlocking(
|
||||
traj3.build()
|
||||
);
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
|
||||
|
||||
|
||||
robot.intake.setPower(0);
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
|
||||
stamp = getRuntime();
|
||||
|
||||
shooter.setManualPower(1);
|
||||
while (getRuntime()-stamp<4.5) {
|
||||
|
||||
|
||||
|
||||
shooter.moveTurret(0.3);
|
||||
|
||||
double time = getRuntime()-stamp;
|
||||
|
||||
|
||||
if (time < 0.3) {
|
||||
|
||||
|
||||
transfer.transferOut();
|
||||
transfer.setTransferPower(1);
|
||||
} else if (time < 1) {
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
|
||||
} else if (time < 1.4) {
|
||||
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 2.6) {
|
||||
transfer.transferOut();
|
||||
|
||||
spindexer.outtake2();
|
||||
|
||||
|
||||
|
||||
} else if (time < 3.0) {
|
||||
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 4.0) {
|
||||
|
||||
transfer.transferOut();
|
||||
|
||||
spindexer.outtake1();
|
||||
|
||||
} else if (time < 4.4) {
|
||||
transfer.transferIn();
|
||||
} else {
|
||||
|
||||
transfer.transferOut();
|
||||
spindexer.outtake3();
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
}
|
||||
|
||||
transfer.update();
|
||||
|
||||
shooter.update();
|
||||
|
||||
spindexer.update();
|
||||
|
||||
}
|
||||
|
||||
spindexer.outtake3();
|
||||
robot.intake.setPower(1);
|
||||
|
||||
Actions.runBlocking(
|
||||
traj4.build()
|
||||
);
|
||||
|
||||
|
||||
Actions.runBlocking(
|
||||
traj5.build()
|
||||
);
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
|
||||
|
||||
|
||||
robot.intake.setPower(0);
|
||||
|
||||
|
||||
stamp = getRuntime();
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
while (getRuntime()-stamp<4.5) {
|
||||
|
||||
|
||||
|
||||
shooter.moveTurret(0.3);
|
||||
|
||||
double time = getRuntime()-stamp;
|
||||
|
||||
|
||||
if (time < 0.3) {
|
||||
|
||||
|
||||
transfer.transferOut();
|
||||
transfer.setTransferPower(1);
|
||||
} else if (time < 1) {
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
|
||||
} else if (time < 1.4) {
|
||||
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 2.6) {
|
||||
transfer.transferOut();
|
||||
|
||||
spindexer.outtake2();
|
||||
|
||||
|
||||
|
||||
} else if (time < 3.0) {
|
||||
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 4.0) {
|
||||
|
||||
transfer.transferOut();
|
||||
|
||||
spindexer.outtake1();
|
||||
|
||||
} else if (time < 4.4) {
|
||||
transfer.transferIn();
|
||||
} else {
|
||||
|
||||
transfer.transferOut();
|
||||
spindexer.outtake3();
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
}
|
||||
|
||||
transfer.update();
|
||||
|
||||
shooter.update();
|
||||
|
||||
spindexer.update();
|
||||
|
||||
}
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
Actions.runBlocking(
|
||||
traj6.build()
|
||||
);
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
TELE.addLine("finished");
|
||||
|
||||
TELE.update();
|
||||
|
||||
sleep (2000);
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,431 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
|
||||
|
||||
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.SequentialAction;
|
||||
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
|
||||
import com.acmerobotics.roadrunner.Vector2d;
|
||||
import com.acmerobotics.roadrunner.ftc.Actions;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
|
||||
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Intake;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Transfer;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
|
||||
|
||||
|
||||
|
||||
@Config
|
||||
@Autonomous
|
||||
public class Red extends LinearOpMode {
|
||||
|
||||
Robot robot;
|
||||
|
||||
MultipleTelemetry TELE;
|
||||
|
||||
MecanumDrive drive;
|
||||
|
||||
AprilTag aprilTag;
|
||||
|
||||
Shooter shooter;
|
||||
|
||||
|
||||
|
||||
public static double angle = 0.1;
|
||||
|
||||
Spindexer spindexer;
|
||||
|
||||
Transfer transfer;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
|
||||
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
TELE = new MultipleTelemetry(
|
||||
telemetry, FtcDashboard.getInstance().getTelemetry()
|
||||
);
|
||||
|
||||
drive = new MecanumDrive(hardwareMap, new Pose2d(
|
||||
0,0,0
|
||||
));
|
||||
|
||||
aprilTag = new AprilTag(robot, TELE);
|
||||
|
||||
shooter = new Shooter(robot, TELE);
|
||||
|
||||
spindexer = new Spindexer(robot, TELE);
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
shooter.setShooterMode("MANUAL");
|
||||
|
||||
shooter.sethoodPosition(hoodDefault);
|
||||
|
||||
transfer = new Transfer(robot);
|
||||
|
||||
transfer.setTransferPosition(transferServo_out);
|
||||
|
||||
Intake intake = new Intake(robot);
|
||||
|
||||
robot.hood.setPosition(hoodDefault);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
|
||||
|
||||
|
||||
TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
.turnTo(Math.toRadians(135))
|
||||
.strafeToLinearHeading(new Vector2d(x2, y2), h2 );
|
||||
|
||||
|
||||
TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(x2, y2, h2))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
|
||||
|
||||
TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
|
||||
.strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b )
|
||||
|
||||
.strafeToLinearHeading(new Vector2d(x3, y3), h3 );
|
||||
|
||||
|
||||
TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(x3, y3, h3))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
|
||||
|
||||
|
||||
TrajectoryActionBuilder park = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1+30), h1 );
|
||||
|
||||
while(opModeInInit()) {
|
||||
|
||||
if (gamepad2.dpadUpWasPressed()){
|
||||
hoodDefault -= 0.01;
|
||||
}
|
||||
|
||||
if (gamepad2.dpadDownWasPressed()){
|
||||
hoodDefault += 0.01;
|
||||
}
|
||||
|
||||
robot.hood.setPosition(hoodDefault);
|
||||
|
||||
shooter.setTurretPosition(turret_red);
|
||||
|
||||
aprilTag.initTelemetry();
|
||||
|
||||
aprilTag.update();
|
||||
shooter.update();
|
||||
|
||||
TELE.update();
|
||||
}
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
if (opModeIsActive()){
|
||||
|
||||
robot.hood.setPosition(hoodDefault);
|
||||
|
||||
transfer.setTransferPower(1);
|
||||
|
||||
transfer.update();
|
||||
|
||||
|
||||
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
shoot0.build()
|
||||
)
|
||||
);
|
||||
|
||||
|
||||
transfer.setTransferPower(1);
|
||||
|
||||
transfer.update();
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
double stamp = getRuntime();
|
||||
|
||||
|
||||
stamp = getRuntime();
|
||||
|
||||
while (getRuntime()-stamp<4.5) {
|
||||
|
||||
|
||||
|
||||
shooter.moveTurret(0.31);
|
||||
|
||||
double time = getRuntime()-stamp;
|
||||
|
||||
|
||||
if (time < 0.3) {
|
||||
|
||||
|
||||
transfer.transferOut();
|
||||
transfer.setTransferPower(1);
|
||||
} else if (time < 1) {
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
|
||||
} else if (time < 1.4) {
|
||||
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 2.6) {
|
||||
transfer.transferOut();
|
||||
|
||||
spindexer.outtake2();
|
||||
|
||||
|
||||
|
||||
} else if (time < 3.0) {
|
||||
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 4.0) {
|
||||
|
||||
transfer.transferOut();
|
||||
|
||||
spindexer.outtake1();
|
||||
|
||||
} else if (time < 4.4) {
|
||||
transfer.transferIn();
|
||||
} else {
|
||||
|
||||
transfer.transferOut();
|
||||
spindexer.outtake3();
|
||||
|
||||
shooter.setManualPower(0);
|
||||
|
||||
}
|
||||
|
||||
transfer.update();
|
||||
|
||||
shooter.update();
|
||||
|
||||
spindexer.update();
|
||||
|
||||
}
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
robot.intake.setPower(1);
|
||||
|
||||
Actions.runBlocking(
|
||||
pickup1.build()
|
||||
);
|
||||
|
||||
|
||||
Actions.runBlocking(
|
||||
shoot1.build()
|
||||
);
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
|
||||
|
||||
|
||||
robot.intake.setPower(0);
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
|
||||
stamp = getRuntime();
|
||||
|
||||
shooter.setManualPower(1);
|
||||
while (getRuntime()-stamp<4.5) {
|
||||
|
||||
|
||||
|
||||
shooter.moveTurret(0.31);
|
||||
|
||||
double time = getRuntime()-stamp;
|
||||
|
||||
|
||||
if (time < 0.3) {
|
||||
|
||||
|
||||
transfer.transferOut();
|
||||
transfer.setTransferPower(1);
|
||||
} else if (time < 1) {
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
|
||||
} else if (time < 1.4) {
|
||||
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 2.6) {
|
||||
transfer.transferOut();
|
||||
|
||||
spindexer.outtake2();
|
||||
|
||||
|
||||
|
||||
} else if (time < 3.0) {
|
||||
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 4.0) {
|
||||
|
||||
transfer.transferOut();
|
||||
|
||||
spindexer.outtake1();
|
||||
|
||||
} else if (time < 4.4) {
|
||||
transfer.transferIn();
|
||||
} else {
|
||||
|
||||
transfer.transferOut();
|
||||
spindexer.outtake3();
|
||||
|
||||
shooter.setManualPower(0);
|
||||
|
||||
}
|
||||
|
||||
transfer.update();
|
||||
|
||||
shooter.update();
|
||||
|
||||
spindexer.update();
|
||||
|
||||
}
|
||||
|
||||
spindexer.outtake3();
|
||||
robot.intake.setPower(1);
|
||||
|
||||
Actions.runBlocking(
|
||||
pickup2.build()
|
||||
);
|
||||
|
||||
|
||||
Actions.runBlocking(
|
||||
shoot2.build()
|
||||
);
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
|
||||
|
||||
|
||||
robot.intake.setPower(0);
|
||||
|
||||
|
||||
stamp = getRuntime();
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
while (getRuntime()-stamp<4.5) {
|
||||
|
||||
|
||||
|
||||
shooter.moveTurret(0.31);
|
||||
|
||||
double time = getRuntime()-stamp;
|
||||
|
||||
|
||||
if (time < 0.3) {
|
||||
|
||||
|
||||
transfer.transferOut();
|
||||
transfer.setTransferPower(1);
|
||||
} else if (time < 1) {
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
|
||||
} else if (time < 1.4) {
|
||||
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 2.6) {
|
||||
transfer.transferOut();
|
||||
|
||||
spindexer.outtake2();
|
||||
|
||||
|
||||
|
||||
} else if (time < 3.0) {
|
||||
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 4.0) {
|
||||
|
||||
transfer.transferOut();
|
||||
|
||||
spindexer.outtake1();
|
||||
|
||||
} else if (time < 4.4) {
|
||||
transfer.transferIn();
|
||||
} else {
|
||||
|
||||
transfer.transferOut();
|
||||
spindexer.outtake3();
|
||||
|
||||
shooter.setManualPower(0);
|
||||
|
||||
}
|
||||
|
||||
transfer.update();
|
||||
|
||||
shooter.update();
|
||||
|
||||
spindexer.update();
|
||||
|
||||
}
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
Actions.runBlocking(
|
||||
park.build()
|
||||
);
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
TELE.addLine("finished");
|
||||
|
||||
TELE.update();
|
||||
|
||||
sleep (2000);
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,298 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
|
||||
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
|
||||
|
||||
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.ParallelAction;
|
||||
import com.acmerobotics.roadrunner.Pose2d;
|
||||
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
|
||||
import com.acmerobotics.roadrunner.Vector2d;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.acmerobotics.roadrunner.Action;
|
||||
import com.acmerobotics.roadrunner.ftc.Actions;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
@Config
|
||||
@Autonomous
|
||||
public class redDaniel extends LinearOpMode {
|
||||
|
||||
Robot robot;
|
||||
|
||||
MultipleTelemetry TELE;
|
||||
|
||||
MecanumDrive drive;
|
||||
|
||||
AprilTag aprilTag;
|
||||
|
||||
int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
|
||||
|
||||
int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
|
||||
|
||||
int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
|
||||
// TODO: change this velocity PID
|
||||
public Action initShooter(int velocity){
|
||||
return new Action(){
|
||||
double velo = 0.0;
|
||||
double initPos = 0.0;
|
||||
double stamp = 0.0;
|
||||
double powPID = 0.0;
|
||||
double ticker = 0.0;
|
||||
public boolean run(@NonNull TelemetryPacket telemetryPacket){
|
||||
velo = -60 * ((((double) robot.shooter1.getCurrentPosition() / 2048) - initPos) / (getRuntime() - stamp));
|
||||
stamp = getRuntime();
|
||||
initPos = (double) robot.shooter1.getCurrentPosition() / 2048;
|
||||
if (Math.abs(velocity - velo) > initTolerance) {
|
||||
powPID = (double) velocity / maxVel;
|
||||
ticker = getRuntime();
|
||||
} else if (velocity - velTolerance > velo) {
|
||||
powPID = powPID + 0.0001;
|
||||
ticker = getRuntime();
|
||||
} else if (velocity + velTolerance < velo) {
|
||||
powPID = powPID - 0.0001;
|
||||
ticker = getRuntime();
|
||||
}
|
||||
robot.shooter1.setPower(powPID);
|
||||
robot.shooter2.setPower(powPID);
|
||||
robot.transfer.setPower((powPID / 4) + 0.75);
|
||||
|
||||
return getRuntime() - ticker < 0.5;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
public void Obelisk (){
|
||||
// TODO: write the code to detect order
|
||||
}
|
||||
|
||||
public Action Shoot(double spindexer){
|
||||
return new Action() {
|
||||
boolean transfer = false;
|
||||
@Override
|
||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||
robot.spin1.setPosition(spindexer);
|
||||
robot.spin2.setPosition(1-spindexer);
|
||||
if (scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01 && scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01){
|
||||
robot.transferServo.setPosition(transferServo_in);
|
||||
transfer = true;
|
||||
}
|
||||
if (scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3) < transferServo_in + 0.01 && scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3) > transferServo_in - 0.01 && transfer){
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
public Action intake (){
|
||||
return new Action() {
|
||||
double position = 0.0;
|
||||
final double intakeTime = 4.0; // TODO: change this so it serves as a backup
|
||||
final double stamp = getRuntime();
|
||||
@Override
|
||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||
if ((getRuntime() % 0.3) >0.15) {
|
||||
position = spindexer_intakePos1 + 0.02;
|
||||
} else {
|
||||
position = spindexer_intakePos1 - 0.02;
|
||||
}
|
||||
robot.spin1.setPosition(position);
|
||||
robot.spin2.setPosition(1-position);
|
||||
|
||||
robot.intake.setPower(1);
|
||||
|
||||
return !(robot.pin1.getState() && robot.pin3.getState() && robot.pin5.getState()) || getRuntime() - stamp > intakeTime;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
public Action ColorDetect (){
|
||||
return new Action() {
|
||||
int t1 = 0;
|
||||
int t2 = 0;
|
||||
int t3 = 0;
|
||||
int tP1 = 0;
|
||||
int tP2 = 0;
|
||||
int tP3 = 0;
|
||||
final double stamp = getRuntime();
|
||||
final double detectTime = 3.0;
|
||||
double position = 0.0;
|
||||
@Override
|
||||
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
|
||||
if ((getRuntime() % 0.3) >0.15) {
|
||||
position = spindexer_intakePos1 + 0.02;
|
||||
} else {
|
||||
position = spindexer_intakePos1 - 0.02;
|
||||
}
|
||||
robot.spin1.setPosition(position);
|
||||
robot.spin2.setPosition(1-position);
|
||||
if (robot.pin1.getState()) {
|
||||
t1 += 1;
|
||||
if (robot.pin0.getState()){
|
||||
tP1 += 1;
|
||||
}
|
||||
}
|
||||
if (robot.pin3.getState()) {
|
||||
t2 += 1;
|
||||
if (robot.pin0.getState()){
|
||||
tP2 += 1;
|
||||
}
|
||||
}
|
||||
if (robot.pin5.getState()) {
|
||||
t3 += 1;
|
||||
if (robot.pin0.getState()){
|
||||
tP3 += 1;
|
||||
}
|
||||
}
|
||||
if (t1 > 20){
|
||||
if (tP1 > 20){
|
||||
b1 = 2;
|
||||
} else {
|
||||
b1 = 1;
|
||||
}
|
||||
}
|
||||
if (t2 > 20){
|
||||
if (tP2 > 20){
|
||||
b2 = 2;
|
||||
} else {
|
||||
b2 = 1;
|
||||
}
|
||||
}
|
||||
if (t3 > 20){
|
||||
if (tP3 > 20){
|
||||
b3 = 2;
|
||||
} else {
|
||||
b3 = 1;
|
||||
}
|
||||
}
|
||||
return !(b1 + b2 + b3 >= 5) || (getRuntime() - stamp < detectTime);
|
||||
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
|
||||
TELE = new MultipleTelemetry(
|
||||
telemetry, FtcDashboard.getInstance().getTelemetry()
|
||||
);
|
||||
|
||||
drive = new MecanumDrive(hardwareMap, new Pose2d(
|
||||
0, 0, 0
|
||||
));
|
||||
|
||||
aprilTag = new AprilTag(robot, TELE);
|
||||
|
||||
TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
|
||||
|
||||
TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
.turnTo(Math.toRadians(h2))
|
||||
.strafeToLinearHeading(new Vector2d(x2, y2), h2);
|
||||
|
||||
TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(x2, y2, h2))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
|
||||
|
||||
TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
|
||||
.strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b)
|
||||
|
||||
.strafeToLinearHeading(new Vector2d(x3, y3), h3);
|
||||
|
||||
TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(x3, y3, h3))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
|
||||
|
||||
TrajectoryActionBuilder park = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1 + 30), h1);
|
||||
|
||||
while (opModeInInit()) {
|
||||
|
||||
if (gamepad2.dpadUpWasPressed()) {
|
||||
hoodDefault -= 0.01;
|
||||
}
|
||||
|
||||
if (gamepad2.dpadDownWasPressed()) {
|
||||
hoodDefault += 0.01;
|
||||
}
|
||||
|
||||
robot.hood.setPosition(hoodDefault);
|
||||
|
||||
robot.turr1.setPosition(turret_red);
|
||||
robot.turr2.setPosition(1 - turret_red);
|
||||
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
|
||||
aprilTag.initTelemetry();
|
||||
|
||||
aprilTag.update();
|
||||
|
||||
TELE.update();
|
||||
}
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
if (opModeIsActive()) {
|
||||
|
||||
robot.hood.setPosition(hoodDefault);
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
shoot0.build()
|
||||
)
|
||||
);
|
||||
|
||||
|
||||
|
||||
Actions.runBlocking(
|
||||
pickup1.build()
|
||||
);
|
||||
|
||||
Actions.runBlocking(
|
||||
shoot1.build()
|
||||
);
|
||||
|
||||
|
||||
Actions.runBlocking(
|
||||
pickup2.build()
|
||||
);
|
||||
|
||||
Actions.runBlocking(
|
||||
shoot2.build()
|
||||
);
|
||||
|
||||
|
||||
Actions.runBlocking(
|
||||
park.build()
|
||||
);
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
teleStart = drive.localizer.getPose();
|
||||
|
||||
TELE.addLine("finished");
|
||||
|
||||
TELE.update();
|
||||
|
||||
sleep(2000);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,4 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.constants;
|
||||
|
||||
public class Color {
|
||||
}
|
||||
@@ -1,29 +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 goalHeight = 42; //in inches
|
||||
|
||||
public static double turretHeight = 12;
|
||||
|
||||
public static double relativeGoalHeight = goalHeight - turretHeight;
|
||||
|
||||
public static double x1 = 50, y1 = 0, h1 = 0;
|
||||
|
||||
public static double x2 = 31, y2 = 32, h2 = Math.toRadians(140);
|
||||
|
||||
public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(140);
|
||||
|
||||
|
||||
public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140);
|
||||
|
||||
public static Pose2d teleStart = new Pose2d(x1,-10,0);
|
||||
|
||||
|
||||
|
||||
}
|
||||
@@ -1,34 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.constants;
|
||||
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
|
||||
@Config
|
||||
public class ServoPositions {
|
||||
|
||||
public static double spindexer_intakePos1 = 0.665;
|
||||
|
||||
public static double spindexer_intakePos3 = 0.29;
|
||||
|
||||
public static double spindexer_intakePos2 = 0.99;
|
||||
|
||||
public static double spindexer_outtakeBall3 = 0.845;
|
||||
|
||||
public static double spindexer_outtakeBall2 = 0.48;
|
||||
public static double spindexer_outtakeBall1 = 0.1;
|
||||
|
||||
public static double transferServo_out = 0.15;
|
||||
|
||||
public static double transferServo_in = 0.38;
|
||||
|
||||
public static double hoodDefault = 0.35;
|
||||
|
||||
public static double hoodHigh = 0.21;
|
||||
|
||||
public static double hoodLow = 1.0;
|
||||
|
||||
public static double turret_red = 0.43;
|
||||
public static double turret_blue = 0.4;
|
||||
|
||||
|
||||
}
|
||||
@@ -1,16 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.constants;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
|
||||
@Config
|
||||
public class ShooterVars {
|
||||
public static double turret_GearRatio = 0.9974;
|
||||
|
||||
public static double turret_Range = 355;
|
||||
|
||||
public static int velTolerance = 300;
|
||||
|
||||
public static int initTolerance = 1000;
|
||||
|
||||
public static int maxVel = 4000;
|
||||
}
|
||||
@@ -1,4 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.constants;
|
||||
|
||||
public class blank {
|
||||
}
|
||||
@@ -1,5 +1,6 @@
|
||||
package org.firstinspires.ftc.teamcode.libs.RR;
|
||||
|
||||
|
||||
import androidx.annotation.NonNull;
|
||||
|
||||
import com.acmerobotics.dashboard.canvas.Canvas;
|
||||
@@ -34,7 +35,6 @@ import com.qualcomm.hardware.lynx.LynxModule;
|
||||
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
import com.qualcomm.robotcore.hardware.VoltageSensor;
|
||||
@@ -58,33 +58,33 @@ public final class MecanumDrive {
|
||||
// TODO: fill in these values based on
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
|
||||
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
|
||||
RevHubOrientationOnRobot.LogoFacingDirection.RIGHT;
|
||||
RevHubOrientationOnRobot.LogoFacingDirection.UP;
|
||||
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
|
||||
RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD;
|
||||
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
|
||||
|
||||
// drive model parameters
|
||||
public double inPerTick = 0.001978956;
|
||||
public double lateralInPerTick = 0.0013863732202094405;
|
||||
public double trackWidthTicks = 6488.883015684446;
|
||||
public double inPerTick = 1;
|
||||
public double lateralInPerTick = inPerTick;
|
||||
public double trackWidthTicks = 0;
|
||||
|
||||
// feedforward parameters (in tick units)
|
||||
public double kS = 1.2147826978829488;
|
||||
public double kV = 0.00032;
|
||||
public double kA = 0.000046;
|
||||
public double kS = 0;
|
||||
public double kV = 0;
|
||||
public double kA = 0;
|
||||
|
||||
// path profile parameters (in inches)
|
||||
public double maxWheelVel = 120;
|
||||
public double maxWheelVel = 50;
|
||||
public double minProfileAccel = -30;
|
||||
public double maxProfileAccel = 120;
|
||||
public double maxProfileAccel = 50;
|
||||
|
||||
// turn profile parameters (in radians)
|
||||
public double maxAngVel = 2* Math.PI; // shared with path
|
||||
public double maxAngAccel = 2* Math.PI;
|
||||
public double maxAngVel = Math.PI; // shared with path
|
||||
public double maxAngAccel = Math.PI;
|
||||
|
||||
// path controller gains
|
||||
public double axialGain = 4;
|
||||
public double lateralGain = 4;
|
||||
public double headingGain = 4; // shared with turn
|
||||
public double axialGain = 0.0;
|
||||
public double lateralGain = 0.0;
|
||||
public double headingGain = 0.0; // shared with turn
|
||||
|
||||
public double axialVelGain = 0.0;
|
||||
public double lateralVelGain = 0.0;
|
||||
@@ -212,6 +212,7 @@ public final class MecanumDrive {
|
||||
headingDelta
|
||||
));
|
||||
|
||||
|
||||
return twist.velocity().value();
|
||||
}
|
||||
}
|
||||
@@ -223,12 +224,12 @@ public final class MecanumDrive {
|
||||
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
|
||||
}
|
||||
|
||||
// TODO: make sure your config has motors with these names (or change them)
|
||||
// TODO: make sure your HardwareConfig has motors with these names (or change them)
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
||||
leftFront = hardwareMap.get(DcMotorEx.class, "fl");
|
||||
leftBack = hardwareMap.get(DcMotorEx.class, "bl");
|
||||
rightBack = hardwareMap.get(DcMotorEx.class, "br");
|
||||
rightFront = hardwareMap.get(DcMotorEx.class, "fr");
|
||||
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
|
||||
leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
|
||||
rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
|
||||
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
|
||||
|
||||
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
@@ -236,19 +237,16 @@ public final class MecanumDrive {
|
||||
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
// TODO: reverse motor directions if needed
|
||||
//
|
||||
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
// leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
// TODO: make sure your config has an IMU with this name (can be BNO or BHI)
|
||||
// TODO: make sure your HardwareConfig has an IMU with this name (can be BNO or BHI)
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
||||
lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
|
||||
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
|
||||
|
||||
voltageSensor = hardwareMap.voltageSensor.iterator().next();
|
||||
|
||||
localizer = new PinpointLocalizer(hardwareMap, PARAMS.inPerTick, pose);
|
||||
localizer = new DriveLocalizer(pose);
|
||||
|
||||
FlightRecorder.write("MECANUM_PARAMS", PARAMS);
|
||||
}
|
||||
|
||||
@@ -28,7 +28,7 @@ public class OTOSLocalizer implements Localizer {
|
||||
private Pose2d currentPose;
|
||||
|
||||
public OTOSLocalizer(HardwareMap hardwareMap, Pose2d initialPose) {
|
||||
// TODO: make sure your config has an OTOS device with this name
|
||||
// TODO: make sure your HardwareConfig has an OTOS device with this name
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
||||
otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos");
|
||||
currentPose = initialPose;
|
||||
|
||||
@@ -16,8 +16,8 @@ import java.util.Objects;
|
||||
@Config
|
||||
public final class PinpointLocalizer implements Localizer {
|
||||
public static class Params {
|
||||
public double parYTicks = -3765.023079161767; // y position of the parallel encoder (in tick units)
|
||||
public double perpXTicks = -1962.6377639490684; // x position of the perpendicular encoder (in tick units)
|
||||
public double parYTicks = 0.0; // y position of the parallel encoder (in tick units)
|
||||
public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units)
|
||||
}
|
||||
|
||||
public static Params PARAMS = new Params();
|
||||
@@ -29,7 +29,7 @@ public final class PinpointLocalizer implements Localizer {
|
||||
private Pose2d txPinpointRobot = new Pose2d(0, 0, 0);
|
||||
|
||||
public PinpointLocalizer(HardwareMap hardwareMap, double inPerTick, Pose2d initialPose) {
|
||||
// TODO: make sure your config has a Pinpoint device with this name
|
||||
// TODO: make sure your HardwareConfig has a Pinpoint device with this name
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
||||
driver = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint");
|
||||
|
||||
@@ -38,7 +38,7 @@ public final class PinpointLocalizer implements Localizer {
|
||||
driver.setOffsets(mmPerTick * PARAMS.parYTicks, mmPerTick * PARAMS.perpXTicks, DistanceUnit.MM);
|
||||
|
||||
// TODO: reverse encoder directions if needed
|
||||
initialParDirection = GoBildaPinpointDriver.EncoderDirection.REVERSED;
|
||||
initialParDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD;
|
||||
initialPerpDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD;
|
||||
|
||||
driver.setEncoderDirections(initialParDirection, initialPerpDirection);
|
||||
|
||||
@@ -232,7 +232,7 @@ public final class TankDrive {
|
||||
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
|
||||
}
|
||||
|
||||
// TODO: make sure your config has motors with these names (or change them)
|
||||
// TODO: make sure your HardwareConfig has motors with these names (or change them)
|
||||
// add additional motors on each side if you have them
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
||||
leftMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "left"));
|
||||
@@ -248,7 +248,7 @@ public final class TankDrive {
|
||||
// TODO: reverse motor directions if needed
|
||||
// leftMotors.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
// TODO: make sure your config has an IMU with this name (can be BNO or BHI)
|
||||
// TODO: make sure your HardwareConfig has an IMU with this name (can be BNO or BHI)
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
||||
lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
|
||||
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
|
||||
|
||||
@@ -37,7 +37,7 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
|
||||
private Pose2d pose;
|
||||
|
||||
public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick, Pose2d initialPose) {
|
||||
// TODO: make sure your config has **motors** with these names (or change them)
|
||||
// TODO: make sure your HardwareConfig has **motors** with these names (or change them)
|
||||
// the encoders should be plugged into the slot matching the named motor
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
||||
par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par0")));
|
||||
|
||||
@@ -46,7 +46,7 @@ public final class TwoDeadWheelLocalizer implements Localizer {
|
||||
private Pose2d pose;
|
||||
|
||||
public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick, Pose2d initialPose) {
|
||||
// TODO: make sure your config has **motors** with these names (or change them)
|
||||
// TODO: make sure your HardwareConfig has **motors** with these names (or change them)
|
||||
// the encoders should be plugged into the slot matching the named motor
|
||||
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
||||
par = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par")));
|
||||
|
||||
@@ -1,238 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystems;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
|
||||
import org.firstinspires.ftc.vision.VisionPortal;
|
||||
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
|
||||
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Collections;
|
||||
import java.util.List;
|
||||
|
||||
public class AprilTag implements Subsystem {
|
||||
|
||||
private AprilTagProcessor aprilTag;
|
||||
private VisionPortal visionPortal;
|
||||
|
||||
private MultipleTelemetry TELE;
|
||||
|
||||
private boolean teleOn = false;
|
||||
|
||||
private int detections = 0;
|
||||
|
||||
List<AprilTagDetection> currentDetections;
|
||||
|
||||
ArrayList<ArrayList<Double>> Data = new ArrayList<>();
|
||||
|
||||
|
||||
|
||||
public AprilTag(Robot robot, MultipleTelemetry tele) {
|
||||
|
||||
|
||||
|
||||
this.aprilTag = robot.aprilTagProcessor;
|
||||
|
||||
|
||||
this.TELE = tele;
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
currentDetections = aprilTag.getDetections();
|
||||
|
||||
UpdateData();
|
||||
|
||||
if(teleOn){
|
||||
tagTELE();
|
||||
initTelemetry();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void initTelemetry (){
|
||||
|
||||
TELE.addData("Camera Preview", "Check Driver Station for stream");
|
||||
TELE.addData("Status", "Initialized - Press START");
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void tagTELE () {
|
||||
|
||||
TELE.addData("# AprilTags Detected", detections);
|
||||
|
||||
// Display info for each detected tag
|
||||
for (ArrayList<Double> detection : Data) {
|
||||
if (detection.get(0) ==1) {
|
||||
// Known AprilTag with metadata
|
||||
TELE.addLine(String.format("\n==== (ID %d) %s ====",
|
||||
detection.get(1).intValue(), ""));
|
||||
|
||||
TELE.addLine(String.format("XYZ: %6.1f %6.1f %6.1f (inch)",
|
||||
detection.get(2),
|
||||
detection.get(3),
|
||||
detection.get(4)));
|
||||
|
||||
TELE.addData("Distance", getDistance(detection.get(1).intValue()));
|
||||
|
||||
TELE.addLine(String.format("PRY: %6.1f %6.1f %6.1f (deg)",
|
||||
detection.get(5),
|
||||
detection.get(6),
|
||||
detection.get(7)));
|
||||
|
||||
TELE.addLine(String.format("RBE: %6.1f %6.1f %6.1f (inch, deg, deg)",
|
||||
detection.get(8),
|
||||
detection.get(9),
|
||||
detection.get(10)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public void turnTelemetryOn(boolean bool) {
|
||||
|
||||
teleOn = bool;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void UpdateData () {
|
||||
|
||||
Data.clear(); // <--- THIS FIXES YOUR ISSUE
|
||||
|
||||
|
||||
detections = currentDetections.size();
|
||||
|
||||
|
||||
for (AprilTagDetection detection : currentDetections) {
|
||||
|
||||
ArrayList<Double> detectionData = new ArrayList<Double>();
|
||||
|
||||
|
||||
|
||||
if (detection.metadata != null) {
|
||||
|
||||
detectionData.add(1.0);
|
||||
// Known AprilTag with metadata
|
||||
|
||||
detectionData.add( (double) detection.id);
|
||||
|
||||
|
||||
|
||||
detectionData.add(detection.ftcPose.x);
|
||||
detectionData.add(detection.ftcPose.y);
|
||||
detectionData.add(detection.ftcPose.z);
|
||||
|
||||
|
||||
|
||||
detectionData.add(detection.ftcPose.pitch);
|
||||
detectionData.add(detection.ftcPose.roll);
|
||||
detectionData.add(detection.ftcPose.yaw);
|
||||
|
||||
detectionData.add(detection.ftcPose.range);
|
||||
detectionData.add(detection.ftcPose.bearing);
|
||||
detectionData.add(detection.ftcPose.elevation);
|
||||
|
||||
} else {
|
||||
|
||||
detectionData.add(0, 0.0);
|
||||
|
||||
}
|
||||
|
||||
Data.add(detectionData);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public int getDetectionCount() {
|
||||
|
||||
return detections;
|
||||
|
||||
}
|
||||
|
||||
public boolean isDetected (int id){
|
||||
return (!filterID(Data, (double) id ).isEmpty());
|
||||
}
|
||||
|
||||
|
||||
|
||||
public double getDistance(int id) {
|
||||
ArrayList<Double> d = filterID(Data, (double) id);
|
||||
if (d.size() >= 5) {
|
||||
double x = d.get(2);
|
||||
double y = d.get(3);
|
||||
double z = d.get(4);
|
||||
return Math.sqrt(x*x + y*y + z*z);
|
||||
}
|
||||
return -1; // tag not found
|
||||
}
|
||||
|
||||
// Returns the position as [x, y, z]
|
||||
public List<Double> getPosition(int id) {
|
||||
ArrayList<Double> d = filterID(Data, (double) id);
|
||||
if (d.size() >= 5) {
|
||||
List<Double> pos = new ArrayList<>();
|
||||
pos.add(d.get(2));
|
||||
pos.add(d.get(3));
|
||||
pos.add(d.get(4));
|
||||
return pos;
|
||||
}
|
||||
return Collections.emptyList();
|
||||
}
|
||||
|
||||
// Returns orientation as [pitch, roll, yaw]
|
||||
public List<Double> getOrientation(int id) {
|
||||
ArrayList<Double> d = filterID(Data, (double) id);
|
||||
if (d.size() >= 8) {
|
||||
List<Double> ori = new ArrayList<>();
|
||||
ori.add(d.get(5));
|
||||
ori.add(d.get(6));
|
||||
ori.add(d.get(7));
|
||||
return ori;
|
||||
}
|
||||
return Collections.emptyList();
|
||||
}
|
||||
|
||||
// Returns range, bearing, elevation as [range, bearing, elevation]
|
||||
public List<Double> getRBE(int id) {
|
||||
ArrayList<Double> d = filterID(Data, (double) id);
|
||||
if (d.size() >= 11) {
|
||||
List<Double> rbe = new ArrayList<>();
|
||||
rbe.add(d.get(8));
|
||||
rbe.add(d.get(9));
|
||||
rbe.add(d.get(10));
|
||||
return rbe;
|
||||
}
|
||||
return Collections.emptyList();
|
||||
}
|
||||
|
||||
// Returns full raw data for debugging or custom processing
|
||||
public ArrayList<Double> getRawData(int id) {
|
||||
return filterID(Data, (double) id);
|
||||
}
|
||||
|
||||
public static ArrayList<Double> filterID(ArrayList<ArrayList<Double>> data, double x) {
|
||||
for (ArrayList<Double> innerList : data) {
|
||||
// Ensure it has a second element
|
||||
if (innerList.size() > 1 && Math.abs(innerList.get(1) - x) < 1e-9) {
|
||||
return innerList; // Return the first match
|
||||
}
|
||||
}
|
||||
// Return an empty ArrayList if no match found
|
||||
return new ArrayList<>();
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,99 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystems;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.arcrobotics.ftclib.gamepad.GamepadEx;
|
||||
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
public class Drivetrain implements Subsystem {
|
||||
|
||||
private final GamepadEx gamepad;
|
||||
|
||||
public MultipleTelemetry TELE;
|
||||
|
||||
private String Mode = "Default";
|
||||
|
||||
private final DcMotorEx fl;
|
||||
|
||||
private final DcMotorEx fr;
|
||||
private final DcMotorEx bl;
|
||||
private final DcMotorEx br;
|
||||
|
||||
private double defaultSpeed = 0.7;
|
||||
|
||||
private double slowSpeed = 0.3;
|
||||
public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1){
|
||||
|
||||
this.fl = robot.frontLeft;
|
||||
this.fr = robot.frontRight;
|
||||
this.br = robot.backRight;
|
||||
this.bl = robot.backLeft;
|
||||
|
||||
this.gamepad = gamepad1;
|
||||
|
||||
this.TELE = tele;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void setMode (String mode){
|
||||
this.Mode = mode;
|
||||
}
|
||||
|
||||
public void setDefaultSpeed (double speed){
|
||||
this.defaultSpeed = speed;
|
||||
}
|
||||
|
||||
public void setSlowSpeed (double speed){
|
||||
this.slowSpeed = speed;
|
||||
}
|
||||
|
||||
public void RobotCentric(double fwd, double strafe, double turn, double turbo){
|
||||
|
||||
double y = -fwd; // Remember, Y stick value is reversed
|
||||
double x = strafe * 1.1; // Counteract imperfect strafing
|
||||
double rx = turn;
|
||||
|
||||
// Denominator is the largest motor power (absolute value) or 1
|
||||
// This ensures all the powers maintain the same ratio,
|
||||
// but only if at least one is out of the range [-1, 1]
|
||||
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
|
||||
double frontLeftPower = (y + x + rx) / denominator;
|
||||
double backLeftPower = (y - x + rx) / denominator;
|
||||
double frontRightPower = (y - x - rx) / denominator;
|
||||
double backRightPower = (y + x - rx) / denominator;
|
||||
|
||||
fl.setPower(frontLeftPower*turbo);
|
||||
bl.setPower(backLeftPower*turbo);
|
||||
fr.setPower(frontRightPower*turbo);
|
||||
br.setPower(backRightPower*turbo);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
if (Objects.equals(Mode, "Default")) {
|
||||
|
||||
RobotCentric(
|
||||
gamepad.getRightY(),
|
||||
gamepad.getRightX(),
|
||||
gamepad.getLeftX(),
|
||||
(gamepad.getTrigger(
|
||||
GamepadKeys.Trigger.RIGHT_TRIGGER) * (1-defaultSpeed)
|
||||
- gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed
|
||||
+ defaultSpeed
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,80 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystems;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.arcrobotics.ftclib.gamepad.GamepadEx;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
public class Intake implements Subsystem {
|
||||
|
||||
private GamepadEx gamepad;
|
||||
|
||||
public MultipleTelemetry TELE;
|
||||
|
||||
|
||||
private DcMotorEx intake;
|
||||
|
||||
private double intakePower = 1.0;
|
||||
|
||||
private int intakeState = 0;
|
||||
|
||||
|
||||
public Intake (Robot robot){
|
||||
|
||||
|
||||
this.intake = robot.intake;
|
||||
|
||||
|
||||
}
|
||||
|
||||
public int getIntakeState() {
|
||||
return intakeState;
|
||||
}
|
||||
|
||||
public void toggle(){
|
||||
if (intakeState !=0){
|
||||
intakeState = 0;
|
||||
} else {
|
||||
intakeState = 1;
|
||||
}
|
||||
}
|
||||
|
||||
public void intakeMinPower(){
|
||||
intakeState = 2;
|
||||
}
|
||||
|
||||
public void intake(){
|
||||
intakeState =1;
|
||||
}
|
||||
|
||||
public void reverse(){
|
||||
intakeState =-1;
|
||||
}
|
||||
|
||||
|
||||
public void stop(){
|
||||
intakeState =0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
if (intakeState == 1){
|
||||
intake.setPower(intakePower);
|
||||
} else if (intakeState == -1){
|
||||
intake.setPower(-intakePower);
|
||||
} else if (intakeState == 2){
|
||||
intake.setPower(intakePower);
|
||||
}else {
|
||||
intake.setPower(0);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,248 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystems;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.*;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.Pose2d;
|
||||
import com.arcrobotics.ftclib.controller.PIDController;
|
||||
import com.arcrobotics.ftclib.controller.PIDFController;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.PIDCoefficients;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.teamcode.constants.Poses;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
public class Shooter implements Subsystem {
|
||||
private final DcMotorEx fly1;
|
||||
private final DcMotorEx fly2;
|
||||
|
||||
private final DcMotorEx encoder;
|
||||
private final Servo hoodServo;
|
||||
|
||||
private final Servo turret1;
|
||||
|
||||
private final Servo turret2;
|
||||
|
||||
private final MultipleTelemetry telemetry;
|
||||
|
||||
private boolean telemetryOn = false;
|
||||
|
||||
private double manualPower = 0.0;
|
||||
private double hoodPos = 0.0;
|
||||
|
||||
private double turretPos = 0.0;
|
||||
private double velocity = 0.0;
|
||||
private double posPower = 0.0;
|
||||
|
||||
public double velo = 0.0;
|
||||
|
||||
private int targetPosition = 0;
|
||||
|
||||
public double powPID = 1.0;
|
||||
|
||||
private double p = 0.0003, i = 0, d = 0.00001, f=0;
|
||||
|
||||
private PIDFController controller;
|
||||
private double pow = 0.0;
|
||||
|
||||
private String shooterMode = "AUTO";
|
||||
|
||||
private String turretMode = "AUTO";
|
||||
|
||||
public Shooter(Robot robot, MultipleTelemetry TELE) {
|
||||
this.fly1 = robot.shooter1;
|
||||
this.fly2 = robot.shooter2;
|
||||
this.telemetry = TELE;
|
||||
this.hoodServo = robot.hood;
|
||||
|
||||
// Reset encoders
|
||||
fly1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
fly2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
|
||||
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
controller = new PIDFController(p, i, d, f);
|
||||
|
||||
controller.setPIDF(p, i, d, f);
|
||||
|
||||
this.turret1 = robot.turr1;
|
||||
|
||||
this.turret2 = robot.turr2;
|
||||
|
||||
this.encoder = robot.shooterEncoder;
|
||||
|
||||
}
|
||||
|
||||
public double gethoodPosition() {
|
||||
return (hoodServo.getPosition());
|
||||
}
|
||||
|
||||
public void sethoodPosition(double pos) { hoodPos = pos; }
|
||||
|
||||
public double getTurretPosition() {
|
||||
return ((turret1.getPosition() + (1 - turret2.getPosition())) / 2);
|
||||
}
|
||||
|
||||
public void setTurretPosition(double pos) { turretPos = pos; }
|
||||
|
||||
public double getVelocity(double vel) {
|
||||
return vel;
|
||||
}
|
||||
|
||||
public void setVelocity(double vel) { velocity = vel; }
|
||||
|
||||
public void setPosPower(double power) { posPower = power; }
|
||||
|
||||
public void setTargetPosition(int pos) {
|
||||
targetPosition = pos;
|
||||
}
|
||||
|
||||
public void setTolerance(int tolerance) {
|
||||
controller.setTolerance(tolerance);
|
||||
}
|
||||
|
||||
public void setControllerCoefficients(double kp, double ki, double kd, double kf) {
|
||||
p = kp;
|
||||
i = ki;
|
||||
d = kd;
|
||||
f = kf;
|
||||
controller.setPIDF(p, i, d, f);
|
||||
|
||||
}
|
||||
|
||||
public PIDCoefficients getControllerCoefficients() {
|
||||
|
||||
return new PIDCoefficients(p, i, d);
|
||||
|
||||
}
|
||||
|
||||
public void setManualPower(double power) { manualPower = power; }
|
||||
|
||||
public String getShooterMode() { return shooterMode; }
|
||||
|
||||
public String getTurretMode() { return turretMode; }
|
||||
|
||||
public double getECPRPosition() {
|
||||
return fly1.getCurrentPosition() / (2 * ecpr);
|
||||
}
|
||||
|
||||
public double getMCPRPosition() {
|
||||
return (double) fly1.getCurrentPosition() / 4;
|
||||
}
|
||||
|
||||
public void setShooterMode(String mode) { shooterMode = mode; }
|
||||
|
||||
public void setTurretMode(String mode) { turretMode = mode; }
|
||||
|
||||
public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset) {
|
||||
|
||||
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
Pose2d deltaPose = new Pose2d(
|
||||
goalPose.position.x - robotPose.position.x,
|
||||
goalPose.position.y - robotPose.position.y,
|
||||
goalPose.heading.toDouble() - (robotPose.heading.toDouble())
|
||||
);
|
||||
|
||||
double distance = Math.sqrt(
|
||||
deltaPose.position.x * deltaPose.position.x
|
||||
+ deltaPose.position.y * deltaPose.position.y
|
||||
+ Poses.relativeGoalHeight * Poses.relativeGoalHeight
|
||||
);
|
||||
|
||||
telemetry.addData("dst", distance);
|
||||
|
||||
double shooterPow = getPowerByDist(distance);
|
||||
|
||||
double hoodAngle = getAngleByDist(distance);
|
||||
|
||||
// hoodServo.setPosition(hoodAngle);
|
||||
|
||||
moveTurret(getTurretPosByDeltaPose(deltaPose, offset));
|
||||
|
||||
return distance;
|
||||
|
||||
//0.9974 * 355
|
||||
|
||||
}
|
||||
|
||||
public double getTurretPosByDeltaPose(Pose2d dPose, double offset) {
|
||||
|
||||
double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
|
||||
|
||||
double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y / dPose.position.x));
|
||||
|
||||
telemetry.addData("deltaAngle", deltaAngle);
|
||||
|
||||
if (deltaAngle > 90) {
|
||||
deltaAngle -= 360;
|
||||
}
|
||||
|
||||
// deltaAngle += aTanAngle;
|
||||
|
||||
deltaAngle /= (335);
|
||||
|
||||
telemetry.addData("dAngle", deltaAngle);
|
||||
|
||||
telemetry.addData("AtanAngle", aTanAngle);
|
||||
|
||||
return ((0.30 - deltaAngle) + offset);
|
||||
|
||||
}
|
||||
|
||||
//62, 0.44
|
||||
|
||||
//56.5, 0.5
|
||||
|
||||
public double getPowerByDist(double dist) {
|
||||
|
||||
//TODO: ADD LOGIC
|
||||
return dist;
|
||||
}
|
||||
|
||||
public double getAngleByDist(double dist) {
|
||||
|
||||
double newDist = dist - 56.5;
|
||||
|
||||
double pos = newDist * ((0.44 - 0.5) / (62 - 56.5)) + 0.46;
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
public void setTelemetryOn(boolean state) { telemetryOn = state; }
|
||||
|
||||
public void moveTurret(double pos) {
|
||||
turret1.setPosition(pos);
|
||||
turret2.setPosition(1 - pos);
|
||||
}
|
||||
|
||||
public double getpowPID() {
|
||||
return powPID;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
if (Objects.equals(shooterMode, "MANUAL")) {
|
||||
fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
fly1.setPower(manualPower);
|
||||
fly2.setPower(manualPower);
|
||||
} else if (Objects.equals(shooterMode, "VEL")) {
|
||||
powPID = velocity;
|
||||
|
||||
fly1.setPower(powPID);
|
||||
fly2.setPower(powPID);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,255 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystems;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.hardware.AnalogInput;
|
||||
import com.qualcomm.robotcore.hardware.DigitalChannel;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
public class Spindexer implements Subsystem{
|
||||
|
||||
private Servo s1;
|
||||
private Servo s2;
|
||||
|
||||
private DigitalChannel p0;
|
||||
|
||||
private DigitalChannel p1;
|
||||
private DigitalChannel p2;
|
||||
private DigitalChannel p3;
|
||||
private DigitalChannel p4;
|
||||
|
||||
private DigitalChannel p5;
|
||||
|
||||
private AnalogInput input;
|
||||
|
||||
private AnalogInput input2;
|
||||
|
||||
|
||||
private MultipleTelemetry TELE;
|
||||
|
||||
private double position = 0.501;
|
||||
|
||||
private boolean telemetryOn = false;
|
||||
|
||||
private boolean ball0 = false;
|
||||
|
||||
private boolean ball1 = false;
|
||||
|
||||
private boolean ball2 = false;
|
||||
|
||||
private boolean green0 = false;
|
||||
|
||||
private boolean green1 = false;
|
||||
|
||||
private boolean green2 = false;
|
||||
|
||||
|
||||
|
||||
|
||||
public Spindexer (Robot robot, MultipleTelemetry tele){
|
||||
|
||||
this.s1 = robot.spin1;
|
||||
this.s2 = robot.spin2;
|
||||
|
||||
this.p0 = robot.pin0;
|
||||
this.p1 = robot.pin1;
|
||||
this.p2 = robot.pin2;
|
||||
this.p3 = robot.pin3;
|
||||
this.p4 = robot.pin4;
|
||||
this.p5 = robot.pin5;
|
||||
|
||||
this.input = robot.analogInput;
|
||||
|
||||
this.input2 = robot.analogInput2;
|
||||
|
||||
this.TELE = tele;
|
||||
|
||||
}
|
||||
|
||||
public void setTelemetryOn(boolean state){
|
||||
telemetryOn = state;
|
||||
}
|
||||
|
||||
public void colorSensorTelemetry() {
|
||||
|
||||
|
||||
TELE.addData("ball0", ball0);
|
||||
TELE.addData("ball1", ball1);
|
||||
TELE.addData("ball2", ball2);
|
||||
TELE.addData("green0", green0);
|
||||
TELE.addData("green1", green1);
|
||||
TELE.addData("green2", green2);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void checkForBalls() {
|
||||
|
||||
if (p0.getState()){
|
||||
ball0 = true;
|
||||
green0 = p1.getState();
|
||||
} else {
|
||||
ball0 = false;
|
||||
}
|
||||
|
||||
if (p2.getState()){
|
||||
ball1 = true;
|
||||
green1 = p3.getState();
|
||||
} else {
|
||||
ball1 = false;
|
||||
}
|
||||
|
||||
if (p4.getState()){
|
||||
ball2 = true;
|
||||
green2 = p5.getState();
|
||||
} else {
|
||||
ball2 = false;
|
||||
}
|
||||
}
|
||||
|
||||
public void setPosition (double pos) {
|
||||
position = pos;
|
||||
}
|
||||
|
||||
public void intake () {
|
||||
position = spindexer_intakePos1;
|
||||
}
|
||||
|
||||
public void intakeShake(double runtime) {
|
||||
if ((runtime % 0.25) >0.125) {
|
||||
position = spindexer_intakePos1 + 0.04;
|
||||
} else {
|
||||
position = spindexer_intakePos1 - 0.04;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
public void outtake3Shake(double runtime) {
|
||||
if ((runtime % 0.25) >0.125) {
|
||||
position = spindexer_outtakeBall3 + 0.04;
|
||||
} else {
|
||||
position = spindexer_outtakeBall3 - 0.04;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public void outtake3 () {
|
||||
position = spindexer_outtakeBall3;
|
||||
}
|
||||
|
||||
public void outtake2 () {
|
||||
position = spindexer_outtakeBall2;
|
||||
}
|
||||
|
||||
public void outtake1 () {
|
||||
position = spindexer_outtakeBall1;
|
||||
}
|
||||
|
||||
|
||||
public int outtakeGreen(int secLast, int Last) {
|
||||
if (green2 && (secLast!=3) && (Last!=3)) {
|
||||
outtake3();
|
||||
return 3;
|
||||
} else if (green1 && (secLast!=2) && (Last!=2)){
|
||||
outtake2();
|
||||
return 2;
|
||||
} else if (green0 && (secLast!=1) && (Last!=1)) {
|
||||
outtake1();
|
||||
return 1;
|
||||
} else {
|
||||
|
||||
if (secLast!=1 && Last!= 1){
|
||||
outtake1();
|
||||
return 1;
|
||||
} else if (secLast!=2 && Last!=2){
|
||||
outtake2();
|
||||
return 2;
|
||||
} else {
|
||||
outtake3();
|
||||
return 3;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void outtakeGreenFs() {
|
||||
if (green0 && ball0) {
|
||||
outtake1();
|
||||
} else if (green1 && ball1){
|
||||
outtake2();
|
||||
} else if (green2 && ball2) {
|
||||
outtake3();
|
||||
}
|
||||
}
|
||||
|
||||
public int greens() {
|
||||
int num = 0;
|
||||
|
||||
if (green0){num++;}
|
||||
|
||||
if (green1){num++;}
|
||||
|
||||
|
||||
if (green2){num++;}
|
||||
|
||||
return num;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
public int outtakePurple(int secLast, int Last) {
|
||||
if (!green2 && (secLast!=3) && (Last!=3)) {
|
||||
outtake3();
|
||||
return 3;
|
||||
} else if (!green1 && (secLast!=2) && (Last!=2)){
|
||||
outtake2();
|
||||
return 2;
|
||||
} else if (!green0 && (secLast!=1) && (Last!=1)) {
|
||||
outtake1();
|
||||
return 1;
|
||||
} else {
|
||||
|
||||
if (secLast!=1 && Last!= 1){
|
||||
outtake1();
|
||||
return 1;
|
||||
} else if (secLast!=2 && Last!=2){
|
||||
outtake2();
|
||||
return 2;
|
||||
} else {
|
||||
outtake3();
|
||||
return 3;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
if (position !=0.501) {
|
||||
|
||||
s1.setPosition(position);
|
||||
s2.setPosition(1 - position);
|
||||
}
|
||||
|
||||
|
||||
if (telemetryOn) {
|
||||
colorSensorTelemetry();
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,58 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystems;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
public class Transfer implements Subsystem{
|
||||
|
||||
private final Servo servo;
|
||||
|
||||
private final DcMotorEx transfer;
|
||||
|
||||
private double motorPow = 0.0;
|
||||
|
||||
private double servoPos = 0.501;
|
||||
|
||||
public Transfer (Robot robot){
|
||||
|
||||
this.servo = robot.transferServo;
|
||||
|
||||
this.transfer = robot.transfer;
|
||||
|
||||
}
|
||||
|
||||
public void setTransferPosition(double pos){
|
||||
this.servoPos = pos;
|
||||
}
|
||||
|
||||
public void setTransferPower (double pow){
|
||||
this.motorPow = pow;
|
||||
}
|
||||
|
||||
public void transferOut(){
|
||||
this.setTransferPosition(transferServo_out);
|
||||
}
|
||||
|
||||
public void transferIn(){
|
||||
this.setTransferPosition(transferServo_in);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
if (servoPos!=0.501){
|
||||
servo.setPosition(servoPos);
|
||||
}
|
||||
|
||||
transfer.setPower(motorPow);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,791 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.Pose2d;
|
||||
import com.arcrobotics.ftclib.gamepad.GamepadEx;
|
||||
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
|
||||
import com.arcrobotics.ftclib.gamepad.ToggleButtonReader;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Intake;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Transfer;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
@Config
|
||||
@TeleOp
|
||||
|
||||
public class TeleopV1 extends LinearOpMode {
|
||||
|
||||
Robot robot;
|
||||
|
||||
Drivetrain drivetrain;
|
||||
|
||||
Intake intake;
|
||||
|
||||
Spindexer spindexer;
|
||||
|
||||
Transfer transfer;
|
||||
|
||||
MultipleTelemetry TELE;
|
||||
|
||||
GamepadEx g1;
|
||||
|
||||
GamepadEx g2;
|
||||
|
||||
public static double defaultSpeed = 1;
|
||||
|
||||
public static double slowMoSpeed = 0.4;
|
||||
|
||||
public static double power = 0.0;
|
||||
|
||||
public static double pos = hoodDefault;
|
||||
|
||||
public boolean all = false;
|
||||
|
||||
public int ticker = 0;
|
||||
|
||||
ToggleButtonReader g1RightBumper;
|
||||
|
||||
ToggleButtonReader g2Circle;
|
||||
|
||||
ToggleButtonReader g2Square;
|
||||
|
||||
ToggleButtonReader g2Triangle;
|
||||
|
||||
ToggleButtonReader g2RightBumper;
|
||||
|
||||
ToggleButtonReader g1LeftBumper;
|
||||
|
||||
ToggleButtonReader g2LeftBumper;
|
||||
|
||||
ToggleButtonReader g2DpadUp;
|
||||
|
||||
ToggleButtonReader g2DpadDown;
|
||||
|
||||
ToggleButtonReader g2DpadRight;
|
||||
|
||||
ToggleButtonReader g2DpadLeft;
|
||||
|
||||
public boolean leftBumper = false;
|
||||
public double g1RightBumperStamp = 0.0;
|
||||
|
||||
public double g1LeftBumperStamp = 0.0;
|
||||
|
||||
public double g2LeftBumperStamp = 0.0;
|
||||
|
||||
public static int spindexerPos = 0;
|
||||
|
||||
public boolean green = false;
|
||||
|
||||
Shooter shooter;
|
||||
|
||||
public boolean scoreAll = false;
|
||||
|
||||
MecanumDrive drive;
|
||||
|
||||
public boolean autotrack = false;
|
||||
|
||||
public int last = 0;
|
||||
public int second = 0;
|
||||
|
||||
public double offset = 0.0;
|
||||
|
||||
public static double rIn = 0.59;
|
||||
|
||||
public static double rOut = 0;
|
||||
|
||||
public boolean notShooting = true;
|
||||
|
||||
public boolean circle = false;
|
||||
|
||||
public boolean square = false;
|
||||
|
||||
public boolean tri = false;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
drive = new MecanumDrive(hardwareMap, teleStart);
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
|
||||
TELE = new MultipleTelemetry(
|
||||
FtcDashboard.getInstance().getTelemetry(),
|
||||
telemetry
|
||||
);
|
||||
|
||||
g1 = new GamepadEx(gamepad1);
|
||||
|
||||
g1RightBumper = new ToggleButtonReader(
|
||||
g1, GamepadKeys.Button.RIGHT_BUMPER
|
||||
);
|
||||
|
||||
g2 = new GamepadEx(gamepad2);
|
||||
|
||||
g1LeftBumper = new ToggleButtonReader(
|
||||
g1, GamepadKeys.Button.LEFT_BUMPER
|
||||
);
|
||||
|
||||
g2Circle = new ToggleButtonReader(
|
||||
g2, GamepadKeys.Button.B
|
||||
);
|
||||
|
||||
g2Triangle = new ToggleButtonReader(
|
||||
g2, GamepadKeys.Button.Y
|
||||
);
|
||||
|
||||
g2Square = new ToggleButtonReader(
|
||||
g2, GamepadKeys.Button.X
|
||||
);
|
||||
|
||||
g2RightBumper = new ToggleButtonReader(
|
||||
g2, GamepadKeys.Button.RIGHT_BUMPER
|
||||
);
|
||||
|
||||
g2LeftBumper = new ToggleButtonReader(
|
||||
g2, GamepadKeys.Button.LEFT_BUMPER
|
||||
);
|
||||
|
||||
g2DpadUp = new ToggleButtonReader(
|
||||
g2, GamepadKeys.Button.DPAD_UP
|
||||
);
|
||||
|
||||
g2DpadDown = new ToggleButtonReader(
|
||||
g2, GamepadKeys.Button.DPAD_DOWN
|
||||
);
|
||||
|
||||
g2DpadLeft = new ToggleButtonReader(
|
||||
g2, GamepadKeys.Button.DPAD_LEFT
|
||||
);
|
||||
|
||||
g2DpadRight = new ToggleButtonReader(
|
||||
g2, GamepadKeys.Button.DPAD_RIGHT
|
||||
);
|
||||
|
||||
drivetrain = new Drivetrain(robot, TELE, g1);
|
||||
|
||||
drivetrain.setMode("Default");
|
||||
|
||||
drivetrain.setDefaultSpeed(defaultSpeed);
|
||||
|
||||
drivetrain.setSlowSpeed(slowMoSpeed);
|
||||
|
||||
intake = new Intake(robot);
|
||||
|
||||
transfer = new Transfer(robot);
|
||||
|
||||
spindexer = new Spindexer(robot, TELE);
|
||||
|
||||
spindexer.setTelemetryOn(true);
|
||||
|
||||
shooter = new Shooter(robot, TELE);
|
||||
|
||||
shooter.setShooterMode("MANUAL");
|
||||
|
||||
robot.rejecter.setPosition(rIn);
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
drive = new MecanumDrive(hardwareMap, teleStart);
|
||||
|
||||
while (opModeIsActive()) {
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
TELE.addData("pose", drive.localizer.getPose());
|
||||
|
||||
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
|
||||
|
||||
TELE.addData("off", offset);
|
||||
|
||||
robot.hood.setPosition(pos);
|
||||
|
||||
g1LeftBumper.readValue();
|
||||
|
||||
if (g1LeftBumper.wasJustPressed()) {
|
||||
g2LeftBumperStamp = getRuntime();
|
||||
|
||||
spindexer.intakeShake(getRuntime());
|
||||
|
||||
leftBumper = true;
|
||||
}
|
||||
|
||||
if (leftBumper) {
|
||||
double time = getRuntime() - g2LeftBumperStamp;
|
||||
|
||||
if (time < 1.0) {
|
||||
robot.rejecter.setPosition(rOut);
|
||||
} else {
|
||||
robot.rejecter.setPosition(rIn);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
intake();
|
||||
|
||||
drivetrain.update();
|
||||
|
||||
TELE.update();
|
||||
|
||||
transfer.update();
|
||||
|
||||
g2RightBumper.readValue();
|
||||
|
||||
g2LeftBumper.readValue();
|
||||
|
||||
g2DpadDown.readValue();
|
||||
|
||||
g2DpadUp.readValue();
|
||||
|
||||
if (!scoreAll) {
|
||||
spindexer.checkForBalls();
|
||||
}
|
||||
|
||||
if (g2DpadUp.wasJustPressed()) {
|
||||
pos -= 0.02;
|
||||
}
|
||||
|
||||
if (g2DpadDown.wasJustPressed()) {
|
||||
pos += 0.02;
|
||||
}
|
||||
|
||||
g2DpadLeft.readValue();
|
||||
|
||||
g2DpadRight.readValue();
|
||||
|
||||
if (g2DpadLeft.wasJustPressed()) {
|
||||
offset -= 0.02;
|
||||
}
|
||||
|
||||
if (g2DpadRight.wasJustPressed()) {
|
||||
offset += 0.02;
|
||||
}
|
||||
|
||||
TELE.addData("hood", pos);
|
||||
|
||||
if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) {
|
||||
|
||||
shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset);
|
||||
|
||||
} else {
|
||||
|
||||
autotrack = false;
|
||||
|
||||
shooter.moveTurret(0.3 + offset);
|
||||
|
||||
}
|
||||
|
||||
if (gamepad2.right_stick_button) {
|
||||
autotrack = true;
|
||||
}
|
||||
|
||||
if (g2RightBumper.wasJustPressed()) {
|
||||
transfer.setTransferPower(1);
|
||||
transfer.transferIn();
|
||||
shooter.setManualPower(1);
|
||||
|
||||
notShooting = false;
|
||||
|
||||
}
|
||||
|
||||
if (g2RightBumper.wasJustReleased()) {
|
||||
transfer.setTransferPower(1);
|
||||
transfer.transferOut();
|
||||
}
|
||||
|
||||
if (gamepad2.left_stick_y > 0.5) {
|
||||
|
||||
shooter.setManualPower(0);
|
||||
} else if (gamepad2.left_stick_y < -0.5) {
|
||||
shooter.setManualPower(1);
|
||||
}
|
||||
|
||||
if (g2LeftBumper.wasJustPressed()) {
|
||||
g2LeftBumperStamp = getRuntime();
|
||||
notShooting = false;
|
||||
scoreAll = true;
|
||||
}
|
||||
|
||||
if (scoreAll) {
|
||||
double time = getRuntime() - g2LeftBumperStamp;
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
TELE.addData("greenImportant", green);
|
||||
|
||||
TELE.addData("last", last);
|
||||
TELE.addData("2ndLast", second);
|
||||
|
||||
int numGreen = spindexer.greens();
|
||||
|
||||
if (square) {
|
||||
|
||||
if (time < 0.3) {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
last = 0;
|
||||
second = 0;
|
||||
|
||||
transfer.transferOut();
|
||||
transfer.setTransferPower(1);
|
||||
} else if (time < 2) {
|
||||
|
||||
if (ticker == 0) {
|
||||
|
||||
if (numGreen == 2) {
|
||||
last = spindexer.outtakePurple(second, last);
|
||||
second = last;
|
||||
} else {
|
||||
last = spindexer.outtakeGreen(second, last);
|
||||
second = last;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
second = last;
|
||||
|
||||
ticker++;
|
||||
|
||||
} else if (time < 2.5) {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
second = last;
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 4) {
|
||||
transfer.transferOut();
|
||||
|
||||
if (ticker == 0) {
|
||||
|
||||
if (numGreen == 2) {
|
||||
last = spindexer.outtakeGreen(second, last);
|
||||
} else {
|
||||
last = spindexer.outtakePurple(second, last);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
ticker++;
|
||||
} else if (time < 4.5) {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 6) {
|
||||
|
||||
transfer.transferOut();
|
||||
|
||||
if (ticker == 0) {
|
||||
|
||||
if (numGreen == 2) {
|
||||
last = spindexer.outtakeGreen(second, last);
|
||||
} else {
|
||||
last = spindexer.outtakePurple(second, last);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
ticker++;
|
||||
|
||||
} else if (time < 6.5) {
|
||||
transfer.transferIn();
|
||||
} else {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
scoreAll = false;
|
||||
transfer.transferOut();
|
||||
|
||||
shooter.setManualPower(0);
|
||||
|
||||
}
|
||||
} else if (tri) {
|
||||
|
||||
if (time < 0.3) {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
last = 0;
|
||||
second = 0;
|
||||
|
||||
transfer.transferOut();
|
||||
transfer.setTransferPower(1);
|
||||
} else if (time < 2) {
|
||||
|
||||
if (ticker == 0) {
|
||||
|
||||
if (numGreen == 2) {
|
||||
last = spindexer.outtakeGreen(second, last);
|
||||
second = last;
|
||||
} else {
|
||||
last = spindexer.outtakePurple(second, last);
|
||||
second = last;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
second = last;
|
||||
|
||||
ticker++;
|
||||
|
||||
} else if (time < 2.5) {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
second = last;
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 4) {
|
||||
transfer.transferOut();
|
||||
|
||||
if (ticker == 0) {
|
||||
|
||||
if (numGreen == 2) {
|
||||
last = spindexer.outtakePurple(second, last);
|
||||
} else {
|
||||
last = spindexer.outtakeGreen(second, last);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
ticker++;
|
||||
} else if (time < 4.5) {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 6) {
|
||||
|
||||
transfer.transferOut();
|
||||
|
||||
if (ticker == 0) {
|
||||
|
||||
if (numGreen == 2) {
|
||||
last = spindexer.outtakeGreen(second, last);
|
||||
} else {
|
||||
last = spindexer.outtakePurple(second, last);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
ticker++;
|
||||
|
||||
} else if (time < 6.5) {
|
||||
transfer.transferIn();
|
||||
} else {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
scoreAll = false;
|
||||
transfer.transferOut();
|
||||
|
||||
shooter.setManualPower(0);
|
||||
|
||||
}
|
||||
} else if (circle) {
|
||||
|
||||
if (time < 0.3) {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
last = 0;
|
||||
second = 0;
|
||||
|
||||
transfer.transferOut();
|
||||
transfer.setTransferPower(1);
|
||||
} else if (time < 2) {
|
||||
|
||||
if (ticker == 0) {
|
||||
|
||||
if (numGreen == 2) {
|
||||
last = spindexer.outtakeGreen(second, last);
|
||||
second = last;
|
||||
} else {
|
||||
last = spindexer.outtakePurple(second, last);
|
||||
second = last;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
second = last;
|
||||
|
||||
ticker++;
|
||||
|
||||
} else if (time < 2.5) {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
second = last;
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 4) {
|
||||
transfer.transferOut();
|
||||
|
||||
if (ticker == 0) {
|
||||
|
||||
if (numGreen == 2) {
|
||||
last = spindexer.outtakeGreen(second, last);
|
||||
} else {
|
||||
last = spindexer.outtakePurple(second, last);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
ticker++;
|
||||
} else if (time < 4.5) {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 6) {
|
||||
|
||||
transfer.transferOut();
|
||||
|
||||
if (ticker == 0) {
|
||||
|
||||
if (numGreen == 2) {
|
||||
last = spindexer.outtakePurple(second, last);
|
||||
} else {
|
||||
last = spindexer.outtakeGreen(second, last);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
ticker++;
|
||||
|
||||
} else if (time < 6.5) {
|
||||
transfer.transferIn();
|
||||
} else {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
scoreAll = false;
|
||||
transfer.transferOut();
|
||||
|
||||
shooter.setManualPower(0);
|
||||
|
||||
}
|
||||
} else {
|
||||
|
||||
if (time < 0.3) {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
last = 0;
|
||||
second = 0;
|
||||
|
||||
if (gamepad2.right_trigger > 0.5) {
|
||||
green = false;
|
||||
|
||||
all = gamepad2.left_trigger > 0.5;
|
||||
|
||||
} else if (gamepad2.left_trigger > 0.5) {
|
||||
green = true;
|
||||
|
||||
all = false;
|
||||
} else {
|
||||
all = true;
|
||||
}
|
||||
|
||||
transfer.transferOut();
|
||||
transfer.setTransferPower(1);
|
||||
} else if (time < 2) {
|
||||
|
||||
if (ticker == 0) {
|
||||
|
||||
if (all) {
|
||||
spindexer.outtake3();
|
||||
last = 3;
|
||||
second = 3;
|
||||
} else if (green) {
|
||||
last = spindexer.outtakeGreen(second, last);
|
||||
second = last;
|
||||
} else {
|
||||
last = spindexer.outtakePurple(second, last);
|
||||
second = last;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
second = last;
|
||||
|
||||
ticker++;
|
||||
|
||||
} else if (time < 2.5) {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
second = last;
|
||||
|
||||
if (gamepad2.right_trigger > 0.5) {
|
||||
green = false;
|
||||
|
||||
all = gamepad2.left_trigger > 0.5;
|
||||
|
||||
} else if (gamepad2.left_trigger > 0.5) {
|
||||
green = true;
|
||||
|
||||
all = false;
|
||||
|
||||
}
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 4) {
|
||||
transfer.transferOut();
|
||||
|
||||
if (ticker == 0) {
|
||||
|
||||
if (all) {
|
||||
spindexer.outtake2();
|
||||
|
||||
last = 2;
|
||||
} else if (green) {
|
||||
last = spindexer.outtakeGreen(second, last);
|
||||
} else {
|
||||
last = spindexer.outtakePurple(second, last);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
ticker++;
|
||||
} else if (time < 4.5) {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
if (gamepad2.right_trigger > 0.5) {
|
||||
green = false;
|
||||
|
||||
all = gamepad2.left_trigger > 0.5;
|
||||
|
||||
} else if (gamepad2.left_trigger > 0.5) {
|
||||
green = true;
|
||||
|
||||
all = false;
|
||||
}
|
||||
|
||||
transfer.transferIn();
|
||||
} else if (time < 6) {
|
||||
|
||||
transfer.transferOut();
|
||||
|
||||
if (ticker == 0) {
|
||||
|
||||
if (all) {
|
||||
spindexer.outtake1();
|
||||
} else if (green) {
|
||||
last = spindexer.outtakeGreen(second, last);
|
||||
} else {
|
||||
last = spindexer.outtakePurple(second, last);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
ticker++;
|
||||
|
||||
} else if (time < 6.5) {
|
||||
transfer.transferIn();
|
||||
} else {
|
||||
|
||||
ticker = 0;
|
||||
|
||||
scoreAll = false;
|
||||
transfer.transferOut();
|
||||
|
||||
shooter.setManualPower(0);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
shooter.update();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void intake() {
|
||||
|
||||
g1RightBumper.readValue();
|
||||
|
||||
g2Circle.readValue();
|
||||
|
||||
g2Square.readValue();
|
||||
|
||||
g2Triangle.readValue();
|
||||
|
||||
if (g1RightBumper.wasJustPressed()) {
|
||||
|
||||
notShooting = true;
|
||||
|
||||
if (getRuntime() - g1RightBumperStamp < 0.3) {
|
||||
intake.reverse();
|
||||
} else {
|
||||
intake.toggle();
|
||||
}
|
||||
|
||||
if (intake.getIntakeState() == 1) {
|
||||
shooter.setManualPower(0);
|
||||
}
|
||||
|
||||
spindexer.intake();
|
||||
|
||||
transfer.transferOut();
|
||||
|
||||
g1RightBumperStamp = getRuntime();
|
||||
|
||||
}
|
||||
|
||||
if (intake.getIntakeState() == 1 && notShooting) {
|
||||
|
||||
spindexer.intakeShake(getRuntime());
|
||||
|
||||
} else {
|
||||
if (g2Circle.wasJustPressed()) {
|
||||
circle = true;
|
||||
tri = false;
|
||||
square = false;
|
||||
|
||||
}
|
||||
|
||||
if (g2Triangle.wasJustPressed()) {
|
||||
circle = false;
|
||||
tri = true;
|
||||
square = false;
|
||||
}
|
||||
|
||||
if (g2Square.wasJustPressed()) {
|
||||
circle = false;
|
||||
tri = false;
|
||||
square = true;
|
||||
}
|
||||
|
||||
if (gamepad2.x) {
|
||||
circle = false;
|
||||
tri = false;
|
||||
square = false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
intake.update();
|
||||
|
||||
spindexer.update();
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,35 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop;
|
||||
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Transfer;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
@Config
|
||||
@TeleOp
|
||||
public class TransferTest extends LinearOpMode {
|
||||
|
||||
Robot robot;
|
||||
|
||||
|
||||
Transfer transfer;
|
||||
|
||||
public static double pos = 0.40;
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
|
||||
transfer = new Transfer(robot);
|
||||
|
||||
waitForStart();
|
||||
|
||||
while (opModeIsActive()){
|
||||
transfer.setTransferPosition(pos);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,59 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.teleop;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
|
||||
|
||||
|
||||
@TeleOp
|
||||
@Config
|
||||
public class WebcamTest extends LinearOpMode {
|
||||
|
||||
AprilTag webcam;
|
||||
|
||||
MultipleTelemetry TELE;
|
||||
|
||||
Robot robot;
|
||||
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
|
||||
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
webcam = new AprilTag(robot, TELE);
|
||||
|
||||
webcam.turnTelemetryOn(true);
|
||||
|
||||
|
||||
|
||||
|
||||
while(opModeInInit()){
|
||||
|
||||
webcam.initTelemetry();
|
||||
|
||||
TELE.update();
|
||||
|
||||
};
|
||||
|
||||
if(isStopRequested()) return;
|
||||
|
||||
|
||||
while (opModeIsActive()){
|
||||
|
||||
webcam.update();
|
||||
|
||||
TELE.update();
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,144 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.tests;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
|
||||
@TeleOp
|
||||
@Config
|
||||
public class ActiveColorSensorTest extends LinearOpMode {
|
||||
Robot robot;
|
||||
MultipleTelemetry TELE;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException{
|
||||
robot = new Robot(hardwareMap);
|
||||
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
int b1Purple = 1;
|
||||
int b1Total = 1;
|
||||
int b2Purple = 1;
|
||||
int b2Total = 1;
|
||||
int b3Purple = 1;
|
||||
int b3Total = 1;
|
||||
|
||||
double totalStamp1 = 0.0;
|
||||
double purpleStamp1 = 0.0;
|
||||
double totalStamp2 = 0.0;
|
||||
double purpleStamp2 = 0.0;
|
||||
double totalStamp3 = 0.0;
|
||||
double purpleStamp3 = 0.0;
|
||||
|
||||
String b1 = "none";
|
||||
String b2 = "none";
|
||||
String b3 = "none";
|
||||
|
||||
double position = 0.0;
|
||||
|
||||
double stamp = getRuntime();
|
||||
|
||||
waitForStart();
|
||||
if (isStopRequested()) return;
|
||||
|
||||
while (opModeIsActive()){
|
||||
|
||||
if ((getRuntime() % 0.3) >0.15) {
|
||||
position = spindexer_intakePos1 + 0.015;
|
||||
} else {
|
||||
position = spindexer_intakePos1 - 0.015;
|
||||
}
|
||||
robot.spin1.setPosition(position);
|
||||
robot.spin2.setPosition(1-position);
|
||||
|
||||
robot.intake.setPower(1);
|
||||
|
||||
// Reset the counters after 1 second of not reading a ball.
|
||||
final double ColorCounterResetDelay = 1.0;
|
||||
// Number of times the loop needs to run before deciding on a color.
|
||||
final int ColorCounterTotalMinCount = 20;
|
||||
// If the color sensor reads a color this percentage of time
|
||||
// out of the total, declare the color.
|
||||
// Usage: (Color Count)/(Total Count) > ColorCounterThreshold
|
||||
final double ColorCounterThreshold = 0.65;
|
||||
|
||||
if (robot.pin1.getState()){
|
||||
if (robot.pin0.getState()){
|
||||
b1Purple ++;
|
||||
}
|
||||
b1Total++;
|
||||
totalStamp1 = getRuntime();
|
||||
}
|
||||
if (getRuntime() - totalStamp1 > ColorCounterResetDelay) {
|
||||
// Too Much time has passed without detecting ball
|
||||
b1 = "none";
|
||||
b1Total = 1;
|
||||
b1Purple = 1;
|
||||
}else if ((b1Total > ColorCounterTotalMinCount) && ((double) b1Purple / b1Total) >= ColorCounterThreshold){
|
||||
// Enough Time has passed and we met the threshold
|
||||
b1 = "Purple";
|
||||
}else if (b1Total > ColorCounterTotalMinCount) {
|
||||
// Enough Time passed WITHOUT meeting the threshold
|
||||
b1 = "Green";
|
||||
}
|
||||
|
||||
if (robot.pin3.getState()){
|
||||
if (robot.pin2.getState()){
|
||||
b2Purple ++;
|
||||
}
|
||||
b2Total++;
|
||||
totalStamp2 = getRuntime();
|
||||
}
|
||||
if (getRuntime() - totalStamp2 > ColorCounterResetDelay) {
|
||||
// Too Much time has passed without detecting ball
|
||||
b2 = "none";
|
||||
b2Total = 1;
|
||||
b2Purple = 1;
|
||||
}else if ((b2Total > ColorCounterTotalMinCount) && ((double) b2Purple / b2Total) >= ColorCounterThreshold){
|
||||
// Enough Time has passed and we met the threshold
|
||||
b2 = "Purple";
|
||||
}else if (b2Total > ColorCounterTotalMinCount) {
|
||||
// Enough Time passed WITHOUT meeting the threshold
|
||||
b2 = "Green";
|
||||
}
|
||||
|
||||
if (robot.pin5.getState()){
|
||||
if (robot.pin4.getState()){
|
||||
b3Purple ++;
|
||||
}
|
||||
b3Total++;
|
||||
totalStamp3 = getRuntime();
|
||||
}
|
||||
if (getRuntime() - totalStamp3 > ColorCounterResetDelay) {
|
||||
// Too Much time has passed without detecting ball
|
||||
b3 = "none";
|
||||
b3Total = 1;
|
||||
b3Purple = 1;
|
||||
}else if ((b3Total > ColorCounterTotalMinCount) && ((double) b3Purple / b3Total) >= ColorCounterThreshold){
|
||||
// Enough Time has passed and we met the threshold
|
||||
b3 = "Purple";
|
||||
}else if (b3Total > ColorCounterTotalMinCount) {
|
||||
// Enough Time passed WITHOUT meeting the threshold
|
||||
b3 = "Green";
|
||||
}
|
||||
|
||||
TELE.addData("Green1:", robot.pin1.getState());
|
||||
TELE.addData("Purple1:", robot.pin0.getState());
|
||||
TELE.addData("Green2:", robot.pin3.getState());
|
||||
TELE.addData("Purple2:", robot.pin2.getState());
|
||||
TELE.addData("Green3:", robot.pin5.getState());
|
||||
TELE.addData("Purple3:", robot.pin4.getState());
|
||||
TELE.addData("1", b1);
|
||||
TELE.addData("2",b2);
|
||||
TELE.addData("3",b3);
|
||||
|
||||
TELE.update();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,63 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.tests;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
@TeleOp
|
||||
@Config
|
||||
public class ColorSensorTest extends LinearOpMode {
|
||||
Robot robot;
|
||||
MultipleTelemetry TELE;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
robot = new Robot(hardwareMap);
|
||||
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
waitForStart();
|
||||
if (isStopRequested()) return;
|
||||
|
||||
while (opModeIsActive()) {
|
||||
|
||||
// ----- COLOR 1 -----
|
||||
double green1 = robot.color1.getNormalizedColors().green;
|
||||
double blue1 = robot.color1.getNormalizedColors().blue;
|
||||
double red1 = robot.color1.getNormalizedColors().red;
|
||||
|
||||
TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
|
||||
TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
|
||||
TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM));
|
||||
|
||||
|
||||
// ----- COLOR 2 -----
|
||||
double green2 = robot.color2.getNormalizedColors().green;
|
||||
double blue2 = robot.color2.getNormalizedColors().blue;
|
||||
double red2 = robot.color2.getNormalizedColors().red;
|
||||
|
||||
TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor());
|
||||
TELE.addData("Color2 green", green2 / (green2 + blue2 + red2));
|
||||
TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM));
|
||||
|
||||
|
||||
// ----- COLOR 3 -----
|
||||
double green3 = robot.color3.getNormalizedColors().green;
|
||||
double blue3 = robot.color3.getNormalizedColors().blue;
|
||||
double red3 = robot.color3.getNormalizedColors().red;
|
||||
|
||||
TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor());
|
||||
TELE.addData("Color3 green", green3 / (green3 + blue3 + red3));
|
||||
TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM));
|
||||
|
||||
|
||||
TELE.update();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
package org.firstinspires.ftc.teamcode.tests;
|
||||
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DigitalChannel;
|
||||
|
||||
@Config
|
||||
@TeleOp
|
||||
|
||||
@Disabled
|
||||
public class ColorSensorTester extends LinearOpMode {
|
||||
|
||||
|
||||
public static String portAName = "pin0";
|
||||
|
||||
public static String portBName = "pin1";
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
|
||||
DigitalChannel pinA = hardwareMap.digitalChannel.get(portAName);
|
||||
DigitalChannel pinB = hardwareMap.digitalChannel.get(portBName);
|
||||
|
||||
|
||||
|
||||
|
||||
MultipleTelemetry TELE = new MultipleTelemetry(
|
||||
telemetry,
|
||||
FtcDashboard.getInstance().getTelemetry()
|
||||
);
|
||||
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
if(isStopRequested()) return;
|
||||
|
||||
while(opModeIsActive()){
|
||||
|
||||
TELE.addData("pinA", pinA.getState());
|
||||
TELE.addData("pinB", pinB.getState());
|
||||
|
||||
TELE.update();
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
package org.firstinspires.ftc.teamcode.tests;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.variables.HardwareConfig;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
import org.firstinspires.ftc.teamcode.utils.subsystems.Limelight;
|
||||
|
||||
@Autonomous
|
||||
@Config
|
||||
|
||||
public class LimelightTest extends LinearOpMode {
|
||||
|
||||
public static String MODE = "AT";
|
||||
public static int pipe = 1;
|
||||
|
||||
|
||||
MultipleTelemetry TELE;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Robot robot;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
|
||||
|
||||
HardwareConfig.USING_LL= true;
|
||||
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
|
||||
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
Limelight limelight = new Limelight(robot, TELE);
|
||||
|
||||
limelight.setMode(MODE);
|
||||
|
||||
limelight.setTelemetryOn(true);
|
||||
|
||||
waitForStart();
|
||||
|
||||
if(isStopRequested()) return;
|
||||
|
||||
while(opModeIsActive()){
|
||||
|
||||
limelight.setPipeline(pipe);
|
||||
|
||||
limelight.setMode(MODE);
|
||||
|
||||
limelight.update();
|
||||
|
||||
TELE.update();
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,45 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.tests;
|
||||
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
@Config
|
||||
@TeleOp
|
||||
public class MotorDirectionDebugger extends LinearOpMode {
|
||||
|
||||
public static double flPower = 0.0;
|
||||
|
||||
public static double frPower = 0.0;
|
||||
|
||||
public static double blPower = 0.0;
|
||||
public static double brPower = 0.0;
|
||||
|
||||
Robot robot;
|
||||
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
if(isStopRequested()) return;
|
||||
|
||||
while(opModeIsActive()){
|
||||
|
||||
robot.frontLeft.setPower(flPower);
|
||||
robot.frontRight.setPower(frPower);
|
||||
robot.backRight.setPower(brPower);
|
||||
robot.backLeft.setPower(blPower);
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,85 +1,59 @@
|
||||
package org.firstinspires.ftc.teamcode.tests;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.utils.Flywheel;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
|
||||
|
||||
@TeleOp
|
||||
@Config
|
||||
@TeleOp
|
||||
public class ShooterTest extends LinearOpMode {
|
||||
|
||||
Robot robot;
|
||||
|
||||
public static double pow = 0.0;
|
||||
public static double vel = 0.0;
|
||||
public static double ecpr = 1024.0; // CPR of the encoder
|
||||
public static double hoodPos = 0.5;
|
||||
public static double turretPos = 0.9;
|
||||
|
||||
public static String flyMode = "VEL";
|
||||
|
||||
public static boolean AutoTrack = false;
|
||||
|
||||
public static int mode = 0;
|
||||
public static double parameter = 0.0;
|
||||
// --- CONSTANTS YOU TUNE ---
|
||||
public static double hoodPos = 0.501;
|
||||
double initPos = 0.0;
|
||||
|
||||
double stamp = 0.0;
|
||||
double stamp1 = 0.0;
|
||||
double ticker = 0.0;
|
||||
double currentPos = 0.0;
|
||||
double velo = 0.0;
|
||||
double velo1 = 0.0;
|
||||
double velo2 = 0.0;
|
||||
double velo3 = 0.0;
|
||||
double velo4 = 0.0;
|
||||
double velo5 = 0.0;
|
||||
|
||||
double stamp1 = 0.0;
|
||||
|
||||
double initPos1 = 0.0;
|
||||
|
||||
double powPID = 0.0;
|
||||
|
||||
public static int maxVel = 4500;
|
||||
|
||||
public static boolean shoot = false;
|
||||
|
||||
public static int spindexPos = 1;
|
||||
|
||||
public static boolean intake = true;
|
||||
|
||||
public static int tolerance = 50;
|
||||
|
||||
double stamp = 0.0;
|
||||
|
||||
public static double kP = 0.001; // small proportional gain (tune this)
|
||||
public static double maxStep = 0.06; // prevents sudden jumps
|
||||
public static double distance = 50;
|
||||
|
||||
MultipleTelemetry TELE;
|
||||
double initPosq = 0.0;
|
||||
double stampq = 0.0;
|
||||
double stamp1q = 0.0;
|
||||
double tickerq = 0.0;
|
||||
double currentPosq = 0.0;
|
||||
double veloq = 0.0;
|
||||
double velo1q = 0.0;
|
||||
double velo2q = 0.0;
|
||||
double velo3q = 0.0;
|
||||
double velo4q = 0.0;
|
||||
double velo5q = 0.0;
|
||||
Robot robot;
|
||||
Flywheel flywheel;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
DcMotorEx leftShooter = robot.shooter1;
|
||||
DcMotorEx rightShooter = robot.shooter2;
|
||||
flywheel = new Flywheel();
|
||||
|
||||
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
Shooter shooter = new Shooter(robot, TELE);
|
||||
|
||||
robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
shooter.setTelemetryOn(true);
|
||||
|
||||
shooter.setShooterMode(flyMode);
|
||||
|
||||
initPos = shooter.getECPRPosition();
|
||||
|
||||
int ticker = 0;
|
||||
MultipleTelemetry TELE = new MultipleTelemetry(
|
||||
telemetry, FtcDashboard.getInstance().getTelemetry()
|
||||
);
|
||||
|
||||
waitForStart();
|
||||
|
||||
@@ -87,124 +61,63 @@ public class ShooterTest extends LinearOpMode {
|
||||
|
||||
while (opModeIsActive()) {
|
||||
|
||||
ticker++;
|
||||
if (mode == 0) {
|
||||
rightShooter.setPower(parameter);
|
||||
leftShooter.setPower(parameter);
|
||||
leftShooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
rightShooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
if (ticker % 2 == 0) {
|
||||
velo5 = velo4;
|
||||
velo4 = velo3;
|
||||
velo3 = velo2;
|
||||
velo2 = velo1;
|
||||
|
||||
if (AutoTrack){
|
||||
hoodPos = hoodAnglePrediction(distance);
|
||||
vel = velPrediction(distance);
|
||||
}
|
||||
|
||||
|
||||
|
||||
shooter.setShooterMode(flyMode);
|
||||
|
||||
shooter.setManualPower(pow);
|
||||
|
||||
robot.hood.setPosition(hoodPos);
|
||||
robot.turr1.setPosition(turretPos);
|
||||
robot.turr2.setPosition(1 - turretPos);
|
||||
if (intake) {
|
||||
robot.transfer.setPower(0);
|
||||
robot.intake.setPower(0.75);
|
||||
robot.spin1.setPosition(spindexer_intakePos1);
|
||||
robot.spin2.setPosition(1 - spindexer_intakePos1);
|
||||
} else {
|
||||
robot.transfer.setPower(.75 + (powPID/4));
|
||||
robot.intake.setPower(0);
|
||||
if (spindexPos == 1) {
|
||||
robot.spin1.setPosition(spindexer_outtakeBall1);
|
||||
robot.spin2.setPosition(1 - spindexer_outtakeBall1);
|
||||
} else if (spindexPos == 2) {
|
||||
robot.spin1.setPosition(spindexer_outtakeBall2);
|
||||
robot.spin2.setPosition(1 - spindexer_outtakeBall2);
|
||||
} else if (spindexPos == 3) {
|
||||
robot.spin1.setPosition(spindexer_outtakeBall3);
|
||||
robot.spin2.setPosition(1 - spindexer_outtakeBall3);
|
||||
}
|
||||
}
|
||||
|
||||
double penguin = 0;
|
||||
if (ticker % 8 ==0){
|
||||
penguin = shooter.getECPRPosition();
|
||||
currentPos = (double) leftShooter.getCurrentPosition() / 28;
|
||||
stamp = getRuntime();
|
||||
velo1 = -60 * ((penguin - initPos1) / (stamp - stamp1));
|
||||
initPos1 = penguin;
|
||||
velo1 = 60 * ((currentPos - initPos) / (stamp - stamp1));
|
||||
initPos = currentPos;
|
||||
stamp1 = stamp;
|
||||
|
||||
velo = (velo1 + velo2 + velo3 + velo4 + velo5) / 5;
|
||||
}
|
||||
if (tickerq % 2 == 0) {
|
||||
velo5q = velo4q;
|
||||
velo4q = velo3q;
|
||||
velo3q = velo2q;
|
||||
velo2q = velo1q;
|
||||
|
||||
currentPosq = (double) rightShooter.getCurrentPosition() / 28;
|
||||
stampq = getRuntime();
|
||||
velo1q = 60 * ((currentPosq - initPosq) / (stampq - stamp1q));
|
||||
initPosq = currentPosq;
|
||||
stamp1q = stampq;
|
||||
|
||||
veloq = (velo1q + velo2q + velo3q + velo4q + velo5q) / 5;
|
||||
}
|
||||
} else if (mode == 1) {
|
||||
leftShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
rightShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
|
||||
rightShooter.setPower(powPID);
|
||||
leftShooter.setPower(powPID);
|
||||
TELE.addData("PIDPower", powPID);
|
||||
}
|
||||
|
||||
|
||||
velo = velo1;
|
||||
|
||||
double feed = vel / maxVel; // Example: vel=2500 → feed=0.5
|
||||
|
||||
if (vel > 500){
|
||||
feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
|
||||
if (hoodPos != 0.501) {
|
||||
robot.hood.setPosition(hoodPos);
|
||||
}
|
||||
|
||||
// --- PROPORTIONAL CORRECTION ---
|
||||
double error = vel - velo1;
|
||||
double correction = kP * error;
|
||||
TELE.addData("Velocity1", velo);
|
||||
TELE.addData("Velocity2", veloq);
|
||||
TELE.addData("Encoder Velocity", flywheel.getVelo());
|
||||
TELE.addData("Power", robot.shooter1.getPower());
|
||||
TELE.addData("Steady?", flywheel.getSteady());
|
||||
TELE.addData("Position1", robot.shooter1.getCurrentPosition()/28);
|
||||
TELE.addData("Position2", robot.shooter2.getCurrentPosition()/28);
|
||||
|
||||
// limit how fast power changes (prevents oscillation)
|
||||
correction = Math.max(-maxStep, Math.min(maxStep, correction));
|
||||
|
||||
// --- FINAL MOTOR POWER ---
|
||||
powPID = feed + correction;
|
||||
|
||||
// clamp to allowed range
|
||||
powPID = Math.max(0, Math.min(1, powPID));
|
||||
|
||||
if (vel - velo > 1000){
|
||||
powPID = 1;
|
||||
} else if (velo - vel > 1000){
|
||||
powPID = 0;
|
||||
}
|
||||
|
||||
shooter.setVelocity(powPID);
|
||||
|
||||
if (shoot) {
|
||||
robot.transferServo.setPosition(transferServo_in);
|
||||
} else {
|
||||
robot.transferServo.setPosition(transferServo_out);
|
||||
}
|
||||
|
||||
shooter.update();
|
||||
|
||||
TELE.addData("Revolutions", shooter.getECPRPosition());
|
||||
TELE.addData("hoodPos", shooter.gethoodPosition());
|
||||
TELE.addData("turretPos", shooter.getTurretPosition());
|
||||
TELE.addData("Power Fly 1", robot.shooter1.getPower());
|
||||
TELE.addData("Power Fly 2", robot.shooter2.getPower());
|
||||
TELE.addData("powPID", shooter.getpowPID());
|
||||
TELE.addData("Velocity", velo);
|
||||
TELE.update();
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public double hoodAnglePrediction(double distance) {
|
||||
double L = 0.298317;
|
||||
double A = 1.02124;
|
||||
double k = 0.0157892;
|
||||
double n = 3.39375;
|
||||
|
||||
double dist = Math.sqrt(distance*distance+24*24);
|
||||
|
||||
return L + A * Math.exp(-Math.pow(k * dist, n));
|
||||
}
|
||||
public static double velPrediction(double distance) {
|
||||
|
||||
double x = Math.sqrt(distance*distance+24*24);
|
||||
|
||||
|
||||
|
||||
double A = -211149.992;
|
||||
double B = -1.19943;
|
||||
double C = 3720.15909;
|
||||
|
||||
return A * Math.pow(x, B) + C;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,33 +1,25 @@
|
||||
package org.firstinspires.ftc.teamcode.utils;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.qualcomm.hardware.rev.RevColorSensorV3;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
|
||||
@Config
|
||||
|
||||
@Disabled
|
||||
@TeleOp
|
||||
public class ConfigureColorRangefinder extends LinearOpMode {
|
||||
|
||||
public static int LED_Brightness = 50;
|
||||
|
||||
public static int lowerGreen = 110;
|
||||
|
||||
public static int higherGreen = 150;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "color"));
|
||||
ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "Color"));
|
||||
waitForStart();
|
||||
/* Using this example configuration, you can detect both artifact colors based on which pin is reading true:
|
||||
pin0 --> purple
|
||||
pin1 --> green */
|
||||
crf.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // green
|
||||
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple
|
||||
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255);
|
||||
crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement
|
||||
|
||||
crf.setLedBrightness(LED_Brightness);
|
||||
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 160 / 360.0 * 255, 190 / 360.0 * 255); // purple
|
||||
crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 25); // 10mm or closer requirement
|
||||
crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 110 / 360.0 * 255, 140 / 360.0 * 255); // green
|
||||
crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 25); // 10mm or closer requirement
|
||||
}
|
||||
}
|
||||
|
||||
@@ -142,7 +134,6 @@ class ColorRangefinder {
|
||||
|
||||
/**
|
||||
* Read distance via I2C
|
||||
*
|
||||
* @return distance in millimeters
|
||||
*/
|
||||
public double readDistance() {
|
||||
|
||||
@@ -0,0 +1,90 @@
|
||||
package org.firstinspires.ftc.teamcode.utils;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
|
||||
@Config
|
||||
public class Flywheel {
|
||||
public static double kP = 0.001; // small proportional gain (tune this)
|
||||
public static double maxStep = 0.06; // prevents sudden jumps
|
||||
double initPos = 0.0;
|
||||
double stamp = 0.0;
|
||||
double stamp1 = 0.0;
|
||||
double ticker = 0.0;
|
||||
double currentPos = 0.0;
|
||||
double velo = 0.0;
|
||||
double velo1 = 0.0;
|
||||
double velo2 = 0.0;
|
||||
double velo3 = 0.0;
|
||||
double velo4 = 0.0;
|
||||
double velo5 = 0.0;
|
||||
double targetVelocity = 0.0;
|
||||
double powPID = 0.0;
|
||||
boolean steady = false;
|
||||
public Flywheel () {
|
||||
//robot = new Robot(hardwareMap);
|
||||
}
|
||||
|
||||
public double getVelo () {
|
||||
return velo;
|
||||
}
|
||||
|
||||
public boolean getSteady() {
|
||||
return steady;
|
||||
}
|
||||
|
||||
private double getTimeSeconds ()
|
||||
{
|
||||
return (double) System.currentTimeMillis()/1000.0;
|
||||
}
|
||||
|
||||
|
||||
public double manageFlywheel(int commandedVelocity, double shooter1CurPos) {
|
||||
targetVelocity = commandedVelocity;
|
||||
|
||||
ticker++;
|
||||
if (ticker % 2 == 0) {
|
||||
velo5 = velo4;
|
||||
velo4 = velo3;
|
||||
velo3 = velo2;
|
||||
velo2 = velo1;
|
||||
|
||||
currentPos = shooter1CurPos / 2048;
|
||||
stamp = getTimeSeconds(); //getRuntime();
|
||||
velo1 = 60 * ((currentPos - initPos) / (stamp - stamp1));
|
||||
initPos = currentPos;
|
||||
stamp1 = stamp;
|
||||
|
||||
velo = (velo1 + velo2 + velo3 + velo4 + velo5) / 5;
|
||||
}
|
||||
// Flywheel control code here
|
||||
if (targetVelocity - velo > 500) {
|
||||
powPID = 1.0;
|
||||
} else if (velo - targetVelocity > 500){
|
||||
powPID = 0.0;
|
||||
} else {
|
||||
double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18;
|
||||
|
||||
// --- PROPORTIONAL CORRECTION ---
|
||||
double error = targetVelocity - velo;
|
||||
double correction = kP * error;
|
||||
|
||||
// limit how fast power changes (prevents oscillation)
|
||||
correction = Math.max(-maxStep, Math.min(maxStep, correction));
|
||||
|
||||
// --- FINAL MOTOR POWER ---
|
||||
powPID = feed + correction;
|
||||
|
||||
// clamp to allowed range
|
||||
powPID = Math.max(0, Math.min(1, powPID));
|
||||
}
|
||||
|
||||
// really should be a running average of the last 5
|
||||
steady = (Math.abs(targetVelocity - velo) < 100.0);
|
||||
|
||||
return powPID;
|
||||
}
|
||||
|
||||
public void update()
|
||||
{
|
||||
}
|
||||
}
|
||||
@@ -1,53 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.utils;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
@TeleOp
|
||||
@Config
|
||||
public class PositionalServoProgrammer extends LinearOpMode {
|
||||
Robot robot;
|
||||
MultipleTelemetry TELE;
|
||||
public static double spindexPos = 0.501;
|
||||
public static double turretPos = 0.501;
|
||||
public static double transferPos = 0.501;
|
||||
public static double hoodPos = 0.501;
|
||||
|
||||
public static double scalar = 1.112;
|
||||
public static double restPos = 0.158;
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
robot = new Robot(hardwareMap);
|
||||
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
waitForStart();
|
||||
if (isStopRequested()) return;
|
||||
while (opModeIsActive()){
|
||||
if (spindexPos != 0.501){
|
||||
robot.spin1.setPosition(spindexPos);
|
||||
robot.spin2.setPosition(1-spindexPos);
|
||||
}
|
||||
if (turretPos != 0.501){
|
||||
robot.turr1.setPosition(turretPos);
|
||||
robot.turr2.setPosition(1-turretPos);
|
||||
}
|
||||
if (transferPos != 0.501){
|
||||
robot.transferServo.setPosition(transferPos);
|
||||
}
|
||||
if (hoodPos != 0.501){
|
||||
robot.hood.setPosition(hoodPos);
|
||||
}
|
||||
TELE.addData("spindexer", scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3));
|
||||
TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3));
|
||||
TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3));
|
||||
TELE.addData("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3));
|
||||
TELE.addData("spindexerA", robot.spin1Pos.getVoltage());
|
||||
TELE.addData("hoodA", robot.hoodPos.getVoltage());
|
||||
TELE.addData("transferServoA", robot.transferServoPos.getVoltage());
|
||||
TELE.addData("turretA", robot.turr1Pos.getVoltage());
|
||||
TELE.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,167 +1,59 @@
|
||||
package org.firstinspires.ftc.teamcode.utils;
|
||||
|
||||
import com.qualcomm.hardware.rev.RevColorSensorV3;
|
||||
import com.qualcomm.robotcore.hardware.AnalogInput;
|
||||
import static org.firstinspires.ftc.teamcode.variables.HardwareConfig.*;
|
||||
|
||||
import com.qualcomm.hardware.limelightvision.Limelight3A;
|
||||
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorImplEx;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.DigitalChannel;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.I2cDevice;
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
||||
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
|
||||
import org.firstinspires.ftc.vision.VisionPortal;
|
||||
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
|
||||
import org.openftc.easyopencv.OpenCvWebcam;
|
||||
|
||||
public class Robot {
|
||||
|
||||
//Initialize Public Components
|
||||
|
||||
public DcMotorEx frontLeft;
|
||||
public DcMotorEx frontRight;
|
||||
public Limelight3A limelight3A;
|
||||
|
||||
public DcMotorEx backLeft;
|
||||
|
||||
public DcMotorEx backRight;
|
||||
|
||||
public DcMotorEx intake;
|
||||
|
||||
public DcMotorEx transfer;
|
||||
public IMU imu;
|
||||
|
||||
public DcMotorEx shooter1;
|
||||
|
||||
public DcMotorEx shooter2;
|
||||
|
||||
public Servo hood;
|
||||
|
||||
public Servo transferServo;
|
||||
|
||||
public Servo rejecter;
|
||||
|
||||
public Servo turr1;
|
||||
|
||||
public Servo turr2;
|
||||
|
||||
public Servo spin1;
|
||||
|
||||
public Servo spin2;
|
||||
|
||||
public DigitalChannel pin0;
|
||||
|
||||
public DigitalChannel pin1;
|
||||
public DigitalChannel pin2;
|
||||
public DigitalChannel pin3;
|
||||
public DigitalChannel pin4;
|
||||
|
||||
public DigitalChannel pin5;
|
||||
|
||||
public AnalogInput analogInput;
|
||||
|
||||
public AnalogInput analogInput2;
|
||||
|
||||
public AnalogInput spin1Pos;
|
||||
|
||||
public AnalogInput spin2Pos;
|
||||
|
||||
public AnalogInput hoodPos;
|
||||
|
||||
public AnalogInput turr1Pos;
|
||||
|
||||
public AnalogInput turr2Pos;
|
||||
|
||||
public AnalogInput transferServoPos;
|
||||
|
||||
public AprilTagProcessor aprilTagProcessor;
|
||||
|
||||
public WebcamName webcam;
|
||||
|
||||
public DcMotorEx shooterEncoder;
|
||||
|
||||
public RevColorSensorV3 color1;
|
||||
|
||||
public RevColorSensorV3 color2;
|
||||
|
||||
public RevColorSensorV3 color3;
|
||||
|
||||
public Robot (HardwareMap hardwareMap) {
|
||||
|
||||
//Define components w/ hardware map
|
||||
|
||||
frontLeft = hardwareMap.get(DcMotorEx.class, "fl");
|
||||
frontRight = hardwareMap.get(DcMotorEx.class, "fr");
|
||||
backLeft = hardwareMap.get(DcMotorEx.class, "bl");
|
||||
backRight = hardwareMap.get(DcMotorEx.class, "br");
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
|
||||
frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
|
||||
backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
|
||||
backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
|
||||
|
||||
intake = hardwareMap.get(DcMotorEx.class, "intake");
|
||||
rejecter = hardwareMap.get(Servo.class, "rejecter");
|
||||
|
||||
shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
|
||||
|
||||
shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
|
||||
limelight3A = hardwareMap.get(Limelight3A.class, "limelight");
|
||||
|
||||
shooter1 = hardwareMap.get(DcMotorEx.class, "s1");
|
||||
shooter2 = hardwareMap.get(DcMotorEx.class, "s2");
|
||||
shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
shooter2.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
hood = hardwareMap.get(Servo.class, "hood");
|
||||
|
||||
hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
|
||||
if (USING_LL) {
|
||||
limelight3A = hardwareMap.get(Limelight3A.class, "limelight");
|
||||
limelight3A.start(); // This tells Limelight to start looking!
|
||||
}
|
||||
|
||||
turr1 = hardwareMap.get(Servo.class, "t1");
|
||||
imu = hardwareMap.get(IMU.class, "imu");
|
||||
|
||||
turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
|
||||
RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
|
||||
|
||||
turr2 = hardwareMap.get(Servo.class, "t2");
|
||||
RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
|
||||
|
||||
turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
|
||||
RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoFacingDirection, usbFacingDirection);
|
||||
|
||||
spin1 = hardwareMap.get(Servo.class, "spin1");
|
||||
imu.initialize(new IMU.Parameters(orientationOnRobot));
|
||||
|
||||
spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
|
||||
|
||||
spin2 = hardwareMap.get(Servo.class, "spin2");
|
||||
|
||||
spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
|
||||
|
||||
pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
|
||||
|
||||
pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
|
||||
|
||||
pin2 = hardwareMap.get(DigitalChannel.class, "pin2");
|
||||
|
||||
pin3 = hardwareMap.get(DigitalChannel.class, "pin3");
|
||||
|
||||
pin4 = hardwareMap.get(DigitalChannel.class, "pin4");
|
||||
|
||||
pin5 = hardwareMap.get(DigitalChannel.class, "pin5");
|
||||
|
||||
analogInput = hardwareMap.get(AnalogInput.class, "analog");
|
||||
|
||||
analogInput2 = hardwareMap.get(AnalogInput.class, "analog2");
|
||||
|
||||
transfer = hardwareMap.get(DcMotorEx.class, "transfer");
|
||||
|
||||
transferServo = hardwareMap.get(Servo.class, "transferServo");
|
||||
|
||||
transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
|
||||
|
||||
transfer.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
|
||||
|
||||
webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
|
||||
|
||||
color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
|
||||
|
||||
color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
|
||||
|
||||
color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,6 +1,8 @@
|
||||
package org.firstinspires.ftc.teamcode.subsystems;
|
||||
package org.firstinspires.ftc.teamcode.utils;
|
||||
|
||||
public interface Subsystem {
|
||||
|
||||
public void update();
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,224 @@
|
||||
package org.firstinspires.ftc.teamcode.utils.subsystems;
|
||||
|
||||
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.hardware.limelightvision.LLResult;
|
||||
import com.qualcomm.hardware.limelightvision.LLResultTypes;
|
||||
import com.qualcomm.hardware.limelightvision.LLStatus;
|
||||
import com.qualcomm.hardware.limelightvision.Limelight3A;
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Position;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
|
||||
import org.firstinspires.ftc.teamcode.variables.HardwareConfig;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
import org.firstinspires.ftc.teamcode.utils.Subsystem;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Objects;
|
||||
|
||||
public class Limelight implements Subsystem {
|
||||
|
||||
private final Limelight3A limelight;
|
||||
private final MultipleTelemetry telemetry;
|
||||
|
||||
private LLResult result;
|
||||
private LLStatus status;
|
||||
|
||||
private boolean telemetryOn = false;
|
||||
private String mode = "AT";
|
||||
|
||||
// ✅ Internal cached data
|
||||
private Pose3D botpose;
|
||||
|
||||
private double captureLatency = 0.0;
|
||||
private double targetingLatency = 0.0;
|
||||
private double parseLatency = 0.0;
|
||||
private double[] pythonOutput = new double[0];
|
||||
|
||||
private double tx = 0.0;
|
||||
private double txnc = 0.0;
|
||||
private double ty = 0.0;
|
||||
private double tync = 0.0;
|
||||
|
||||
private double ta = 0.0;
|
||||
|
||||
private List<LLResultTypes.BarcodeResult> barcodeResults = new ArrayList<>();
|
||||
private List<LLResultTypes.ClassifierResult> classifierResults = new ArrayList<>();
|
||||
private List<LLResultTypes.DetectorResult> detectorResults = new ArrayList<>();
|
||||
private List<LLResultTypes.FiducialResult> fiducialResults = new ArrayList<>();
|
||||
private List<LLResultTypes.ColorResult> colorResults = new ArrayList<>();
|
||||
|
||||
private IMU imu;
|
||||
|
||||
public Limelight(Robot robot, MultipleTelemetry tele) {
|
||||
|
||||
HardwareConfig.USING_LL= true;
|
||||
|
||||
this.limelight = robot.limelight3A;
|
||||
this.telemetry = tele;
|
||||
limelight.pipelineSwitch(1);
|
||||
|
||||
this.imu = robot.imu;
|
||||
|
||||
this.imu.resetYaw();
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void setPipeline(int pipeline) {limelight.pipelineSwitch(pipeline);}
|
||||
|
||||
public void setTelemetryOn(boolean state) { telemetryOn = state; }
|
||||
|
||||
public void setMode(String newMode) { this.mode = newMode; }
|
||||
|
||||
|
||||
|
||||
/** ✅ MAIN UPDATE LOOP */
|
||||
@Override
|
||||
public void update() {
|
||||
|
||||
|
||||
|
||||
result = limelight.getLatestResult();
|
||||
status = limelight.getStatus();
|
||||
|
||||
|
||||
|
||||
if (result != null && (Objects.equals(status.getPipelineType(), "pipe_python") || result.isValid())){
|
||||
// Refresh all cached values
|
||||
botpose = result.getBotpose();
|
||||
captureLatency = result.getCaptureLatency();
|
||||
targetingLatency= result.getTargetingLatency();
|
||||
parseLatency = result.getParseLatency();
|
||||
pythonOutput = result.getPythonOutput();
|
||||
tx = result.getTx();
|
||||
txnc = result.getTxNC();
|
||||
ty = result.getTy();
|
||||
tync = result.getTyNC();
|
||||
ta = result.getTa();
|
||||
barcodeResults = result.getBarcodeResults();
|
||||
classifierResults = result.getClassifierResults();
|
||||
detectorResults = result.getDetectorResults();
|
||||
fiducialResults = result.getFiducialResults();
|
||||
colorResults = result.getColorResults();
|
||||
}
|
||||
|
||||
if (telemetryOn) telemetryUpdate();
|
||||
}
|
||||
|
||||
/** ✅ Telemetry Output */
|
||||
private void telemetryUpdate() {
|
||||
// ✅ Use getters instead of directly accessing 'status' or cached fields
|
||||
telemetry.addData("Name", "%s", getStatus().getName());
|
||||
telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d",
|
||||
getStatus().getTemp(),
|
||||
getStatus().getCpu(),
|
||||
(int) getStatus().getFps());
|
||||
telemetry.addData("Pipeline", "Index: %d, Type: %s",
|
||||
getStatus().getPipelineIndex(),
|
||||
getStatus().getPipelineType());
|
||||
telemetry.addData("ResultNull", result == null);
|
||||
telemetry.addData("ResultValid", result.isValid());
|
||||
|
||||
|
||||
|
||||
|
||||
if (result != null && result.isValid()) {
|
||||
telemetry.addData("LL Latency", getTotalLatency());
|
||||
telemetry.addData("Capture Latency", getCaptureLatency());
|
||||
telemetry.addData("Targeting Latency", getTargetingLatency());
|
||||
telemetry.addData("Parse Latency", getParseLatency());
|
||||
telemetry.addData("PythonOutput", java.util.Arrays.toString(getPythonOutput()));
|
||||
telemetry.addData("tx", getTx());
|
||||
telemetry.addData("txnc", getTxNC());
|
||||
telemetry.addData("ty", getTy());
|
||||
telemetry.addData("tync", getTyNC());
|
||||
telemetry.addData("ta", getTa());
|
||||
|
||||
|
||||
|
||||
|
||||
telemetry.addData("BotX", getBotX());
|
||||
|
||||
telemetry.addData("BotY", getBotY());
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if (Objects.equals(mode, "BR"))
|
||||
for (LLResultTypes.BarcodeResult br : getBarcodeResults())
|
||||
telemetry.addData("Barcode", "Data: %s", br.getData());
|
||||
|
||||
if (Objects.equals(mode, "CL"))
|
||||
for (LLResultTypes.ClassifierResult cr : getClassifierResults())
|
||||
telemetry.addData("Classifier", "Class: %s, Confidence: %.2f",
|
||||
cr.getClassName(), cr.getConfidence());
|
||||
|
||||
if (Objects.equals(mode, "DE"))
|
||||
for (LLResultTypes.DetectorResult dr : getDetectorResults())
|
||||
telemetry.addData("Detector", "Class: %s, Area: %.2f",
|
||||
dr.getClassName(), dr.getTargetArea());
|
||||
|
||||
if (Objects.equals(mode, "FI"))
|
||||
for (LLResultTypes.FiducialResult fr : getFiducialResults())
|
||||
telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f",
|
||||
fr.getFiducialId(), fr.getFamily(),
|
||||
fr.getTargetXDegrees(), fr.getTargetYDegrees());
|
||||
|
||||
if (Objects.equals(mode, "CO"))
|
||||
for (LLResultTypes.ColorResult cr : getColorResults())
|
||||
telemetry.addData("Color", "X: %.2f, Y: %.2f",
|
||||
cr.getTargetXDegrees(), cr.getTargetYDegrees());
|
||||
} else {
|
||||
telemetry.addData("Limelight", "No data available");
|
||||
}
|
||||
}
|
||||
|
||||
// ✅ Getter methods (for use anywhere else in your code)
|
||||
|
||||
public Pose3D getBotPose() {
|
||||
if (botpose == null) {
|
||||
botpose = new Pose3D(
|
||||
new Position(),
|
||||
new YawPitchRollAngles(AngleUnit.DEGREES, 0.0, 0.0, 0.0, 0L)
|
||||
);
|
||||
}
|
||||
return botpose;
|
||||
}
|
||||
|
||||
public double getCaptureLatency() { return captureLatency; }
|
||||
public double getTargetingLatency() { return targetingLatency; }
|
||||
public double getTotalLatency() { return captureLatency + targetingLatency; }
|
||||
public double getParseLatency() { return parseLatency; }
|
||||
public double[] getPythonOutput() { return pythonOutput; }
|
||||
|
||||
public double getTx() { return tx; }
|
||||
public double getTxNC() { return txnc; }
|
||||
public double getTy() { return ty; }
|
||||
public double getTyNC() { return tync;}
|
||||
|
||||
public double getTa() {return ta;}
|
||||
|
||||
public double getBotX() {return getBotPose().getPosition().x;}
|
||||
|
||||
public double getBotY() {return getBotPose().getPosition().y;}
|
||||
|
||||
|
||||
|
||||
|
||||
public List<LLResultTypes.BarcodeResult> getBarcodeResults() { return barcodeResults; }
|
||||
public List<LLResultTypes.ClassifierResult> getClassifierResults() { return classifierResults; }
|
||||
public List<LLResultTypes.DetectorResult> getDetectorResults() { return detectorResults; }
|
||||
public List<LLResultTypes.FiducialResult> getFiducialResults() { return fiducialResults; }
|
||||
public List<LLResultTypes.ColorResult> getColorResults() { return colorResults; }
|
||||
|
||||
public LLStatus getStatus() { return status; }
|
||||
public LLResult getRawResult() { return result; }
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
package org.firstinspires.ftc.teamcode.variables;
|
||||
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
|
||||
@Config
|
||||
public class HardwareConfig {
|
||||
|
||||
public static boolean USING_LL = false;
|
||||
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user