-
Notifications
You must be signed in to change notification settings - Fork 1
/
Nictest.py
153 lines (141 loc) · 3.82 KB
/
Nictest.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
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
142
143
144
145
146
147
148
149
150
151
152
153
from UDPComms import Publisher
import time
x = 0
# drive_pub = Publisher(8830) = controls movement of pupper (basically mode 1)
# arm_pub = Publisher(8410) = controls more movements of upper (mode 2)
# mode 2 is what you can do when pupper is not in trot mode when using a controller.
drive_pub = Publisher(8830)
# arm_pub = Publisher(8410)
# L1 = activate/disactivate
# R1 = transition between Rest mode and Trot mode.
# circle = dance or hold for 3 seconds to turn off system
# trinagle = NOTHING
# X = jump
# L2 = nothing
# R2 = Nothing
# The range for the following are form (-1, 1)
# ly = forward or backwards
# lx = left or right
# rx = turn left or right (pitch)
# ry = pitches the robot forward
def activate():
drive_pub.send({"L1": 1,
"R1": 0,
"x": 0,
"circle": 0,
"triangle": 0,
"L2": 0,
"R2": 0,
"ly": 0,
"lx": 0,
"rx": 0,
"message_rate": 20,
"ry": 0,
"dpady": 0,
"dpadx": 0})
def turnL():
drive_pub.send({"L1": 0,
"R1": 0,
"x": 0,
"circle": 0,
"triangle": 0,
"L2": 0,
"R2": 0,
"ly": 0,
"lx": 0,
"rx": -1,
"message_rate": 20,
"ry": 0,
"dpady": 0,
"dpadx": 0})
def walkL():
drive_pub.send({"L1": 1,
"R1": 0,
"x": 0,
"circle": 0,
"triangle": 0,
"L2": 0,
"R2": 0,
"ly": 0,
"lx": -1,
"rx": 0,
"message_rate": 20,
"ry": 0,
"dpady": 0,
"dpadx": 0})
def jump():
drive_pub.send({"L1": 0,
"R1": 0,
"x": 1,
"circle": 0,
"triangle": 0,
"L2": 0,
"R2": 0,
"ly": 0,
"lx": 0,
"rx": 0,
"message_rate": 20,
"ry": 0,
"dpady": 0,
"dpadx": 0})
def null():
drive_pub.send({"L1": 0,
"R1": 0,
"x": 0,
"circle": 0,
"triangle": 0,
"L2": 0,
"R2": 0,
"ly": 0,
"lx": 0,
"rx": 0,
"message_rate": 20,
"ry": 0,
"dpady": 0,
"dpadx": 0})
def trot():
drive_pub.send({"L1": 0,
"R1": 1,
"x": 0,
"circle": 0,
"triangle": 0,
"L2": 0,
"R2": 0,
"ly": 0,
"lx": 0,
"rx": 0,
"message_rate": 20,
"ry": 0,
"dpady": 0,
"dpadx": 0})
def forward():
drive_pub.send({"L1": 0,
"R1": 0,
"x": 0,
"circle": 0,
"triangle": 0,
"L2": 0,
"R2": 0,
"ly": 1,
"lx": 0,
"rx": 0,
"message_rate": 20,
"ry": 0,
"dpady": 0,
"dpadx": 0})
# TODO: create functions that allow the robot to move around (forward,back,right,left,....)
# Remember: The inputs are mainly digital except for the lx,ly and rx,ry controls.
# The digital inputs do not reset after being call unless you design them to! (i.e., if you press L1 it will remaind press)
# Each action needs to be press once then can be ignored (you don't have to keep L1 as 1 you can create an activate function the forget about it)
# TODO: make the robot move through the racing track
if __name__ == "__main__":
activate()
time.sleep(1)
trot()
time.sleep(.1)
#if x <= 10:
while True:
walkL()
time.sleep(1)
turnL()
#fnan