-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrobot.py
45 lines (32 loc) · 1.33 KB
/
robot.py
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
from commands2 import CommandScheduler, TimedCommandRobot, WaitCommand
from wpilib import Preferences, CameraServer, SmartDashboard
from src.robotcontainer import RobotContainer
class Robot(TimedCommandRobot):
__slots__ = ("auto_active", "core")
def __init__(self):
super().__init__()
# so we can see the webcam feed
CameraServer().launch()
self.core = RobotContainer()
Preferences.initBoolean("auto_active", False)
def disabledPeriodic(self) -> None:
# TODO: edit this to allow for some slight tolerance in the starting position
SmartDashboard.putBoolean(
"position correct?",
self.core.getAutonomousCommand()._startingPose
== self.core.odometry.estimate_pose(),
)
def robotPeriodic(self) -> None:
CommandScheduler.getInstance().run()
def autonomousInit(self) -> None:
self.core.drivetrain.brake()
self.auto_active = Preferences.getBoolean("auto_active", False)
def autonomousPeriodic(self) -> None: ...
def autonomousExit(self) -> None:
self.core.drivetrain.stop()
WaitCommand(0.5).schedule()
self.core.drivetrain.coast()
def teleopInit(self) -> None:
self.core.drivetrain.coast()
def testInit(self) -> None:
CommandScheduler.getInstance().cancelAll()