RobotCode2023/src/main/java/org/carlmontrobotics/robotcode2023/Robot.java at feea3e35fcdba53bf13f72e7042476d2409fe903 · DeepBlueRobotics/RobotCode2023 · 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
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package org.carlmontrobotics.robotcode2023;

import com.pathplanner.lib.server.PathPlannerServer;

import org.carlmontrobotics.lib199.MotorErrors;
import org.carlmontrobotics.lib199.sim.MockedSparkEncoder;

import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandScheduler;

public class Robot extends TimedRobot {

  public static Robot robot;
  private RobotContainer robotContainer;

  @Override
  public void robotInit() {
    robot = this;
    DataLogManager.start();
    DriverStation.startDataLog(DataLogManager.getLog());
    if (!DriverStation.isFMSAttached())
      PathPlannerServer.startServer(5811);
    robotContainer = new RobotContainer();

    SmartDashboard.putBoolean("safeMode", false);
  }

  @Override
  public void simulationInit() {
    MockedSparkEncoder.setGearing(Constants.Drivetrain.driveFrontLeftPort, Constants.Drivetrain.driveGearing);
    MockedSparkEncoder.setGearing(Constants.Drivetrain.driveFrontLeftPort, Constants.Drivetrain.driveGearing);
    MockedSparkEncoder.setGearing(Constants.Drivetrain.driveFrontLeftPort, Constants.Drivetrain.driveGearing);
    MockedSparkEncoder.setGearing(Constants.Drivetrain.driveFrontLeftPort, Constants.Drivetrain.driveGearing);
  }

  @Override
  public void robotPeriodic() {
    CommandScheduler.getInstance().run();
    MotorErrors.printSparkMaxErrorMessages();

    if (SmartDashboard.getBoolean("safeMode", false)) {
      RobotContainer.driverMode = RobotContainer.DriverMode.BABY;
    } else if (!RobotContainer.driverMode.isSlow()) {
      RobotContainer.driverMode = RobotContainer.DriverMode.NORM;
    }
  }

  @Override
  public void disabledInit() {
    new Thread(() -> {
      try {
        Thread.sleep(5000);
      } catch (InterruptedException e) {
        e.printStackTrace();
        return;
      }

      robotContainer.drivetrain.coast();
    }).start();
  }

  @Override
  public void disabledPeriodic() {
  }

  @Override
  public void disabledExit() {
  }

  @Override
  public void autonomousInit() {
    CommandScheduler.getInstance().cancelAll();
    robotContainer.drivetrain.brake();
    robotContainer.getAutonomousCommand().schedule();
  }

  @Override
  public void autonomousPeriodic() {
  }

  @Override
  public void autonomousExit() {
  }

  @Override
  public void teleopInit() {
    CommandScheduler.getInstance().cancelAll();
    robotContainer.drivetrain.brake();
  }

  @Override
  public void teleopPeriodic() {
  }

  @Override
  public void teleopExit() {
  }

  @Override
  public void testInit() {
    CommandScheduler.getInstance().cancelAll();
    robotContainer.drivetrain.brake();
  }

  @Override
  public void testPeriodic() {
  }

  @Override
  public void testExit() {
  }
}