Merge remote-tracking branch 'origin/robotica-split' into robotica-split

# Conflicts: # TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java
svhsrobotics · Feb 27, 2024 · bc4c4d3 · bc4c4d3
2 parents e0e1bcc + 6cdfe85
commit bc4c4d3
Show file tree

Hide file tree

Showing 5 changed files with 69 additions and 9 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java
@@ -56,7 +56,7 @@ public static double getMotorVelocityF(double ticksPerSecond) {
      * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s.
      * You are free to raise this on your own if you would like. It is best determined through experimentation.
      */
-    public static double MAX_VEL = 30;
+    public static double MAX_VEL = 70;
     public static double MAX_ACCEL = MAX_VEL;
     public static double MAX_ANG_ACCEL = Math.toRadians(360);
     public static double MAX_ANG_VEL = Math.toRadians(360);
@@ -222,7 +222,7 @@ public void dropPurplePixel(boolean state) {
 
 
     public static double PLANE_START_ANGLE = 0.6;
-    public static double PLANE_LAUNCH_ANGLE = 0.80;
+    public static double PLANE_LAUNCH_ANGLE = 0.74;
     public static double PLANE_FOR_INCR = 0.001;
     public static int PLANE_DELAY = 10;
     @Override
@@ -244,7 +244,7 @@ public TrajectoryDrive getDrive() {
 
     public void recalibrateShoulder() {
         // Raise the arm up
-        shoulderMotor.setTargetPosition(500);
+        shoulderMotor.setTargetPosition(700);
         shoulderMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
         shoulderMotor.setPower(1);
         while (shoulderMotor.isBusy() && !GlobalOpMode.opMode.isStopRequested()) {}

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java
@@ -113,7 +113,7 @@ public void runOpMode() throws InterruptedException {
         List<Component> componentList = new ArrayList<>();
 
         if (config.placePixel) {
-            componentList.add(new PurplePixelComponent(robot, tensorPos, Objects.equals(config.park, "inner") || Objects.equals(config.park, "board")));
+            componentList.add(new PurplePixelComponent(robot, tensorPos, Objects.equals(config.park, "inner")));
         }
 
         switch (config.park) {

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java
@@ -154,6 +154,7 @@ public void runOpMode() throws InterruptedException {
                 // HANG
                 if (gamepad2.y) {
                     rrobot.hangMotor.setPower(1.0);
+                    rrobot.planeAngleServo.setPosition(0.3);
                 } else if (gamepad2.x) {
                     rrobot.hangMotor.setPower(-1.0);
                 } else {
@@ -204,4 +205,4 @@ public void runOpMode() throws InterruptedException {
 
     public static int PICKUP_ARM = -116 ;
     public static double PICKUP_WRIST = 0.472;
-}
+    public s
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java
@@ -1,14 +1,20 @@
 package org.firstinspires.ftc.teamcode.opmode.components;
 
+import static org.firstinspires.ftc.teamcode.util.Units.fi;
+
 import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
 
 import org.firstinspires.ftc.teamcode.drive.Robot;
+import org.firstinspires.ftc.teamcode.drive.RoboticaBot;
+import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive;
+import org.firstinspires.ftc.teamcode.opmode.TestTeleOp;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
 import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.util.GlobalOpMode;
 import org.firstinspires.ftc.teamcode.vision.TensorFlowDetection;
 
 public class GoToBoard extends Component {
-
-
     TensorFlowDetection.PropPosition propPos;
 
     public GoToBoard(Robot robot, TensorFlowDetection.PropPosition prop) {
@@ -21,6 +27,56 @@ public void drive() {
         Pose2d startPose = getRobot().getDrive().getPoseEstimate();
         TrajectorySequenceBuilder trajB = getRobot().getDrive().trajectorySequenceBuilder(startPose);
 
+        // If we're on the blue side, then set this to 1 foot, -1 foot on red side
+        int y = (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) || (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE) ? fi(4,0) : fi(-4,0);
+
+        trajB.lineTo(new Vector2d(startPose.getX(), y))
+            .lineTo(new Vector2d(fi(3, 10), y))
+                .turnTo(Math.toRadians(0))
+            //.turn(Math.toRadians(90))
+            .lineTo(new Vector2d(fi(3,8), fi(-2,11)));
+
+        TrajectorySequence traj = trajB.build();
+
+        getRobot().getDrive().followTrajectorySequence(traj);
+
+        if (getRobot().getClass() == RoboticaBot.class) {
+            ((RoboticaBot) getRobot()).recalibrateShoulder();
+            ((RoboticaBot) getRobot()).setShoulderTargetPosition(TestTeleOp.RAISED_ARM);
+        }
+
+        //GlobalOpMode.opMode.sleep(2000);
+
+        TrajectorySequenceBuilder trajC = getRobot().getDrive().trajectorySequenceBuilder(getRobot().getDrive().getPoseEstimate());
+        trajC.back(5);
+        getRobot().getDrive().followTrajectorySequence(trajC.build());
+
+        ((RoboticaBot) getRobot()).elbowServo.innerServo.setPower(-1.0);
+        GlobalOpMode.opMode.sleep(400);
+        ((RoboticaBot) getRobot()).elbowServo.innerServo.setPower(0.0);
+        ((RoboticaBot) getRobot()).wristTwistServo.setPosition(0.5);
+        ((RoboticaBot) getRobot()).wristLiftServo.setPosition(TestTeleOp.RAISED_WRIST);
+
+        TrajectorySequenceBuilder trajD = getRobot().getDrive().trajectorySequenceBuilder(getRobot().getDrive().getPoseEstimate());
+        trajD.forward(8);
+        getRobot().getDrive().followTrajectorySequence(trajD.build());
+
+        ((RoboticaBot) getRobot()).pinchServo.innerServo.setPower(1.0); // OPEN
+
+        GlobalOpMode.opMode.sleep(900);
+
+        ((RoboticaBot) getRobot()).pinchServo.innerServo.setPower(0.0);
+
+        TrajectorySequenceBuilder trajE = getRobot().getDrive().trajectorySequenceBuilder(getRobot().getDrive().getPoseEstimate());
+        trajE.back(8);
+        getRobot().getDrive().followTrajectorySequence(trajE.build());
+
+
+        GlobalOpMode.opMode.sleep(30000);
+
+
+
+
 
 
 //        //getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD

diff --git a/.../src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java b/.../src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java
@@ -77,9 +77,10 @@ public void drive() {
             }
         }
 
-        trajB.waitSeconds(1)
+        //trajB.waitSeconds(0.5)
+        trajB
                 .addTemporalMarker(() -> getRobot().dropPurplePixel(true))
-                .waitSeconds(2);
+                .waitSeconds(0.5);
 
         //if (propPosition == TensorFlowDetection.PropPosition.CENTER && (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE || getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE )) {
         if (propPosition == TensorFlowDetection.PropPosition.LEFT && getRobot().getClass() == RoboticaBot.class && getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) {
@@ -95,5 +96,7 @@ public void drive() {
         TrajectorySequence traj = trajB.build();
         getRobot().getDrive().followTrajectorySequence(traj);
         getRobot().dropPurplePixel(false);
+
+        //GlobalOpMode.opMode.sleep(1000);
     }
 }