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

Goog

This commit is contained in:
sihan 2024-03-18 18:51:49 -05:00
parent 10a35ed79d
commit a99f08d695
8 changed files with 53 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -33,11 +33,16 @@ import lombok.Getter;
public class Robot {
public static boolean clawIsOpen;
public static double WRISTDELAY = .08;
double delay;
public static int MIN_ERROR = 1;
public static int MAX_ERROR = 6;
public double distance = Double.MAX_VALUE;
public Pose2d estimatedPose;
public Pose2d drivePose;
public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE;
public armMacroStates armMacroState = armMacroStates.IDLE;
@Getter
public Arm arm;
double delay;
@Getter
private MecanumDrive drive;
@Getter
@ -130,15 +135,23 @@ public class Robot {
return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate());
}
public void update() {
Pose2d estimatedPose = null;
if (camera != null) {
estimatedPose = this.camera.estimatePoseFromAprilTag();
public void refreshPoseEstimateFromAprilTag() {
this.drive.update();
drivePose = this.drive.getPoseEstimate();
estimatedPose = this.camera.estimatePoseFromAprilTag();
if (estimatedPose != null) {
distance = Math.sqrt(Math.pow(drivePose.getX() - estimatedPose.getX(), 2) + Math.pow(drivePose.getY() - estimatedPose.getY(), 2));
}
if (distance < MAX_ERROR && distance > MIN_ERROR) {
this.drive.update(estimatedPose);
}
}
public void update() {
this.drive.update();
this.arm.update();
this.wrist.update();
this.drive.update(estimatedPose);
}
public enum pickupMacroStates {
@ -392,10 +405,10 @@ public class Robot {
public static double KP = 0.2;
public static double TOL = 0.005;
public static double MAX_DELTA = 0.04;
private PIDFController wristPController;
//Values
public static double WRISTPICKUP = 0.3;
public static double WRISTSCORE = .98;
private PIDFController wristPController;
//Servo
private Servo wrist;

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java
View File

@ -60,12 +60,13 @@ import java.util.List;
*/
@Config
public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(3, 0, 2.5);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(12, 0, .5);
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(5, 0, 4);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(15, 0, 1);
public static double LATERAL_MULTIPLIER = 1;
public static double VX_WEIGHT = 1;
public static double VY_WEIGHT = 1;
public static double OMEGA_WEIGHT = 1;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java
View File

@ -42,7 +42,7 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
public static double PARALLEL_Y = 3.94; // Y is the strafe direction
public static double PERPENDICULAR_X = 5.10;
public static double PERPENDICULAR_Y = 0.20;
public static double PERPENDICULAR_Y = 0.51;
public static double X_MULTIPLIER = 1; // Multiplier in the X direction
public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java
View File

@ -5,6 +5,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.tearabite.ielib.common.Alliance;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@ -105,6 +106,7 @@ public class AutoRed extends LinearOpMode {
this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
this.robot = new Robot().init(hardwareMap);
this.robot.getCamera().setAlliance(Alliance.Blue);
// this.robot.getCamera().initTargetingCamera();
this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
@ -112,16 +114,16 @@ public class AutoRed extends LinearOpMode {
// Do super fancy chinese shit
while (!this.isStarted()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
// randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
// parkLocation();
randomization = "CENTER";
randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
parkLocation();
// randomization = "CENTER";
this.telemetry.addData("Park Position", parkLocation);
this.telemetry.update();
}
robot.update();
scorePreloadOne();
boardScore();
sleep(250);
boardScore();
this.robot.refreshPoseEstimateFromAprilTag();
sleep(250);
this.robot.getClaw().open();
sleep(250);
park();

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java
View File

@ -169,7 +169,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
this.telemetry.update();
}
this.robot.update();
scorePreloadOne();
boardScore();
@ -188,12 +188,12 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
toStack();
break;
}
sleep(500);
this.robot.getClaw().close();
sleep(250);
this.robot.getArm().armRest();
scoreStack();
this.robot.refreshPoseEstimateFromAprilTag();
this.robot.getClaw().setPos(.83);
scoreTest();
park();

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java
View File

@ -89,13 +89,16 @@ public class MainTeleOp extends OpMode {
} else {
this.robot.getPlane().planeLock();
}
this.robot.update();
//Telemetry
int PositionLeft = this.robot.getSlides().slidesLeft.getCurrentPosition();
telemetry.addData("positionLeft", (PositionLeft));
int PositionRight = this.robot.getSlides().slidesRight.getCurrentPosition();
telemetry.addData("positionRight", (PositionRight));
telemetry.addData("Camera Pos Estimate", (robot.estimatedPose));
telemetry.addData("Pos Estimate", (robot.drivePose));
telemetry.addData("Difference", (robot.distance));
telemetry.update();
//Update Robot
this.robot.update();
}
}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java
View File

@ -6,6 +6,7 @@ import static org.firstinspires.ftc.teamcode.util.Constants.TARGETING_WEBCAM;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.tearabite.ielib.common.Alliance;
import com.tearabite.ielib.localization.AprilTagPoseEstimator;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
@ -23,9 +24,9 @@ public class Camera {
private VisionPortal aprilTagPortal;
private VisionPortal propPortal;
private boolean initialized;
private static final Pose2d TAG_CAMERA_OFFSETS = new Pose2d(8.27, 0, 0);
private static final Pose2d TAG_CAMERA_OFFSETS = new Pose2d(-8.27, -0.95, 0);
private AprilTagPoseEstimator aprilTagPoseEstimator;
public static float PROP_REJECTION_VERTICAL_UPPER = 480f * 0.33f;
public static float PROP_REJECTION_VERTICAL_UPPER = 40;
public static float PROP_REJECTION_VERTICAL_LOWER = 440;
@ -78,9 +79,13 @@ public class Camera {
return targetingCameraInitialized ? prop.getRed() : INVALID_DETECTION;
}
public void setAlliance(Alliance alliance) {
this.prop.setAlliance(alliance);
}
public StarterPosition getStartingPosition() {
if (!targetingCameraInitialized) {
return StarterPosition.UNKNOWN;
return StarterPosition.UNKNOWN;
}
Detection detection = this.getRed();

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java
View File

@ -18,10 +18,10 @@ import static org.firstinspires.ftc.teamcode.vision.Camera.PROP_REJECTION_VERTIC
import android.graphics.Canvas;
import com.tearabite.ielib.common.Alliance;
import com.tearabite.ielib.vision.ScalarRange;
import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import org.firstinspires.ftc.vision.VisionProcessor;
import org.opencv.core.Core;
import org.opencv.core.Mat;
@ -34,7 +34,7 @@ import java.util.ArrayList;
public class PropDetectionPipeline implements VisionProcessor {
CenterStageCommon.Alliance alliance;
Alliance alliance;
private Mat blurred = new Mat();
private Mat hsv = new Mat();
private Mat mask = new Mat();
@ -55,11 +55,11 @@ public class PropDetectionPipeline implements VisionProcessor {
Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0);
Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
if (alliance == CenterStageCommon.Alliance.Red) {
if (alliance == Alliance.Red) {
red.setContour(detect(FTC_RED_RANGE_1, FTC_RED_RANGE_2));
}
if (alliance == CenterStageCommon.Alliance.Blue) {
if (alliance == Alliance.Blue) {
blue.setContour(detect(FTC_BLUE_RANGE));
}
@ -132,11 +132,11 @@ public class PropDetectionPipeline implements VisionProcessor {
return this.blue;
}
public void setAlliance(CenterStageCommon.Alliance alliance) {
public void setAlliance(Alliance alliance) {
this.alliance = alliance;
}
public CenterStageCommon.Alliance getAlliance() {
public Alliance getAlliance() {
return this.alliance;
}
}