diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java index 5073180..1031f5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java @@ -14,6 +14,8 @@ public class endGame_Mechs { private DcMotor hang; public static double initPos = 0.42; public static double launchPos = 0.8; + public static double finger_out = 0.5; + public static double finger_in = 0.2; public static int initHang = -1000; public static double hold = 0.8; public static double release = 0.5; @@ -32,6 +34,8 @@ public class endGame_Mechs { public endGame_Mechs(HardwareMap hardwareMap) { this.servo = hardwareMap.get(Servo.class, "Drone"); this.servo.setPosition(initPos); + this.servo = hardwareMap.get(Servo.class, "Finger"); + this.servo.setPosition(finger_out); this.hang = hardwareMap.get(DcMotor.class, "Hang"); this.hang.setTargetPosition(0); this.hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -54,6 +58,11 @@ public class endGame_Mechs { } + public void Finger_in() {this.servo.setPosition(finger_in);} + + public void Finger_out () {this.servo.setPosition(finger_out);} + + public void hang_init_pos(){ this.hang.setTargetPosition(0); this.hang.setPower(1); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java index f786cc1..8b6bb44 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.opmode.autonomous; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1; import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2; import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3; import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4; @@ -27,24 +28,24 @@ public class BlueBackStageAuto extends AutoBase { public static final Pose2d DROP_1 = new Pose2d(24.5, 45, Math.toRadians(-90)); public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.6, 34.5, Math.toRadians(-180)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.8, 33.5, Math.toRadians(-180)); public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180)); public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-187)); - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.4, 32.6, Math.toRadians(-187)); + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, 31.6, Math.toRadians(-187)); public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-187)); //public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36 - public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 11.1 , Math.toRadians(-180));//-36 + public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 11.1 , Math.toRadians(-187));//-36 - public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-180)); + public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-187)); - public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 11.1, Math.toRadians(-180)); + public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 11.1, Math.toRadians(-187)); - public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-180)); + public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-187)); @Override @@ -137,7 +138,7 @@ public class BlueBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0))); break; case 2: - builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-0))); + builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.5))); break; case 3: builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5))); @@ -150,7 +151,7 @@ public class BlueBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 5: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.68); + robot.intake.setDcMotor(0.7); robot.intake.setpos(STACK5); if (!robot.drive.isBusy()) { macroState++; @@ -159,13 +160,13 @@ public class BlueBackStageAuto extends AutoBase { //First 2 pixels off the stack are intaken by this case 6: robot.intake.setDcMotor(0.68); - robot.intake.setpos(STACK5); + robot.intake.setpos(STACK4); macroTime = getRuntime(); macroState++; break; //goes back to the easel case 7: - if (getRuntime() > macroTime + 0.03) { + if (getRuntime() > macroTime + 0.5) { //robot.intake.setDcMotor(-0.0); robot.intake.setDcMotor(-0.35); builder = this.robot.getTrajectorySequenceBuilder(); @@ -192,14 +193,14 @@ public class BlueBackStageAuto extends AutoBase { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 60, getRuntime()); macroState++; } break; //extending the macro and about to score case 9: if (robot.macroState != 0) { - robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 60, getRuntime()); } if (robot.macroState == 0 && !robot.drive.isBusy()) { robot.resetMacro(0, getRuntime()); @@ -219,7 +220,7 @@ public class BlueBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2))); break; case 2: - builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2))); + builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-3))); break; case 3: builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5 ))); @@ -241,13 +242,13 @@ public class BlueBackStageAuto extends AutoBase { //Third and 4th pixels off the stack are intaken by this case 12: robot.intake.setDcMotor(0.68); - robot.intake.setpos(STACK2); + robot.intake.setpos(STACK1); macroTime = getRuntime(); macroState++; break; //goes back to the easel case 13: - if (getRuntime() > macroTime + 0.03) { + if (getRuntime() > macroTime + 0.5) { robot.intake.setDcMotor(-0.35); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); @@ -273,7 +274,7 @@ public class BlueBackStageAuto extends AutoBase { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1 + 140, getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); macroState++; } break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java index 80913f9..2fc5903 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java @@ -77,6 +77,11 @@ public class MainTeleOp extends OpMode { this.robot.endGameMechs.reset(); } + if (controller1.getRightBumper().isPressed()) { + this.robot.endGameMechs.Finger_in(); + }else { + this.robot.endGameMechs.Finger_out(); + } //Hang Motor // if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){ @@ -88,6 +93,7 @@ public class MainTeleOp extends OpMode { // hang_counter = false; // } + if (controller1.getB().isPressed()) { this.robot.endGameMechs.hang_final_pos(); } else { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java index 7cf0d43..fc39375 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java @@ -31,6 +31,8 @@ import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigu import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder; import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceRunner; +import org.firstinspires.ftc.teamcode.roadrunner.util.AxisDirection; +import org.firstinspires.ftc.teamcode.roadrunner.util.BNO055IMUUtil; import org.firstinspires.ftc.teamcode.roadrunner.util.LynxModuleUtil; import java.util.ArrayList; @@ -101,7 +103,7 @@ public class SampleMecanumDrive extends MecanumDrive { // and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location // // For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y. - // BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y); + BNO055IMUUtil.remapZAxis(imu, AxisDirection.POS_X); rightRear = hardwareMap.get(DcMotorEx.class, "BackRight");