Decode26/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRobotController.java at master · SympleOrg/Decode26 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.seattlesolvers.solverslib.command.Command;
import com.seattlesolvers.solverslib.command.ParallelCommandGroup;
import com.seattlesolvers.solverslib.command.RepeatCommand;
import com.seattlesolvers.solverslib.command.SequentialCommandGroup;
import com.seattlesolvers.solverslib.command.WaitUntilCommand;
import com.seattlesolvers.solverslib.pedroCommand.FollowPathCommand;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedropathing.Paths;
import org.firstinspires.ftc.teamcode.subsystems.driveTrain.MecanumDriveSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.gate.GateSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.shooter.ShooterSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.storage.StorageSubsystem;
import org.firstinspires.ftc.teamcode.util.TeamColor;
import org.firstinspires.ftc.teamcode.util.controlcommands.ActuatorCommands;
import org.firstinspires.ftc.teamcode.util.opModes.SympleCommandOpMode;

public class AutoRobotController extends RobotControllerBase {
    private final MecanumDriveSubsystem mecanumDriveSubsystem;
    private final IntakeSubsystem intakeSubsystem;
    private final GateSubsystem gateSubsystem;
    private final StorageSubsystem storageSubsystem;
    private final ShooterSubsystem shooterSubsystem;

    private final TeamColor teamColor;

    private final ActuatorCommands actuatorCommands;

    private Command autoCommand;

    public AutoRobotController(HardwareMap hMap, Telemetry telemetry, Gamepad driverController, Gamepad actionController, String logFilePrefix, boolean logData, TeamColor teamColor) {
        super(hMap, telemetry, driverController, actionController, logFilePrefix, logData);

        this.mecanumDriveSubsystem = new MecanumDriveSubsystem(this.getHardwareMap(), this.getTelemetry(), this.getDataLogger());
        this.intakeSubsystem = new IntakeSubsystem(this.getHardwareMap(), this.getTelemetry(), this.getDataLogger());
        this.storageSubsystem = new StorageSubsystem(this.getHardwareMap(), this.getTelemetry(), this.getDataLogger());
        this.gateSubsystem = new GateSubsystem(this.getHardwareMap(), this.getTelemetry(), this.getDataLogger());
        this.shooterSubsystem = new ShooterSubsystem(this.getHardwareMap(), this.getTelemetry(), this.getDataLogger());

        this.teamColor = teamColor;

        this.actuatorCommands = new ActuatorCommands(
                this.mecanumDriveSubsystem,
                this.intakeSubsystem,
                this.gateSubsystem,
                this.storageSubsystem,
                this.shooterSubsystem
        );
    }

    @Override
    public void createKeyBindings() {

    }

    @Override
    public void initialize() {
        if (this.teamColor == TeamColor.RED) {
            getPathFollower().setStartingPose(RobotConstants.AutoConstants.RED_GOAL_POSE);
            this.autoCommand = new ParallelCommandGroup(
                    this.actuatorCommands.storageGoToShooter(),
                    new SequentialCommandGroup(
                            new FollowPathCommand(getPathFollower(), Paths.createRedPath(getPathFollower())),
                            new RepeatCommand(
                                    new SequentialCommandGroup(
                                            new WaitUntilCommand(this.shooterSubsystem::isFastEnough),
                                            this.gateSubsystem.goToState(RobotConstants.GateConstants.GateState.PUSH),
                                            new WaitUntilCommand(() -> !this.shooterSubsystem.isFastEnough()),
                                            this.gateSubsystem.goToState(RobotConstants.GateConstants.GateState.ZERO)
                                    )
                            ).withTimeout(20_000)
                    )
            );
        }
    }

    @Override
    public void initializeLoop() {

    }

    @Override
    public void postInitialize() {
        this.autoCommand.schedule();
    }

    @Override
    public void run() {

    }

    @Override
    public void postRun() {

    }

    public static class Builder extends RobotControllerBase.Builder {
        private TeamColor teamColor;

        public Builder() {
            this.logFilePrefix = "Auto";
        }

        @Override
        public Builder initializeDefaults(SympleCommandOpMode opMode) {
            super.initializeDefaults(opMode);
            return this;
        }

        public Builder setTeamColor(TeamColor teamColor) {
            this.teamColor = teamColor;
            return this;
        }

        @Override
        public AutoRobotController build() {
            if (teamColor == null) throw new IllegalArgumentException("No set team color!");
            return new AutoRobotController(this.hardwareMap, this.telemetry, this.driverController, this.actionController, this.logFilePrefix, this.logData, this.teamColor);
        }
    }
}