2020code5970/robot.py at master · beavertronics/2020code5970 · 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
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
#!/usr/bin/env python3
# vim: set sw=4 noet ts=4 fileencoding=utf-8:

# Robotics specifc libraries
import wpilib
from wpilib.command import Scheduler
from wpilib.buttons import JoystickButton
import time
from networktables import NetworkTables
# from command import Scheduler
from commandbased import CommandBasedRobot

# Non robot specific libraries
import os
import sys
import math

#Linux path
sys.path.append('./subsystems')
sys.path.append('./commands')
sys.path.append('./tests')
sys.path.append('./vision')

#RoboRIO path
sys.path.insert(0, '/home/lvuser/py/subsystems')
sys.path.insert(0, '/home/lvuser/py/commands')
sys.path.insert(0, '/home/lvuser/py/tests')
sys.path.insert(0, '/home/lvuser/py/vision')

# Subsidiary objects on the robot. Ex: Cube Intake from 2017/18 season
from shooter import Shooter
from feeder import Feeder
from carrier import Carrier
from intake import Intake
#from winch import Winch
#from climber import Climber
#from climber_big import Climber_Big
#from climber_little import Climber_Little

from drivetrain import Drivetrain

# Teleop init command
from oi import OI

# For testing
import unittest
from parametrized import ParametrizedTestCase
from test_do_tank_drive import Test_Do_Tank_Drive
#from test_shooter import Test_Shooter
#from test_shooter_encoder import Test_Shooter_Encoder
from test_feeder import Test_Feeder
from test_carrier import Test_Carrier
from test_intake import Test_Intake
#from test_winch import Test_Winch
from test_climber import Test_Climber
from test_drivetrain import Test_Drivetrain

# Auto Commands
from command_bad_auto import Command_Bad_Auto

class BeaverTronicsRobot(CommandBasedRobot):

	def robotInit(self):
		super().__init__()
		# Instances of classes

		# Instantiate Subsystems
		#XXX DEBUGGING
		self.drivetrain = Drivetrain(self)
		self.shooter = Shooter(self)
		self.carrier = Carrier(self)
		self.feeder = Feeder(self)
		self.intake = Intake(self)
		#self.winch = Winch(self)
		#self.climber = Climber(self)
		#self.climber_big = Climber_Big(self)
		#self.climber_little = Climber_Little(self)

		# Instantiate Joysticks
		self.left_joy = wpilib.Joystick(1)
		self.right_joy = wpilib.Joystick(2)
		# Instantiate Xbox
		self.xbox = wpilib.Joystick(3)

		# Instantiate OI; must be AFTER joysticks are inited
		self.oi = OI(self)

		self.timer = wpilib.Timer()

		# untested vision
		#wpilib.CameraServer.launch("vision.py:main")
		#wpilib.CameraServer.launch()

	def autonomousInit(self):
		# Scheduler.getInstance().removeAll()
		data = wpilib.DriverStation.getInstance().getGameSpecificMessage()

	def autonomousPeriodic(self):
		Scheduler.getInstance().addCommand(Command_Bad_Auto(self))
		Scheduler.getInstance().run()

	def teleopInit(self):
		pass

	def teleopPeriodic(self):
		Scheduler.getInstance().run()
		self.logger.info(self.getPeriod())

	def disabledInit(self):
		#self.drivetrain.stop_robot()
		pass

	def disabledPeriodic(self):
		pass

	def testInit(self):
		return None

	def testPeriodic(self):
		suite = unittest.TestSuite()
		#suite.addTest(ParametrizedTestCase.parametrize(
		#	Test_Shooter, param=self.shooter))
		suite.addTest(ParametrizedTestCase.parametrize(
			Test_Drivetrain, param=self.drivetrain))
		#suite.addTest(ParametrizedTestCase.parametrize(
		#	Test_Shooter_Encoder, param=self.shooter.shooter_encoder))
		suite.addTest(ParametrizedTestCase.parametrize(
			Test_Feeder, param=self.feeder))
		suite.addTest(ParametrizedTestCase.parametrize(
			Test_Carrier, param=self.carrier))
		suite.addTest(ParametrizedTestCase.parametrize(
			Test_Intake, param=self.intake))
		suite.addTest(ParametrizedTestCase.parametrize(
			Test_Winch, param=self.winch))
		suite.addTest(ParametrizedTestCase.parametrize(
			Test_Climber, param=self.climber))
		# TextTestRunner just outputs to stdout what is happening
		unittest.TextTestRunner(verbosity=2).run(suite)

if __name__ == "__main__":
	wpilib.run(BeaverTronicsRobot)