eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Drone launch pause

This commit is contained in:
Scott Barnes 2024-01-25 23:05:14 -06:00
parent 5535535302
commit 821b3b12e9
4 changed files with 20 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
TeamCode/src/main/java/opmodes/MainTeleOp.java
View File

@ -2,6 +2,7 @@ package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_FORWARD_OFFSET_IN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_LAUNCH_PAUSE_S;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_SIDE_OFFSET_IN;
@ -36,6 +37,8 @@ public class MainTeleOp extends OpMode {
private FtcDashboard dashboard;
private boolean isGantryFlipped = false;
private int targetTagId;
private double droneLaunchStart;
private boolean wasRobotDroneLaunch;
@Override
public void init() {
@ -140,11 +143,15 @@ public class MainTeleOp extends OpMode {
// Robot Drone
if (robotDroneLaunch) {
if (robotDroneLaunch && !wasRobotDroneLaunch) {
this.robot.getDrone().raise();
} else {
this.droneLaunchStart = this.getRuntime();
} else if (robotDroneLaunch && this.getRuntime() > this.droneLaunchStart + DRONE_LAUNCH_PAUSE_S){
this.robot.getDrone().launch();
} else if (!robotDroneLaunch) {
this.robot.getDrone().reset();
}
this.wasRobotDroneLaunch = robotDroneLaunch;
// Robot Lift
if (robotLiftRotation || this.liftArmShouldBeUp) {
@ -213,7 +220,7 @@ public class MainTeleOp extends OpMode {
double y = targetTagId == 2 ? 36f : -36f;
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(34, y, 0));
builder.lineToLinearHeading(new Pose2d(35, y, 0));
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
}

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java
View File

@ -120,13 +120,7 @@ public class Camera {
return null;
}
int numDetections = aprilTagDetections.size();
Pose2d acc = new Pose2d(0, 0, 0);
for (AprilTagDetection aprilTagDetection : aprilTagDetections) {
acc = acc.plus(estimatePoseFromAprilTag(aprilTagDetection));
}
return acc.div(numDetections);
return estimatePoseFromAprilTag(aprilTagDetections.stream().max((a, b) -> (int)(a.decisionMargin - b.decisionMargin)).get());
}
private Pose2d estimatePoseFromAprilTag(@NonNull AprilTagDetection aprilTagDetection) {
@ -159,6 +153,7 @@ public class Camera {
// AprilTagDetection foo = aprilTagDetection;
// telemetry.addData("id", foo.id);
// telemetry.addData("decisionMargin", foo.decisionMargin);
// telemetry.addData("ax", foo.metadata.fieldPosition.get(0));
// telemetry.addData("ay", foo.metadata.fieldPosition.get(1));
// telemetry.addData("yaw", foo.ftcPose.yaw);

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drone.java
View File

@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_LAUNCH_POS;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_ROTATION_UP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_ROTATION_UP_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_STOW_POS;
@ -16,6 +17,10 @@ public class Drone {
}
public void raise() {
this.droneLaunchServo.setPosition(DRONE_ROTATION_UP);
}
public void launch() {
this.droneLaunchServo.setPosition(DRONE_LAUNCH_POS);
}

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java
View File

@ -46,7 +46,7 @@ public class RobotConfig {
public static int GANTRY_LIFT_DELTA = 50;
public static double GANTRY_ARM_KP = 0.1;
public static double X_KP = 0.1;
public static double X_MAX = 0.84;
public static double X_MAX = 0.86;
public static double X_MIN = 0.16;
public static double X_CENTER = 0.54;
public static double GANTRY_ARM_DELTA_MAX = 0.013;
@ -54,9 +54,10 @@ public class RobotConfig {
public static double SLIDE_POWER = 0.25;
// Robot Drone Launch
public static double DRONE_ROTATION_UP = 0.2;
public static double DRONE_ROTATION_UP = 0.38;
public static double DRONE_LAUNCH_POS = 0.5;
public static double DRONE_STOW_POS = 0.2;
public static double DRONE_LAUNCH_PAUSE_S = 0.5;
// Robot Lift
public static double LIFT_ROTATION_UP = 0.4;