-
Notifications
You must be signed in to change notification settings - Fork 0
/
climber.py
88 lines (71 loc) · 2.68 KB
/
climber.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
import phoenix5 as ctre
import wpilib
from controller import Controller
C_LEFT = 3
C_RIGHT = 4
CLIMBER_SPEED = 1
PORT_LOWER_L = 2
PORT_LOWER_R = 3
PORT_UPPER_L = 4
PORT_UPPER_R = 1
class Climber:
def __init__(self):
# End Switches
self.end_lower_l = wpilib.DigitalInput(PORT_LOWER_L)
self.end_lower_r = wpilib.DigitalInput(PORT_LOWER_R)
self.end_upper_l = wpilib.DigitalInput(PORT_UPPER_L)
self.end_upper_r = wpilib.DigitalInput(PORT_UPPER_R)
self.end_lower_l_value = False
self.end_lower_r_value = False
self.end_upper_l_value = False
self.end_upper_r_value = False
self.is_homed = False
# Motors
self.m_left = ctre.WPI_VictorSPX(C_LEFT)
self.m_right = ctre.WPI_VictorSPX(C_RIGHT)
self.m_right.setInverted(True)
self.climber_motors = wpilib.MotorControllerGroup(self.m_left, self.m_right)
def update_dashboard(self, dashboard):
dashboard.putBoolean("Climber left lower end", self.end_lower_l.get())
dashboard.putBoolean("Climber right lower", self.end_lower_r.get())
dashboard.putBoolean("Climber left upper end", self.end_upper_l.get())
dashboard.putBoolean("Climber right upper end", self.end_upper_r.get())
def teleop_control(self, controller: Controller):
if controller.is_held('RB_BUTTON'):
self.climb_up()
if controller.is_held('LB_BUTTON'):
self.climb_down()
if not controller.are_any_held('RB_BUTTON', 'LB_BUTTON'):
self.stop()
def update_end_switches(self):
self.end_lower_l_value = not self.end_lower_l.get()
self.end_lower_r_value = not self.end_lower_r.get()
self.end_upper_l_value = not self.end_upper_l.get()
self.end_upper_r_value = not self.end_upper_r.get()
def climb_up(self):
self.update_end_switches()
if not self.end_upper_l_value:
self.m_left.set(CLIMBER_SPEED)
else:
self.m_left.set(0)
if not self.end_upper_r_value:
self.m_right.set(CLIMBER_SPEED)
else:
self.m_right.set(0)
def stop(self):
self.climber_motors.set(0)
def climb_down(self):
self.update_end_switches()
if not self.end_lower_l_value:
self.m_left.set(-CLIMBER_SPEED)
else:
self.m_left.set(0)
if not self.end_lower_r_value:
self.m_right.set(-CLIMBER_SPEED)
else:
self.m_right.set(0)
def home(self):
self.update_end_switches()
self.climb_down()
if self.end_lower_l_value and self.end_upper_r_value:
self.is_homed = True