Merge remote-tracking branch 'origin/master'

1198159 · 1198159 · commit 673a07bc2d7b · 2021-11-11T10:52:16.000-08:00
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/Controls.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/Controls.java
@@ -1,21 +1,23 @@
 package org.firstinspires.ftc.teamcode;
 
+import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.BUCKET_CONNECTED;
 import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.CAP_CONNECTED;
 import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.CAROUSEL_CONNECTED;
 import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.DEPOSIT_CONNECTED;
 import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.DRIVE_CONNECTED;
 import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.INTAKE_CONNECTED;
-import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.LIFT_CONNECTED;
 
 import com.technototes.library.command.WaitCommand;
 import com.technototes.library.control.gamepad.CommandAxis;
 import com.technototes.library.control.gamepad.CommandButton;
 import com.technototes.library.control.gamepad.CommandGamepad;
 import com.technototes.library.control.gamepad.Stick;
 
-import org.firstinspires.ftc.teamcode.commands.arm.ArmCollectCommand;
-import org.firstinspires.ftc.teamcode.commands.arm.ArmLevel1Command;
-import org.firstinspires.ftc.teamcode.commands.arm.ArmLevel3Command;
+import org.firstinspires.ftc.teamcode.commands.bucket.BucketCollectedCommand;
+import org.firstinspires.ftc.teamcode.commands.bucket.BucketCollectingCommand;
+import org.firstinspires.ftc.teamcode.commands.bucket.BucketUnloadLevel1Command;
+import org.firstinspires.ftc.teamcode.commands.bucket.BucketUnloadLevel2Command;
+import org.firstinspires.ftc.teamcode.commands.bucket.BucketUnloadLevel3Command;
 import org.firstinspires.ftc.teamcode.commands.carousel.CarouselLeftCommand;
 import org.firstinspires.ftc.teamcode.commands.carousel.CarouselRightCommand;
 import org.firstinspires.ftc.teamcode.commands.deposit.DumpVariableCommand;
@@ -70,12 +72,13 @@ public Controls(CommandGamepad g, Robot r){
         driveLeftStick = gamepad.leftStick;
         driveRightStick = gamepad.rightStick;
 
-        if(LIFT_CONNECTED) bindArmControls();
+//        if(LIFT_CONNECTED) bindArmControls();
         if(DEPOSIT_CONNECTED) bindDepositControls();
         if(DRIVE_CONNECTED) bindDriveControls();
         if(INTAKE_CONNECTED) bindIntakeControls();
         if(CAROUSEL_CONNECTED) bindCarouselControls();
         if(CAP_CONNECTED) bindCapControls();
+        if(BUCKET_CONNECTED) bindBucketControls();
     }
 
 
@@ -88,10 +91,19 @@ public void bindDepositControls(){
         // slideAdjustInButton.whilePressed(new ArmTranslateCommand(robot.depositSubsystem, 0.01));
     }
 
-    public void bindArmControls(){
-        neutralHubButton.whenPressed(new ArmLevel1Command(robot.armSubsystem));
-        specificHubButton.whenPressed(new ArmLevel3Command(robot.armSubsystem));
-        toIntakeButton.whenPressed(new ArmCollectCommand(robot.armSubsystem));
+//    public void bindArmControls(){
+//        neutralHubButton.whenPressed(new ArmLevel1Command(robot.armSubsystem));
+//        specificHubButton.whenPressed(new ArmLevel3Command(robot.armSubsystem));
+//        toIntakeButton.whenPressed(new ArmCollectCommand(robot.armSubsystem));
+//    }
+
+    public void bindBucketControls(){
+        neutralHubButton.whenPressed(new BucketUnloadLevel1Command(robot.bucketSubsystem));
+        specificHubButton.whenPressed(new BucketUnloadLevel3Command(robot.bucketSubsystem));
+        toIntakeButton.whenPressed(new BucketCollectedCommand(robot.bucketSubsystem)
+                                .andThen(new BucketCollectingCommand(robot.bucketSubsystem))
+                                .andThen(new BucketCollectedCommand(robot.bucketSubsystem))
+        );
     }
 
     public void bindDriveControls(){
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/AutonomousDuckCommandGroup.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/AutonomousDuckCommandGroup.java
@@ -11,11 +11,11 @@
 import org.firstinspires.ftc.teamcode.subsystems.DepositSubsystem;
 import org.firstinspires.ftc.teamcode.subsystems.DrivebaseSubsystem;
 
-public class AutonomousDuckCommand extends SequentialCommandGroup {
-    public AutonomousDuckCommand(DrivebaseSubsystem drive, CarouselSubsystem carousel){
+public class AutonomousDuckCommandGroup extends SequentialCommandGroup {
+    public AutonomousDuckCommandGroup(DrivebaseSubsystem drive, CarouselSubsystem carousel){
           super(new TrajectorySequenceCommand(drive, AutonomousConstants.DUCK_START_TO_CAROUSEL),
                   new CarouselSpinCommand(carousel).withTimeout(3),
                   new TrajectorySequenceCommand(drive, AutonomousConstants.DUCK_CAROUSEL_TO_PARK),
-                  CommandScheduler.getInstance()::terminateOpMode);
+                  CommandScheduler.getInstance()::terminateOpMode); //ending
     }
 }
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/AutonomousLoopCommandGroup.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/AutonomousLoopCommandGroup.java
@@ -0,0 +1,16 @@
+package org.firstinspires.ftc.teamcode.commands.autonomous;
+
+import com.technototes.library.command.SequentialCommandGroup;
+import com.technototes.path.command.TrajectorySequenceCommand;
+
+import org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DrivebaseSubsystem;
+
+public class AutonomousLoopCommandGroup extends SequentialCommandGroup {
+    public AutonomousLoopCommandGroup(DrivebaseSubsystem drivebaseSystem, BucketSubsystem bucketSystem){
+//        new TrajectorySequenceCommand(); //from shelf to depot
+        new DepotLoadingBlockCommand();
+//        new TrajectorySequenceCommand(); //from depot to shelf
+        new ShelfUnloadingLevel3Command();
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/AutonomousPhaseCommand.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/AutonomousPhaseCommand.java
@@ -0,0 +1,25 @@
+/**
+ * equivalent to AutonomousCycleCommand.java in TechnoOne
+ */
+package org.firstinspires.ftc.teamcode.commands.autonomous;
+
+import com.technototes.library.command.CommandScheduler;
+import com.technototes.library.command.SequentialCommandGroup;
+import com.technototes.path.command.TrajectorySequenceCommand;
+
+import org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.CarouselSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.DrivebaseSubsystem;
+
+public class AutonomousPhaseCommand extends SequentialCommandGroup {
+    public AutonomousPhaseCommand(DrivebaseSubsystem drivebaseSystem, CarouselSubsystem carouselSystem, BucketSubsystem bucketSystem){
+        super(new AutonomousDuckCommandGroup(drivebaseSystem, carouselSystem), //goto carousel and spin it
+//                new TrajectorySequenceCommand(), //duck goto shipping hub
+                new ShelfUnloadingLevel3Command(), //unload the preloaded cube
+                new AutonomousLoopCommandGroup(drivebaseSystem, bucketSystem), //assuming running for three loops
+                new AutonomousLoopCommandGroup(drivebaseSystem, bucketSystem),
+                new AutonomousLoopCommandGroup(drivebaseSystem, bucketSystem),
+//                new TrajectorySequenceCommand(), //goto parking
+                CommandScheduler.getInstance()::terminateOpMode); //ending
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/DepotLoadingBlockCommand.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/DepotLoadingBlockCommand.java
@@ -0,0 +1,15 @@
+package org.firstinspires.ftc.teamcode.commands.autonomous;
+
+import com.technototes.library.command.Command;
+
+public class DepotLoadingBlockCommand implements Command {
+    @Override
+    public void init() {
+
+    }
+
+    @Override
+    public void execute() {
+
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/ShelfUnloadingLevel1Command.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/ShelfUnloadingLevel1Command.java
@@ -0,0 +1,17 @@
+package org.firstinspires.ftc.teamcode.commands.autonomous;
+
+import com.technototes.library.command.Command;
+import com.technototes.library.subsystem.Subsystem;
+
+public class ShelfUnloadingLevel1Command implements Command {
+
+    @Override
+    public void init() {
+
+    }
+
+    @Override
+    public void execute() {
+
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/ShelfUnloadingLevel2Command.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/ShelfUnloadingLevel2Command.java
@@ -0,0 +1,15 @@
+package org.firstinspires.ftc.teamcode.commands.autonomous;
+
+import com.technototes.library.command.Command;
+
+public class ShelfUnloadingLevel2Command implements Command {
+    @Override
+    public void init() {
+
+    }
+
+    @Override
+    public void execute() {
+
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/ShelfUnloadingLevel3Command.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/ShelfUnloadingLevel3Command.java
@@ -0,0 +1,15 @@
+package org.firstinspires.ftc.teamcode.commands.autonomous;
+
+import com.technototes.library.command.Command;
+
+public class ShelfUnloadingLevel3Command implements Command {
+    @Override
+    public void init() {
+
+    }
+
+    @Override
+    public void execute() {
+
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketCollectedCommand.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketCollectedCommand.java
@@ -0,0 +1,10 @@
+package org.firstinspires.ftc.teamcode.commands.bucket;
+
+import org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem;
+import static org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem.BucketConstant.COMBINATION_COLLECTED;
+
+public class BucketCollectedCommand extends BucketCommand{
+    public BucketCollectedCommand(BucketSubsystem bs) {
+        super(bs, COMBINATION_COLLECTED);
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketCollectingCommand.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketCollectingCommand.java
@@ -0,0 +1,10 @@
+package org.firstinspires.ftc.teamcode.commands.bucket;
+
+import org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem;
+import static org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem.BucketConstant.COMBINATION_COLLECTING;
+
+public class BucketCollectingCommand extends BucketCommand{
+    public BucketCollectingCommand(BucketSubsystem bs) {
+        super(bs, COMBINATION_COLLECTING);
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketCommand.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketCommand.java
@@ -0,0 +1,42 @@
+package org.firstinspires.ftc.teamcode.commands.bucket;
+
+import com.technototes.library.command.Command;
+
+import org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem;
+
+import java.util.function.DoubleSupplier;
+
+public class BucketCommand implements Command {
+    /**
+     * for some reason it must be a DoubleSupplier
+     */
+    BucketSubsystem bucketSys;
+    DoubleSupplier doubleSupplier_motor;
+    DoubleSupplier doubleSupplier_servo;
+
+    public BucketCommand(BucketSubsystem bs, DoubleSupplier ds_m, DoubleSupplier ds_s){
+        this.bucketSys = bs;
+        this.doubleSupplier_motor = ds_m;
+        this.doubleSupplier_servo = ds_s;
+        addRequirements(bs);
+    }
+    public BucketCommand(BucketSubsystem bs, double d_m, double d_s){
+        this(bs, ()->d_m, ()-> d_s);
+    }
+    public BucketCommand(BucketSubsystem bs, double[] combo){
+        this(bs, combo[0], combo[1]);
+    }
+
+    @Override
+    public void init() {
+        bucketSys.setPositionCombination(doubleSupplier_motor.getAsDouble(),doubleSupplier_servo.getAsDouble());
+    }
+
+    @Override
+    public void execute() {}
+
+    @Override
+    public boolean isFinished() {
+        return bucketSys.isAtTarget();
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketTranslateCommand.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketTranslateCommand.java
@@ -0,0 +1,10 @@
+package org.firstinspires.ftc.teamcode.commands.bucket;
+
+import org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem;
+
+public class BucketTranslateCommand extends BucketCommand{
+
+    public BucketTranslateCommand(BucketSubsystem bs, double motor_targetPosition, double servo_targetPosition) {
+        super(bs, motor_targetPosition, servo_targetPosition);
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketUnloadLevel1Command.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketUnloadLevel1Command.java
@@ -0,0 +1,10 @@
+package org.firstinspires.ftc.teamcode.commands.bucket;
+
+import org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem;
+import static org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem.BucketConstant.COMBINATION_LEVEL1;
+
+public class BucketUnloadLevel1Command extends BucketCommand{
+    public BucketUnloadLevel1Command(BucketSubsystem bs) {
+        super(bs, COMBINATION_LEVEL1);
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketUnloadLevel2Command.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketUnloadLevel2Command.java
@@ -0,0 +1,11 @@
+package org.firstinspires.ftc.teamcode.commands.bucket;
+
+import org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem;
+
+import static org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem.BucketConstant.COMBINATION_LEVEL2;
+
+public class BucketUnloadLevel2Command extends BucketCommand{
+    public BucketUnloadLevel2Command(BucketSubsystem bs) {
+        super(bs, COMBINATION_LEVEL2);
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketUnloadLevel3Command.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/commands/bucket/BucketUnloadLevel3Command.java
@@ -0,0 +1,11 @@
+package org.firstinspires.ftc.teamcode.commands.bucket;
+
+import org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem;
+import static org.firstinspires.ftc.teamcode.subsystems.BucketSubsystem.BucketConstant.COMBINATION_LEVEL3;
+
+public class BucketUnloadLevel3Command extends BucketCommand{
+
+    public BucketUnloadLevel3Command(BucketSubsystem bs) {
+        super(bs, COMBINATION_LEVEL3);
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/BlueDuckAuto.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/BlueDuckAuto.java
@@ -0,0 +1,31 @@
+/**
+ * equivalent to CycleBlueAuto.java
+ */
+package org.firstinspires.ftc.teamcode.opmodes;
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.technototes.library.command.CommandScheduler;
+import com.technototes.library.logger.Loggable;
+import com.technototes.library.structure.CommandOpMode;
+import com.technototes.library.util.Alliance;
+
+import org.firstinspires.ftc.teamcode.Robot;
+import org.firstinspires.ftc.teamcode.commands.autonomous.AutonomousConstants;
+import org.firstinspires.ftc.teamcode.commands.autonomous.AutonomousPhaseCommand;
+
+@Autonomous(name = "Blue Duck")
+public class BlueDuckAuto extends CommandOpMode implements Loggable{
+    public Robot robot;
+
+    @Override
+    public void uponInit() {
+        AutonomousConstants.ALLIANCE = Alliance.BLUE;
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        robot = new Robot();
+        robot.drivebaseSubsystem.setPoseEstimate(AutonomousConstants.DUCK_START_SELECT.get());
+
+        CommandScheduler.getInstance().scheduleForState(new AutonomousPhaseCommand(robot.drivebaseSubsystem, robot.carouselSubsystem, robot.bucketSubsystem), OpModeState.RUN);
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/RedDuckAuto.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/RedDuckAuto.java
@@ -0,0 +1,31 @@
+/**
+ * copy from BlueDuckAuto.java
+ */
+package org.firstinspires.ftc.teamcode.opmodes;
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.technototes.library.command.CommandScheduler;
+import com.technototes.library.logger.Loggable;
+import com.technototes.library.structure.CommandOpMode;
+import com.technototes.library.util.Alliance;
+
+import org.firstinspires.ftc.teamcode.Robot;
+import org.firstinspires.ftc.teamcode.commands.autonomous.AutonomousConstants;
+import org.firstinspires.ftc.teamcode.commands.autonomous.AutonomousPhaseCommand;
+
+@Autonomous(name = "Red Duck")
+public class RedDuckAuto extends CommandOpMode implements Loggable{
+    public Robot robot;
+
+    @Override
+    public void uponInit() {
+        AutonomousConstants.ALLIANCE = Alliance.RED;
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        robot = new Robot();
+        robot.drivebaseSubsystem.setPoseEstimate(AutonomousConstants.DUCK_START_SELECT.get());
+
+        CommandScheduler.getInstance().scheduleForState(new AutonomousPhaseCommand(robot.drivebaseSubsystem, robot.carouselSubsystem, robot.bucketSubsystem), OpModeState.RUN);
+    }
+}
diff --git a/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BucketSubsystem.java b/TechnoTwoCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BucketSubsystem.java

Original file line number	Diff line number	Diff line change
`@@ -11,11 +11,11 @@`
`11`	`11`	`import org.firstinspires.ftc.teamcode.subsystems.DepositSubsystem;`
`12`	`12`	`import org.firstinspires.ftc.teamcode.subsystems.DrivebaseSubsystem;`
`13`	`13`
`14`		`-public class AutonomousDuckCommand extends SequentialCommandGroup {`
`15`		`- public AutonomousDuckCommand(DrivebaseSubsystem drive, CarouselSubsystem carousel){`
	`14`	`+public class AutonomousDuckCommandGroup extends SequentialCommandGroup {`
	`15`	`+ public AutonomousDuckCommandGroup(DrivebaseSubsystem drive, CarouselSubsystem carousel){`
`16`	`16`	`super(new TrajectorySequenceCommand(drive, AutonomousConstants.DUCK_START_TO_CAROUSEL),`
`17`	`17`	`new CarouselSpinCommand(carousel).withTimeout(3),`
`18`	`18`	`new TrajectorySequenceCommand(drive, AutonomousConstants.DUCK_CAROUSEL_TO_PARK),`
`19`		`- CommandScheduler.getInstance()::terminateOpMode);`
	`19`	`+ CommandScheduler.getInstance()::terminateOpMode); //ending`
`20`	`20`	`}`
`21`	`21`	`}`