Specimen States written

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java

@@ -13,7 +13,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
 
-@Autonomous(name = "Specimen Test", group = "Competition")
+@Autonomous(name = "PreLoadedBlue", group = "Competition")
 public class PreLoadedBlueBasketAuto extends OpMode {
     private Telemetry telemetryA;
 
@@ -27,7 +27,7 @@ public class PreLoadedBlueBasketAuto extends OpMode {
     public void init() {
         follower = new Follower(hardwareMap);
 
-        follower.setMaxPower(.45);
+        follower.setMaxPower(.20);
 
         follower.setStartingPose(startPose);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java

@@ -1,44 +1,85 @@
 package org.firstinspires.ftc.teamcode;
 
+import com.acmerobotics.roadrunner.SequentialAction;
+import com.acmerobotics.roadrunner.SleepAction;
+import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
+import org.firstinspires.ftc.robotcore.external.State;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
 
-@Autonomous(name = "Specimen Auto", group = "Dev")
+@Autonomous(name = "Specimen Auto", group = "Development")
 
 public class SpecimenAuto extends OpMode {
     private Follower follower;
     private int state;
     private ElapsedTime runtime;
 
+    private DualMotorSliderSubsystem lift;
+    private ClawSubsystem claw;
+    private WristSubsystem wrist;
+    private ArmSubsystem arm;
+    private HangMotorSubsystem hang;
 
+
+
+    @Override
      public void init(){
-        state = 0;
+         lift = new DualMotorSliderSubsystem(hardwareMap);
-        follower = new Follower(hardwareMap);
+         claw = new ClawSubsystem(hardwareMap);
-        follower.setMaxPower(.90);
+         arm = new ArmSubsystem(hardwareMap);
-        runtime = new ElapsedTime();
+         wrist = new WristSubsystem(hardwareMap);
+         hang = new HangMotorSubsystem(hardwareMap);
+         follower = new Follower(hardwareMap);
+         runtime = new ElapsedTime();
+
+         state = 0;
+         lift.init();
+         claw.init();
+         wrist.InitAuto();
+         arm.initAuto();
+
+        follower.setMaxPower(.45);
+
 
      }
 
     @Override
     public void loop() {
+        telemetry.addData("state", state);
          switch(state) {
              case 0:
-                  //path 1
+                 wrist.toSpecimenPrep();
+                 arm.toSpecimenPrep();
+                 state = 1;
+                 new SleepAction(5);
                  break;
              case 1:
-                  //specimen drop
+                state = 2;
                  break;
              case 2:
-                  //path 2
+                 lift.toSpecimanHangHeight();
+                 new SleepAction(5);
+                 state = 3;
                  break;
              case 3:
-                 //specimen pick up
+                 wrist.toSpecimenHang();
+                 new SleepAction(5);
+                 state = 4;
                  break;
              case 4:
-                 //path 3
+                 lift.toSpecimanReleaseHeight();
+                 new SleepAction(5);
+                 state = 5;
                  break;
              case 5:
                  //specimen drop
@@ -68,5 +109,7 @@ public class SpecimenAuto extends OpMode {
                  //specimen drop
                  break;
          }
+         new SleepAction(5);
     }
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java

@@ -21,7 +21,7 @@ public class SpecimenAutoLineTest extends OpMode {
 
     private PathChain path;
 
-    private final Pose startPose = new Pose(8, 60);
+    private final Pose startPose = new Pose(8, 55);
 
     @Override
     public void init() {
@@ -36,12 +36,12 @@ public class SpecimenAutoLineTest extends OpMode {
                 .addPath(
                         // Line 1
                         new BezierLine(
-                                new Point(8.000, 60.000, Point.CARTESIAN),
+                                new Point(8.000, 55.000, Point.CARTESIAN),
                                 new Point(39.500, 60.000, Point.CARTESIAN)
                         )
                 )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
+               .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
-                .addPath(
+/*                .addPath(
                         // Line 2
                         new BezierLine(
                                 new Point(39.500, 60.000, Point.CARTESIAN),
@@ -155,7 +155,7 @@ public class SpecimenAutoLineTest extends OpMode {
                                 new Point(37.000, 68.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(180)).build();
+                .setConstantHeadingInterpolation(Math.toRadians(180))*/.build();
         follower.followPath(path);
 
         telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

@@ -19,12 +19,18 @@ public class RobotConstants {
     public final static double wristHangBlueberrySkyhook = 0.7;
     public final static int slideHangBlueberrySkyhook = 500;
     public final static int slideBelowFloor = -150;
+    public final static int slideSpecimanHang = 900;
+    public final static int slideSpecimanRelease = 200;
     public final static int backwardBucketDrop = 4670;
     public final static double armBucket = 0.45;
+    public final static double armSpecimen = 0.155;
     public final static double armInit = 0.13;
     public final static double wristInit = 0.125;
     public final static double wristPickup = 0.475;
     public final static double wristBucket = 0.56;
+    public final static double wristSpecimenPrep = 0.63;
+    public final static double wristSpecimenHang = 0.53;
+
     public final static double wristFloor = 0.75;
     public final static double wristBackwardBucket = 0.725;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java

@@ -8,6 +8,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armHangBlueb
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armInit;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armReverseBucket;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armSpecimen;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.ServoImplEx;
@@ -15,7 +16,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
 public class ArmSubsystem {
 
     public enum ArmState {
-        PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERE_BUCKET
+        PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERE_BUCKET, SPECIMEN
     }
 
     private ServoImplEx arm;
@@ -54,6 +55,10 @@ public class ArmSubsystem {
         arm.setPosition(armBucket);
         setState(ArmState.BUCKET);
     }
+    public void toSpecimenPrep() {
+        arm.setPosition(armSpecimen);
+        setState(ArmState.SPECIMEN);
+    }
     public void toInitPosition(){
         arm.setPosition(armInit);
         setState(ArmState.INIT);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java

@@ -7,6 +7,8 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBu
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideBelowFloor;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideHangBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideSpecimanHang;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideSpecimanRelease;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -149,6 +151,16 @@ public class DualMotorSliderSubsystem {
 
     }
 
+    public void toSpecimanHangHeight(){
+        setTargetPosition(slideSpecimanHang);
+    }
+
+    public void toSpecimanReleaseHeight(){
+        setTargetPosition(slideSpecimanRelease);
+    }
+
+
+
     public int getFixedPosition() {
         return liftSlideLeft.getCurrentPosition();
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java

@@ -94,11 +94,11 @@ public class LiftArmWrist extends LinearOpMode {
             }
 
             if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) {
-                lift.toHighBucketPosition();
+                lift.toSpecimanHangHeight();
             }
 
             if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
-                lift.toLowBucketPosition();
+                lift.toSpecimanReleaseHeight();
             }
 
             if (currentGamepad1.y && !previousGamepad1.y) {
@@ -111,11 +111,11 @@ public class LiftArmWrist extends LinearOpMode {
 
 
             if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
-               lift.toHighBucketPosition();
+               lift.setTargetPosition(lift.getCurrentPosition() + 250);
             }
 
             if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
-                lift.toLowBucketPosition();
+                lift.setTargetPosition(lift.getCurrentPosition() - 250);
 
             }
             if(currentGamepad1.left_trigger > 0){

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java

@@ -9,6 +9,8 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlu
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristInit;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpecimenHang;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpecimenPrep;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristtravel;
 
 import androidx.annotation.NonNull;
@@ -74,6 +76,16 @@ public class WristSubsystem {
         wrist.setPosition(wristBucket);
     }
 
+    public void toSpecimenPrep() {
+        setState(WristState.SPECIMEN);
+        wrist.setPosition(wristSpecimenPrep);
+    }
+
+    public void toSpecimenHang() {
+        setState(WristState.SPECIMEN);
+        wrist.setPosition(wristSpecimenHang);
+    }
+
     public void toPickupPosition() {
         setState(WristState.PICKUP);
         wrist.setPosition(wristPickup);