Claw, Gantry, and Lift

This commit is contained in:
Thomas 2023-12-17 12:36:58 -06:00
parent 63c998258e
commit 35e963b1d7
5 changed files with 10 additions and 8 deletions

View File

@ -140,11 +140,11 @@ public abstract class AutoBase extends LinearOpMode {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
if (this.alliance == CenterStageCommon.Alliance.Blue) { if (this.alliance == CenterStageCommon.Alliance.Blue) {
builder.lineToLinearHeading(new Pose2d(36, 11, 0)); builder.lineToLinearHeading(new Pose2d(35, 11, 0));
builder.lineToLinearHeading(new Pose2d(36, 38, 0)); builder.lineToLinearHeading(new Pose2d(35, 38, 0));
} else { } else {
builder.lineToLinearHeading(new Pose2d(36, -11, 0)); builder.lineToLinearHeading(new Pose2d(35, -11, 0));
builder.lineToLinearHeading(new Pose2d(36, -38, 0)); builder.lineToLinearHeading(new Pose2d(35, -36, 0));
} }
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());

View File

@ -8,10 +8,12 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@TeleOp(name = "MainTeleOp", group = "Main") @TeleOp(name = "MainTeleOp", group = "Main")
public class MainTeleOp extends OpMode { public class MainTeleOp extends OpMode {
@ -50,7 +52,7 @@ public class MainTeleOp extends OpMode {
boolean clawDown = gamepad2.a || clawDownSafe; // A boolean clawDown = gamepad2.a || clawDownSafe; // A
// Robot Drone Launch // Robot Drone Launch
boolean robotDroneLaunch = gamepad1.left_bumper; // Change if Merck wants (LT) boolean robotDroneLaunch = gamepad1.left_bumper && gamepad1.right_bumper; // Change if Merck wants (LT)
// Robot Lift // Robot Lift
boolean robotLiftRotation = gamepad2.right_trigger > 0.05; // RT boolean robotLiftRotation = gamepad2.right_trigger > 0.05; // RT

View File

@ -17,7 +17,7 @@ public class RedFrontStage extends AutoBase {
super( super(
CenterStageCommon.Alliance.Red, CenterStageCommon.Alliance.Red,
new Pose2d(-36, -63, Math.toRadians(90)), new Pose2d(-36, -63, Math.toRadians(90)),
new Pose2d(62, -12)); new Pose2d(61, -12));
} }
// propLeft will be a reverse of BlueFrontpropRight // propLeft will be a reverse of BlueFrontpropRight

View File

@ -19,7 +19,7 @@ import org.opencv.core.Point;
@Config @Config
public class Camera { public class Camera {
public static float PROP_REJECTION_VERTICAL_UPPER = 150; public static float PROP_REJECTION_VERTICAL_UPPER = 150;
public static float PROP_REJECTION_VERTICAL_LOWER = 250; public static float PROP_REJECTION_VERTICAL_LOWER = 275;
private PropDetectionPipeline prop; private PropDetectionPipeline prop;
private AprilTagProcessor aprilTag; private AprilTagProcessor aprilTag;
private VisionPortal visionPortal; private VisionPortal visionPortal;

View File

@ -101,7 +101,7 @@ public class Gantry {
} }
public void intake() { public void intake() {
this.screwServo.setPower(-1); this.screwServo.setPower(1);
} }
public void deposit() { public void deposit() {