2026Paths-Autos/Auto.java at main · Team4169/2026Paths-Autos · 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
package frc.robot;

import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkMax;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.Timer;
import frc.robot.subsystems.SwerveSubsystem;

public class Auto {

    // ── Tuning constants ──────────────────────────────────────────────────
    private static final double DRIVE_SPEED_MPS = -1.0; // m/s backward — TUNE ME
    private static final double FOLD_TIME       = 0.4;
    private static final double DRIVE_TIME      = 4.2;  // TUNE ME
    private static final double SHOOT_TIME      = 7.0;

    // ── Auto phases ───────────────────────────────────────────────────────
    private enum Phase { FOLDING_DOWN, DRIVING, SHOOTING, DONE }
    private Phase m_phase;
    private final Timer m_timer = new Timer();

    // ── Drivetrain ────────────────────────────────────────────────────────
    private final SwerveSubsystem m_swerve;

    // ── Mechanisms ────────────────────────────────────────────────────────
    private final SparkMax  m_intakeMotor;
    private final SparkMax  m_intakeFoldMotor;
    private final SparkFlex m_shooterMotor;
    private final SparkFlex m_kickerMainMotor;
    private final SparkMax  m_kickerAuxMotor;

    public Auto(
            SwerveSubsystem swerve,
            SparkMax intakeMotor,
            SparkMax intakeFoldMotor,
            SparkFlex shooterMotor,
            SparkFlex kickerMainMotor,
            SparkMax kickerAuxMotor) {

        m_swerve          = swerve;
        m_intakeMotor     = intakeMotor;
        m_intakeFoldMotor = intakeFoldMotor;
        m_shooterMotor    = shooterMotor;
        m_kickerMainMotor = kickerMainMotor;
        m_kickerAuxMotor  = kickerAuxMotor;
    }

    /** Call once from Robot.autonomousInit() */
    public void init() {
        m_phase = Phase.FOLDING_DOWN;
        m_timer.reset();
        m_timer.start();
        stopAll();
        m_intakeFoldMotor.set(0.1);
    }

    /** Call every loop from Robot.autonomousPeriodic() */
    public void update() {
        switch (m_phase) {

            case FOLDING_DOWN:
                m_intakeFoldMotor.set(0.1);
                if (m_timer.hasElapsed(FOLD_TIME)) {
                    m_intakeFoldMotor.stopMotor();
                    m_timer.reset();
                    m_phase = Phase.DRIVING;
                    driveStraight();
                }
                break;

            case DRIVING:
                driveStraight();
                if (m_timer.hasElapsed(DRIVE_TIME)) {
                    m_swerve.driveRobotRelative(new ChassisSpeeds(0, 0, 0));
                    m_timer.reset();
                    m_phase = Phase.SHOOTING;
                    m_shooterMotor.set(-0.4);
                    m_kickerMainMotor.set(0.5);
                    m_kickerAuxMotor.set(-0.44);
                    m_intakeMotor.set(-0.8);
                }
                break;

            case SHOOTING:
                if (m_timer.hasElapsed(SHOOT_TIME)) {
                    stopAll();
                    m_phase = Phase.DONE;
                }
                break;

            case DONE:
                break;
        }
    }

    /** Call from Robot.autonomousExit() or if auto is interrupted. */
    public void stop() {
        stopAll();
    }

    // ── Helpers ───────────────────────────────────────────────────────────

    private void driveStraight() {
        m_swerve.driveRobotRelative(new ChassisSpeeds(DRIVE_SPEED_MPS, 0, 0));
    }

    private void stopAll() {
        m_swerve.driveRobotRelative(new ChassisSpeeds(0, 0, 0));
        m_intakeMotor.stopMotor();
        m_intakeFoldMotor.stopMotor();
        m_shooterMotor.stopMotor();
        m_kickerMainMotor.stopMotor();
        m_kickerAuxMotor.stopMotor();
    }
}