stuff

svhsrobotics · Feb 26, 2024 · 6cdfe85 · 6cdfe85
1 parent 202e670
commit 6cdfe85
Show file tree

Hide file tree

Showing 4 changed files with 65 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);
@@ -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/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
@@ -9,7 +9,6 @@
 import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive;
 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 PurplePixelComponent extends Component {
@@ -78,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) {
@@ -97,6 +97,6 @@ public void drive() {
         getRobot().getDrive().followTrajectorySequence(traj);
         getRobot().dropPurplePixel(false);
 
-        GlobalOpMode.opMode.sleep(1000);
+        //GlobalOpMode.opMode.sleep(1000);
     }
 }