/
robot.py
executable file
·78 lines (50 loc) · 2.1 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
#!/usr/bin/env python3
import wpilib
class MyRobot(wpilib.SampleRobot):
'''Main robot class'''
def robotInit(self):
'''Robot-wide initialization code should go here'''
self.lstick = wpilib.Joystick(0)
self.rstick = wpilib.Joystick(1)
self.l_motor = wpilib.Jaguar(1)
self.r_motor = wpilib.Jaguar(2)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
self.robot_drive = wpilib.RobotDrive(self.l_motor, self.r_motor)
self.motor = wpilib.Jaguar(4)
self.limit1 = wpilib.DigitalInput(1)
self.limit2 = wpilib.DigitalInput(2)
self.position = wpilib.AnalogInput(2)
def disabled(self):
'''Called when the robot is disabled'''
while self.isDisabled():
wpilib.Timer.delay(0.01)
def autonomous(self):
'''Called when autonomous mode is enabled'''
timer = wpilib.Timer()
timer.start()
while self.isAutonomous() and self.isEnabled():
if timer.get() < 2.0:
self.robot_drive.arcadeDrive(-1.0, -.3)
else:
self.robot_drive.arcadeDrive(0, 0)
wpilib.Timer.delay(0.01)
def operatorControl(self):
'''Called when operation control mode is enabled'''
timer = wpilib.Timer()
timer.start()
while self.isOperatorControl() and self.isEnabled():
self.robot_drive.arcadeDrive(self.lstick)
# Move a motor with a Joystick
y = self.rstick.getY()
# stop movement backwards when 1 is on
if self.limit1.get():
y = max(0, y)
# stop movement forwards when 2 is on
if self.limit2.get():
y = min(0, y)
self.motor.set(y)
wpilib.Timer.delay(0.04)
if __name__ == '__main__':
wpilib.run(MyRobot,
physics_enabled=True)