001/************************* PROJECT RON *************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
003/* Use of this source code is governed by an MIT-style license */
004/* that can be found in the repository LICENSE file.           */
005/***************************************************************/
006package com.stuypulse.robot;
007
008import com.ctre.phoenix6.SignalLogger;
009import com.stuypulse.robot.commands.shooter.ShooterSetShoot;
010import com.stuypulse.robot.commands.vision.SetIMUMode;
011import com.stuypulse.robot.commands.vision.SetMegaTagMode;
012import com.stuypulse.robot.commands.vision.SetVisionEnabled;
013import com.stuypulse.robot.commands.vision.WhitelistAllTags;
014import com.stuypulse.robot.constants.Field;
015import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
016import com.stuypulse.robot.subsystems.vision.LimelightVision;
017import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode;
018import com.stuypulse.robot.util.LoggedSignals;
019import com.stuypulse.robot.util.simulation.RobotVisualizer;
020import com.stuypulse.robot.util.simulation.Simulation;
021import com.stuypulse.robot.util.simulation.SimulationConstants;
022import com.stuypulse.robot.util.swerve.AlignmentUtil;
023
024import dev.doglog.DogLog;
025import dev.doglog.DogLogOptions;
026import edu.wpi.first.wpilibj.DataLogManager;
027import edu.wpi.first.wpilibj.DriverStation;
028import edu.wpi.first.wpilibj.DriverStation.Alliance;
029import edu.wpi.first.wpilibj.PowerDistribution;
030import edu.wpi.first.wpilibj.TimedRobot;
031import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
032import edu.wpi.first.wpilibj2.command.Command;
033import edu.wpi.first.wpilibj2.command.CommandScheduler;
034
035/**
036 * <h2>Robot Class</h2>
037 * 
038 * This is the main class for robot code, instantiated in {@link com.stuypulse.robot.Main} It extends TimedRobot, meaning that the methods in this
039 * class are called automatically during specific states of the robot.
040 * 
041 * This robot is structured using the <a href="https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html">CommandBased</a> framework.
042 */
043public class Robot extends TimedRobot {
044
045    private RobotContainer robot;
046
047    private Command auto;
048
049    private static Alliance alliance;
050
051    /**
052     * Checks the alliance the robot is on
053     * @return true if the robot is on the blue alliance, false if the robot is on the red alliance
054     */
055    public static boolean isBlue() {
056        return alliance == Alliance.Blue;
057    }
058
059    /** ********************* */
060    /** ROBOT SCHEDULEING ** */
061    /** ********************* */
062    @Override
063    public void robotInit() {
064        robot = new RobotContainer();
065        if (DriverStation.getAlliance().isPresent()) {
066            alliance = DriverStation.getAlliance().get();
067        } else {
068            alliance = Alliance.Blue;
069        }
070        DataLogManager.start();
071        DataLogManager.logNetworkTables(true);
072        System.out.println("]LOGGING DIRECTORY]: " + DataLogManager.getLogDir());
073        SignalLogger.start();
074        DogLog.setOptions(new DogLogOptions()
075            .withCaptureDs(true)
076            .withNtTunables(true)
077            .withLogExtras(true)
078            .withNtPublish(true));
079        DogLog.setPdh(new PowerDistribution());
080    }
081
082    /**
083     * This method runs when the robot connects to Driver Station.
084     * 
085     * It is used to update the robot's current alliance.
086     */
087    @Override
088    public void driverStationConnected() {
089        alliance = DriverStation.getAlliance().get();
090    }
091
092    /**
093     * This function is called every 20ms, regardless of the robot mode.
094     */
095    @Override
096    public void robotPeriodic() {
097        LoggedSignals.refreshAll();
098        CommandScheduler.getInstance().run();
099        DogLog.forceNt.log("Bot/Alliance", alliance.name());
100        DogLog.forceNt.log("Match Time", DriverStation.getMatchTime());
101        SmartDashboard.putData("Command Scheduler", CommandScheduler.getInstance());
102
103        DogLog.log("Alignment/Target Heading To Hub", 
104            AlignmentUtil.getTargetAlignmentAngle(CommandSwerveDrivetrain.getInstance().getPose(), Field.getHubPose()).getDegrees());
105    }
106
107    /******************/
108    /*** SIMULATION ***/
109    /******************/
110
111    /**
112     * This method is called when the robot first starts in simulation mode.
113     */
114    @Override
115    public void simulationInit() {
116        // start off in a convenient spot
117        CommandSwerveDrivetrain.getInstance()
118            .resetPose(SimulationConstants.ROBOTS_STARTING_POSITIONS[0]);
119    }
120
121    /**
122     * This method is called every 20ms during simulation mode.
123     */
124    @Override
125    public void simulationPeriodic() {
126        Simulation.getInstance().update();
127        RobotVisualizer.getInstance().update();
128    }
129
130    /*********************/
131    /*** DISABLED MODE ***/
132    /*********************/
133
134    /**
135     * This method is called each time when the robot is disabled.
136     */
137    @Override
138    public void disabledInit() {
139        CommandScheduler.getInstance().schedule(new SetIMUMode(1));
140        CommandScheduler.getInstance().schedule(new SetMegaTagMode(MegaTagMode.MEGATAG1));
141    }
142
143    /**
144     * This method is called every 20ms when the robot is disabled.
145     */
146    @Override
147    public void disabledPeriodic() {
148        CommandScheduler.getInstance().schedule(new SetIMUMode(1));
149        CommandScheduler.getInstance().schedule(new SetMegaTagMode(MegaTagMode.MEGATAG1));
150    }
151
152    /***********************/
153    /*** AUTONOMOUS MODE ***/
154    /***********************/
155
156    /**
157     * This method is called at the start of autonomous mode.
158     */
159    @Override
160    public void autonomousInit() {
161        auto = robot.getAutonomousCommand();
162        CommandScheduler.getInstance().schedule(new SetMegaTagMode(MegaTagMode.MEGATAG2));
163        CommandScheduler.getInstance().schedule(new SetIMUMode(4));
164        CommandScheduler.getInstance().schedule(new WhitelistAllTags());
165        LimelightVision.getInstance().disable();
166        if (auto != null) {
167            CommandScheduler.getInstance().schedule(auto);
168        }
169    }
170
171    /**
172     * This method is called every 20ms during autonomous mode.
173     */
174    @Override
175    public void autonomousPeriodic() {
176    }
177
178    /**
179     * This method is called when autonomous mode ends.
180     */
181    @Override
182    public void autonomousExit() {
183        LimelightVision.getInstance().captureRewind(25);
184    }
185
186    /*******************/
187    /*** TELEOP MODE ***/
188    /*******************/
189
190    /**
191     * This method is called at the start of teleop mode.
192     */
193    @Override
194    public void teleopInit() {
195        CommandScheduler.getInstance().schedule(new SetMegaTagMode(MegaTagMode.MEGATAG2));
196        CommandScheduler.getInstance().schedule(new SetIMUMode(4));
197        CommandScheduler.getInstance().schedule(new WhitelistAllTags());
198        CommandScheduler.getInstance().schedule(new ShooterSetShoot()); // start at SHOOT state
199        if (auto != null) {
200            auto.cancel();
201        }
202        CommandScheduler.getInstance().schedule(new SetVisionEnabled());
203        Boolean autonWon = DriverStation.getGameSpecificMessage()
204                .equals(String.valueOf(alliance.name().charAt(0)).toUpperCase());
205        DogLog.log("Auton Won", autonWon);
206    }
207
208    /**
209     * This method is called every 20ms in teleop mode.
210     */
211    @Override
212    public void teleopPeriodic() {
213    }
214
215    /**
216     * This method is called when teleop mode ends.
217     */
218    @Override
219    public void teleopExit() {
220        LimelightVision.getInstance().captureRewind(165);
221    }
222
223    /*****************/
224    /*** TEST MODE ***/
225    /*****************/
226
227    /**
228     * This method is called at the start of test mode.
229     */
230    @Override
231    public void testInit() {
232        CommandScheduler.getInstance().cancelAll();
233    }
234
235    /**
236     * This method is called every 20ms in test mode.
237     */
238    @Override
239    public void testPeriodic() {
240    }
241
242    /**
243     * This method is called when test mode ends.
244     */
245    @Override
246    public void testExit() {
247    }
248}