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

Camera, Autos, Autoalign etc

This commit is contained in:
Thomas 2024-02-08 16:46:12 -06:00
parent be58f95d0c
commit a036de64ab
8 changed files with 125 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
TeamCode/src/main/java/opmodes/AutoBase.java
View File

@ -35,13 +35,24 @@ public abstract class AutoBase extends LinearOpMode {
protected CenterStageCommon.PropLocation propLocation;
protected final Pose2d initialPosition;
protected final CenterStageCommon.Alliance alliance;
protected final Pose2d park;
protected final Pose2d parkLeft;
protected final Pose2d parkRight;
protected Pose2d park;
protected int delay = 0;
boolean leftWasPressed = false;
boolean rightWasPressed = false;
boolean upWasPressed = false;
boolean downWasPressed = false;
protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d park) {
protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d parkLeft, Pose2d parkRight) {
this.alliance = alliance;
this.initialPosition = initialPosition;
this.park = park;
this.parkLeft = parkLeft;
this.parkRight = parkRight;
}
@Override
@ -50,11 +61,33 @@ public abstract class AutoBase extends LinearOpMode {
this.robot = new Robot(hardwareMap, telemetry, initialPosition, alliance);
this.dashboard = FtcDashboard.getInstance();
this.dashboardTelemetry = dashboard.getTelemetry();
// Wait for match to start
while(!isStarted() && !isStopRequested()) {
this.robot.update();
this.sleep(20);
boolean leftPressed = gamepad1.dpad_left;
boolean rightPressed = gamepad1.dpad_right;
boolean upPressed = gamepad1.dpad_up;
boolean downPressed = gamepad1.dpad_down;
this.telemetry.addData("To select parking location, use the dpad right or left. To add delay, use the dpad up to increase delay, and dpad down to decrease delay", "");
if(leftPressed && !leftWasPressed) {
this.park = parkLeft;
this.telemetry.addData("Park set to", "Left");
} else if(rightPressed && !rightWasPressed) {
this.park = parkRight;
this.telemetry.addData("Park set to", "Right");
} else if(upPressed && !upWasPressed) {
this.delay += 1000;
this.telemetry.addData("Delay increased by", "1000");
this.sleep(200);
} else if(downPressed && !downWasPressed) {
this.delay -= 1000;
this.telemetry.addData("Delay decreased by", "1000");
this.sleep(200);
}
this.telemetry.addData("Delay", this.delay);
this.telemetry.addData("Park set to", this.park);
}
if (isStopRequested()) { // remove later if nessacary as recent addition might be interefering
return;
@ -75,6 +108,7 @@ public abstract class AutoBase extends LinearOpMode {
propRight();
break;
}
this.sleep(delay);
moveToEasel();
prepareToScore();
scorePreloadedPixel();
@ -130,6 +164,7 @@ public abstract class AutoBase extends LinearOpMode {
}
Pose2d inferredPos = this.robot.getCamera().estimatePoseFromAprilTag();
// inferredPos = inferredPos != null ? inferredPos : robot.getDrive().getPoseEstimate(); // new code (maybe get rid of)
this.robot.getDrive().setPoseEstimate(inferredPos);
Pose2d target = new Pose2d(
60 - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, // 60 is the X position of the april tag
@ -164,7 +199,7 @@ public abstract class AutoBase extends LinearOpMode {
}
protected void determinePropLocation() {
this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN);
this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN); // changed from setArmPositionAsync
while (!this.robot.getClaw().isArmAtPosition()) {
this.robot.update();
@ -173,7 +208,7 @@ public abstract class AutoBase extends LinearOpMode {
sleep(250);
setPropLocationIfVisible(Center, Unknown);
setPropLocationIfVisible(Center, Unknown); //only works if arm is async
if (this.propLocation != Center) {
peekRight();
}

132
TeamCode/src/main/java/opmodes/AutoSandbox.java
View File

@ -1,73 +1,73 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "AutoSandbox", preselectTeleOp = "MainTeleOp")
public class AutoSandbox extends AutoBase {
private final Pose2d rendezvous = new Pose2d(12, 48, Math.toRadians(-89));
public AutoSandbox() {
super(
CenterStageCommon.Alliance.Blue,
new Pose2d(12, 63, Math.toRadians(-90)),
new Pose2d(62, 62));
}
protected void propLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33)));
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
}
protected void propCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(12,48, Math.toRadians(90)));
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
builder.lineToConstantHeading(new Vector2d(12,12));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
// builder = this.robot.getTrajectorySequenceBuilder();
// builder.turn((Math.toRadians(90)));
// this.robot.getDrive().followTrajectorySequence(builder.build());
//package opmodes;
//
//import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
//
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.acmerobotics.roadrunner.geometry.Vector2d;
//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
//
//import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
//import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
//
//@Autonomous(name = "AutoSandbox", preselectTeleOp = "MainTeleOp")
//public class AutoSandbox extends AutoBase {
// private final Pose2d rendezvous = new Pose2d(12, 48, Math.toRadians(-89));
//
// public AutoSandbox() {
// super(
// CenterStageCommon.Alliance.Blue,
// new Pose2d(12, 63, Math.toRadians(-90)),
// new Pose2d(62, 62));
// }
//
// protected void propLeft() {
// TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
// builder.lineToConstantHeading(rendezvous.vec());
// builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33)));
// builder.addDisplacementMarker(10, () -> {
// this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
// });
// this.robot.getDrive().followTrajectorySequence(builder.build());
//
// openAndLiftClaw();
}
protected void propRight() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0)));
builder.lineToConstantHeading(new Vector2d(19, 34));
builder.addTemporalMarker(0.5, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
}
}
// }
//
// protected void propCenter() {
//
// TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
// builder.lineToLinearHeading(new Pose2d(12,48, Math.toRadians(90)));
// builder.addDisplacementMarker(10, () -> {
// this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
// });
// builder.lineToConstantHeading(new Vector2d(12,12));
//
// this.robot.getDrive().followTrajectorySequence(builder.build());
//
// openAndLiftClaw();
//
//// builder = this.robot.getTrajectorySequenceBuilder();
//// builder.turn((Math.toRadians(90)));
//// this.robot.getDrive().followTrajectorySequence(builder.build());
//
//
//// TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
//// builder.lineToConstantHeading(rendezvous.vec());
//// builder.addDisplacementMarker(10, () -> {
//// this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
//// });
//// this.robot.getDrive().followTrajectorySequence(builder.build());
////
//// openAndLiftClaw();
// }
//
// protected void propRight() {
// TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
// builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0)));
// builder.lineToConstantHeading(new Vector2d(19, 34));
// builder.addTemporalMarker(0.5, () -> {
// this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
// });
// this.robot.getDrive().followTrajectorySequence(builder.build());
//
// openAndLiftClaw();
// }
//}

9
TeamCode/src/main/java/opmodes/BlueBackStage.java
View File

@ -11,17 +11,18 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "BlueBackStage", preselectTeleOp = "MainTeleOp")
public class BlueBackStage extends AutoBase {
private static final int delay = 0;
public BlueBackStage() {
super(
CenterStageCommon.Alliance.Blue,
new Pose2d(12, 63, Math.toRadians(90)),
new Pose2d(62, 62));
new Pose2d(62, 62),
new Pose2d(62, 12, Math.toRadians(0)));
}
protected void propLeft() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(17.3, 41.6, Math.toRadians(108.25)));
@ -35,7 +36,6 @@ public class BlueBackStage extends AutoBase {
}
protected void propCenter() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(12, 42, initialPosition.getHeading()));
@ -45,7 +45,6 @@ public class BlueBackStage extends AutoBase {
}
protected void propRight() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(18.25, 32, Math.toRadians(0)));

6
TeamCode/src/main/java/opmodes/BlueFrontStage.java
View File

@ -11,17 +11,17 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "BlueFrontStage", preselectTeleOp = "MainTeleOp")
public class BlueFrontStage extends AutoBase {
private static final int delay = 0;
public BlueFrontStage() {
super(
CenterStageCommon.Alliance.Blue,
new Pose2d(-36, 63, Math.toRadians(90)),
new Pose2d(62, 62),
new Pose2d(62, 12, Math.toRadians(0)));
}
protected void propLeft() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-43.5, 31.5, Math.toRadians(180)));
@ -31,7 +31,6 @@ public class BlueFrontStage extends AutoBase {
}
protected void propCenter() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-36, 42, initialPosition.getHeading()));
@ -41,7 +40,6 @@ public class BlueFrontStage extends AutoBase {
}
protected void propRight() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-39.34, 40.45, Math.toRadians(64.3)));

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

@ -3,6 +3,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.GANTRY_ARM_MIN;
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;
@ -39,6 +40,7 @@ public class MainTeleOp extends OpMode {
private int targetTagId;
private double droneLaunchStart;
private boolean wasRobotDroneLaunch;
private double gantryArmPosition = 0;
@Override
public void init() {

5
TeamCode/src/main/java/opmodes/RedBackStage.java
View File

@ -11,17 +11,16 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "RedBackStage", preselectTeleOp = "MainTeleOp")
public class RedBackStage extends AutoBase {
private static final int delay = 0;
public RedBackStage() {
super(
CenterStageCommon.Alliance.Red,
new Pose2d(12, -63, Math.toRadians(-90)),
new Pose2d(61, -12),
new Pose2d(62, -62));
}
protected void propLeft() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(18.25, -33.5, Math.toRadians(0)));
@ -31,7 +30,6 @@ public class RedBackStage extends AutoBase {
}
protected void propCenter() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(12, -42, initialPosition.getHeading()));
@ -41,7 +39,6 @@ public class RedBackStage extends AutoBase {
}
protected void propRight() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(15.9, -41.35, Math.toRadians(244.15)));

8
TeamCode/src/main/java/opmodes/RedFrontStage.java
View File

@ -11,17 +11,17 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "RedFrontStage", preselectTeleOp = "MainTeleOp")
public class RedFrontStage extends AutoBase {
private static final int delay = 0;
public RedFrontStage() {
super(
CenterStageCommon.Alliance.Red,
new Pose2d(-36, -63, Math.toRadians(-90)),
new Pose2d(61, -12));
new Pose2d(61, -12),
new Pose2d(62, -62));
}
protected void propLeft() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-40.46, -41.93, Math.toRadians(291.8)));
@ -31,7 +31,6 @@ public class RedFrontStage extends AutoBase {
}
protected void propCenter() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-36, -42, initialPosition.getHeading()));
@ -41,7 +40,6 @@ public class RedFrontStage extends AutoBase {
}
protected void propRight() {
this.sleep(delay);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-41.82, -33.68, Math.toRadians(180)));

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

@ -32,16 +32,16 @@ public class RobotConfig {
public static double AUTO_STRAFE_SLOWDOWN = 4;
// Arm
public static double PICKUP_ARM_MIN = 0.175;
public static double PICKUP_ARM_MAX = 0.760;
public static double CLAW_MIN = 0.89;
public static double CLAW_MAX = 0.65;
public static double PICKUP_ARM_MIN = .19;
public static double PICKUP_ARM_MAX = 0.76; // Changed .760 --> .74 Claw arm go up
public static double CLAW_MIN = 0.91; // Changed .89 --> .88 Probably the Claw Clamp
public static double CLAW_MAX = 0.65; // Claw release
public static double CLAW_ARM_DELTA = 0.03;
public static double CLAW_KP = 0.3;
public static double CLAW_ARM_KP = 0.15;
// Gantry
public static double GANTRY_ARM_MIN = 0.42;
public static double GANTRY_ARM_MIN = 0.43; // Changed 0.42 --> 0.45 pickup position
public static double GANTRY_ARM_MAX = 0.96;
public static int GANTRY_LIFT_DELTA = 50;
public static double GANTRY_ARM_KP = 0.1;
@ -49,7 +49,7 @@ public class RobotConfig {
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;
public static double GANTRY_ARM_DELTA_MAX = 0.015;
public static int SLIDE_UP = 820;
public static double SLIDE_POWER = 0.25;