forked from DenfeldRobotics4009/2016_Freckles
/
robot.py
109 lines (88 loc) · 3.32 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
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
#!/usr/bin/env python3
__author__ = "nikolojedison"
#Import robot-specific libraries:
import wpilib
from wpilib.command import Scheduler
import networktables
#Import our robot-specific libraries:
from utilities.settings import Settings
#Import our subsystems:
from subsystems.drivetrain import Drivetrain
from subsystems.hat import Hat
from subsystems.ears import Ears
from subsystems.tilt import Tilt
#Import our button mapping:
from oi import OI
#Import our commands:
from macros.play_macro import PlayMacro
class Mantis(wpilib.SampleRobot):
"""Fluffy ears to scratch, lost his tail, cute little paws, likes to play fetch."""
def robotInit(self):
"""Initialise the robot."""
#Initialise the subsystems and the button mapping:
self.drivetrain = Drivetrain(self)
self.ears = Ears(self)
self.hat = Hat(self)
self.tilt = Tilt(self)
self.oi = OI(self)
#Initialise the macro shenanigans (should probably clean this up once I get the dashboard sorted)
self.macroTimeout = self.oi.smart_dashboard.getInt("Macro", 15)
self.macroName = self.oi.smart_dashboard.getString("Macro Name", "macro.csv")
Settings.str_macro_name = self.macroName
Settings.num_macro_timeout = self.macroTimeout
macro_string = str(Settings.num_macro_timeout)
print("Robot initialized with a macro timeout of " + macro_string)
self.simpleAutonCommand = PlayMacro(self, "macro.csv")
self.shooterAutonCommand = PlayMacro(self, "macro_launch.csv")
def autonomous(self):
"""Auton code."""
print("Autonomous mode activated")
try:
if self.oi.smart_dashboard.getBoolean("Play Macro"):
self.simpleAutonCommand.start()
elif self.oi.smart_dashboard.getBoolean("Shoot"):
self.shooterAutonCommand.start()
else:
print("No macro selected, waiting for teleop...")
except KeyError:
pass
#Logging loop
while self.isAutonomous() and self.isEnabled():
Scheduler.getInstance().run()
self.log()
wpilib.Timer.delay(.05) #don't burn up the cpu
def operatorControl(self):
"""Teleop code."""
print("Teleop activated")
self.simpleAutonCommand.cancel()
self.shooterAutonCommand.cancel()
#Make the joystick work for driving:
joystick = self.oi.getStick()
#Logging loop
while self.isOperatorControl() and self.isEnabled():
self.log()
Scheduler.getInstance().run()
wpilib.Timer.delay(.05) #don't burn up the cpu
def disabled(self):
"""Code to run when disabled."""
print("Robot disabled")
#Stop the drivetrain for safety's sake:
self.drivetrain.driveManual(0,0)
self.hat.manualSet(0)
self.ears.manualSet(0)
self.tilt.manualSet(0)
#Logging loop
while self.isDisabled():
self.log()
wpilib.Timer.delay(.05) #don't burn up the cpu
def test(self):
"""Code for testing."""
print("Test mode activated")
def log(self):
"""Log our subsystems."""
self.drivetrain.log()
self.ears.log()
self.hat.log()
self.tilt.log()
if __name__ == "__main__":
wpilib.run(Mantis)