16750 Horizontal slide configuration and testing on the bot. - Maggie

technototes · Nov 2, 2024 · e8d6746 · e8d6746
1 parent f629962
commit e8d6746
Show file tree

Hide file tree

Showing 8 changed files with 138 additions and 67 deletions.
diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java
@@ -33,7 +33,7 @@ public static Command SampleScoring(Robot r) {
             );
     }
 
-    public static Command Obs_Parking(Robot r) {
+    public static TrajectorySequenceCommand Obs_Parking(Robot r) {
         return new TrajectorySequenceCommand(r.drivebase, AutoConstants.OBS_START_TO_OBS_PARK);
     }
 }
diff --git a/.../java/org/firstinspires/ftc/sixteen750/commands/slides/HorizontalSlideNeutralCommand.java b/.../java/org/firstinspires/ftc/sixteen750/commands/slides/HorizontalSlideNeutralCommand.java
@@ -1,21 +1,22 @@
 package org.firstinspires.ftc.sixteen750.commands.slides;
 
 import com.technototes.library.command.Command;
-
+import org.firstinspires.ftc.sixteen750.Robot;
 import org.firstinspires.ftc.sixteen750.subsystems.HorizontalSlidesSubsystem;
 
 public class HorizontalSlideNeutralCommand implements Command {
+
     private HorizontalSlidesSubsystem subsystem;
 
-    public HorizontalSlideNeutralCommand(HorizontalSlidesSubsystem n){
+    public HorizontalSlideNeutralCommand(HorizontalSlidesSubsystem n) {
         subsystem = n;
         addRequirements(n);
     }
 
     @Override
-    public void execute(){
+    public void execute() {
         subsystem.slidesin();
-        subsystem.ClawServoChomp();
-        subsystem.ClawWristServoPickup();
+        subsystem.ClawServoBigOpen();
+        subsystem.ClawWristServoTransfer();
     }
 }
diff --git a/.../main/java/org/firstinspires/ftc/sixteen750/commands/slides/HorizontalSlidesCommands.java b/.../main/java/org/firstinspires/ftc/sixteen750/commands/slides/HorizontalSlidesCommands.java
@@ -0,0 +1,93 @@
+package org.firstinspires.ftc.sixteen750.commands.slides;
+
+import com.technototes.library.command.Command;
+import com.technototes.library.command.SequentialCommandGroup;
+import org.firstinspires.ftc.sixteen750.Robot;
+
+public class HorizontalSlidesCommands {
+
+    //command.create(something something)
+    //low basket, high basket, low specimen, high specimen, bucket transfer, clawwrist transfer, inc/dec, pickup, claw
+    public static Command horizontalExtend(Robot r) {
+        return Command.create(r.horizontalSlidesSubsystem::slidesout, r.horizontalSlidesSubsystem);
+    }
+
+    public static Command horizontalRetract(Robot r) {
+        return Command.create(r.horizontalSlidesSubsystem::slidesin, r.horizontalSlidesSubsystem);
+    }
+
+    public static Command clawChomp(Robot r) {
+        return Command.create(
+            r.horizontalSlidesSubsystem::ClawServoChomp,
+            r.horizontalSlidesSubsystem
+        );
+    }
+
+    public static Command clawOpen(Robot r) {
+        return Command.create(
+            r.horizontalSlidesSubsystem::ClawServoBigOpen,
+            r.horizontalSlidesSubsystem
+        );
+    }
+
+    public static Command wristTransfer(Robot r) {
+        return Command.create(
+            r.horizontalSlidesSubsystem::ClawWristServoTransfer,
+            r.horizontalSlidesSubsystem
+        );
+    }
+
+    public static SequentialCommandGroup transferring(Robot r) {
+        return new SequentialCommandGroup(
+            Command.create(
+                r.horizontalSlidesSubsystem::ClawWristServoTransfer,
+                r.horizontalSlidesSubsystem
+            ),
+            Command.create(r.horizontalSlidesSubsystem::slidesin, r.horizontalSlidesSubsystem),
+            Command.create(
+                r.horizontalSlidesSubsystem::ClawServoBigOpen,
+                r.horizontalSlidesSubsystem
+            )
+            // commands for vertical slide bucket transfer position first, then wrist transferring
+        );
+    }
+
+    public static Command wristPickup(Robot r) {
+        return Command.create(
+            r.horizontalSlidesSubsystem::ClawWristServoPickup,
+            r.horizontalSlidesSubsystem
+        );
+    }
+
+    public static Command wristIncrement(Robot r) {
+        return Command.create(
+            r.horizontalSlidesSubsystem::ClawWristServoIncrement,
+            r.horizontalSlidesSubsystem
+        );
+    }
+
+    public static Command wristDecrement(Robot r) {
+        return Command.create(
+            r.horizontalSlidesSubsystem::ClawWristServoDecrement,
+            r.horizontalSlidesSubsystem
+        );
+    }
+
+    public static SequentialCommandGroup intake(Robot r) {
+        return new SequentialCommandGroup(
+            Command.create(
+                r.horizontalSlidesSubsystem::ClawWristServoTransfer,
+                r.horizontalSlidesSubsystem
+            ),
+            Command.create(r.horizontalSlidesSubsystem::slidesout, r.horizontalSlidesSubsystem),
+            Command.create(
+                r.horizontalSlidesSubsystem::ClawWristServoPickup,
+                r.horizontalSlidesSubsystem
+            ),
+            Command.create(
+                r.horizontalSlidesSubsystem::ClawServoBigOpen,
+                r.horizontalSlidesSubsystem
+            )
+        );
+    }
+}
diff --git a/...een750/src/main/java/org/firstinspires/ftc/sixteen750/commands/slides/SlidesCommands.java b/...een750/src/main/java/org/firstinspires/ftc/sixteen750/commands/slides/SlidesCommands.java
diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorController.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorController.java
@@ -1,13 +1,11 @@
 package org.firstinspires.ftc.sixteen750.controls;
 
-import com.technototes.library.command.Command;
 import com.technototes.library.control.CommandButton;
 import com.technototes.library.control.CommandGamepad;
 import com.technototes.library.control.Stick;
-
 import org.firstinspires.ftc.sixteen750.Robot;
 import org.firstinspires.ftc.sixteen750.Setup;
-import org.firstinspires.ftc.sixteen750.commands.slides.SlidesCommands;
+import org.firstinspires.ftc.sixteen750.commands.slides.HorizontalSlidesCommands;
 
 public class OperatorController {
 
@@ -20,6 +18,8 @@ public class OperatorController {
     public CommandButton wristTransfer;
     public CommandButton wristIncrement;
     public CommandButton wristDecrement;
+    public CommandButton horislidesExtend;
+    public CommandButton horislidesRetract;
 
     public OperatorController(CommandGamepad g, Robot r) {
         robot = r;
@@ -33,9 +33,11 @@ private void AssignNamedControllerButton() {
         closeClaw = gamepad.ps_cross;
         wristTransfer = gamepad.ps_triangle;
         wristPickup = gamepad.ps_square;
-        wristIncrement = gamepad.dpadUp;
-        wristDecrement = gamepad.dpadDown;
+        wristIncrement = gamepad.dpadRight;
+        wristDecrement = gamepad.dpadLeft;
         horislidesLeftStick = gamepad.leftStick;
+        horislidesExtend = gamepad.dpadUp;
+        horislidesRetract = gamepad.dpadDown;
     }
 
     private void BindButtons() {
@@ -46,11 +48,13 @@ private void BindButtons() {
     }
 
     private void bindHorizontalSlidesControls() {
-        openClaw.whenPressed(SlidesCommands.clawOpen(robot));
-        closeClaw.whenPressed(SlidesCommands.clawChomp(robot));
-        wristPickup.whenPressed(SlidesCommands.wristPickup(robot));
-        wristTransfer.whenPressed(SlidesCommands.wristTransfer(robot));
-        wristIncrement.whenPressed(SlidesCommands.wristIncrement(robot));
-        wristDecrement.whenPressed(SlidesCommands.wristDecrement(robot));
+        openClaw.whenPressed(HorizontalSlidesCommands.clawOpen(robot));
+        closeClaw.whenPressed(HorizontalSlidesCommands.clawChomp(robot));
+        wristPickup.whenPressed(HorizontalSlidesCommands.wristPickup(robot));
+        wristTransfer.whenPressed(HorizontalSlidesCommands.wristTransfer(robot));
+        wristIncrement.whenPressed(HorizontalSlidesCommands.wristIncrement(robot));
+        wristDecrement.whenPressed(HorizontalSlidesCommands.wristDecrement(robot));
+        horislidesExtend.whenPressed(HorizontalSlidesCommands.horizontalExtend(robot));
+        horislidesRetract.whenPressed(HorizontalSlidesCommands.horizontalRetract(robot));
     }
 }
diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java
@@ -11,6 +11,7 @@
 import org.firstinspires.ftc.sixteen750.Robot;
 import org.firstinspires.ftc.sixteen750.Setup;
 import org.firstinspires.ftc.sixteen750.commands.driving.DrivingCommands;
+import org.firstinspires.ftc.sixteen750.commands.slides.HorizontalSlideNeutralCommand;
 import org.firstinspires.ftc.sixteen750.controls.DriverController;
 import org.firstinspires.ftc.sixteen750.controls.OperatorController;
 import org.firstinspires.ftc.sixteen750.helpers.StartingPosition;
@@ -38,6 +39,16 @@ public void uponInit() {
                 DrivingCommands.ResetGyro(robot.drivebase),
                 OpModeState.INIT
             );
+            //            CommandScheduler.scheduleOnceForState(
+            //                    new HorizontalSlideNeutralCommand(robot.horizontalSlidesSubsystem),
+            //                    OpModeState.RUN
+            //            );
         }
     }
+
+    @Override
+    public void uponStart() {
+        robot.horizontalSlidesSubsystem.slidesin();
+        robot.horizontalSlidesSubsystem.ClawWristServoTransfer();
+    }
 }
diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/SingleTeleOp.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/SingleTeleOp.java
@@ -35,5 +35,9 @@ public void uponInit() {
             DrivingCommands.ResetGyro(robot.drivebase),
             OpModeState.INIT
         );
+        CommandScheduler.scheduleOnceForState(
+            new HorizontalSlideNeutralCommand(robot.horizontalSlidesSubsystem),
+            OpModeState.RUN
+        );
     }
 }
diff --git a/.../src/main/java/org/firstinspires/ftc/sixteen750/subsystems/HorizontalSlidesSubsystem.java b/.../src/main/java/org/firstinspires/ftc/sixteen750/subsystems/HorizontalSlidesSubsystem.java
@@ -20,21 +20,21 @@ public class HorizontalSlidesSubsystem implements Subsystem, Loggable {
     public static double WRIST_POS = 0;
     public static double LINK_POS = 1;
 
-    public static double LinkServoExtend = 0;
-    public static double LinkServoRetract = 0;
-    public static double ClawServoClose = 0;
-    public static double ClawServoOpen = 0.3;
+    public static double LinkServoExtend = 0.6;
+    public static double LinkServoRetract = 1;
+    public static double ClawServoClose = 0.1;
+    public static double ClawServoOpen = 0.5;
     public static double WristServoTransfer = 0.3;
     public static double WristServoPickup = 1;
     public static double WristServoIncrement = 0.555;
 
     @Log(name = "wristTarget")
     public double wristTargetPos;
+
     public Servo wristServo;
     public Servo clawServo;
     public Servo linkServo;
 
-
     private boolean isHardware;
 
     public HorizontalSlidesSubsystem(Hardware hw) {
@@ -74,12 +74,12 @@ public void ClawServoBigOpen() {
     }
 
     public void ClawWristServoPickup() {
-        wristServo.setPosition(WristServoPickup); //lowers claw to intake
+        setWristPos(WristServoPickup); //lowers claw to intake
     }
 
     public void ClawWristServoTransfer() {
         // positions for the arm of the bot for transfer
-        wristServo.setPosition(WristServoTransfer);
+        setWristPos(WristServoTransfer);
     }
 
     public void ClawWristServoIncrement() {
@@ -91,7 +91,7 @@ public void ClawWristServoDecrement() {
         // the arm's position to score
         setWristPos(wristTargetPos - WristServoIncrement);
     }
-  
+
     private void setWristPos(double w) {
         if (wristServo != null) {
             wristServo.setPosition(w);