-
Notifications
You must be signed in to change notification settings - Fork 0
/
laserpointer_control.py
313 lines (269 loc) · 8.98 KB
/
laserpointer_control.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
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
# Example code to test 2 axis Robot
# JJRobots
# Example of stelarium web coordinates: 307°55'51.1" +38°26'32.9"
import tkinter as tk
from tkinter import ttk
import serial
import socket
import struct
import time
import re
try:
import pygame
from pygame.locals import *
except:
print("Warning: You need to install pygame (pip install pygame) if you want to use a GamePad...")
# Variables for Wifi connection:
IP = "192.168.4.1" # Default ROBOT IP (with BROBOT JJROBOTS_XX wifi)
PORT = 2222 # Default ROBOT port
IN_PORT = 2223 # Default ROBOT input port
LaserPointerSock = None
SERIAL_PORT = False # True if you want to use serial port, False (default) to use Wifi conection
# If you use wifi connection (SERIAL_PORT=False) be sure to connect first your PC to your robor netork (JJROBOTS_xx)
COM_PORT = '/dev/ttyACM0' # You could check the Serial port number on Arduino
COM_BAUD = 115200
ser = None
def sendCommand(Header,p1,p2,p3=0,p4=0,p5=0,p6=0,p7=0,p8=0):
base = bytearray(Header) # message
param1 = bytearray(struct.pack(">h",p1))
param2 = bytearray(struct.pack(">h",p2))
param3 = bytearray(struct.pack(">h",p3))
param4 = bytearray(struct.pack(">h",p4))
param5 = bytearray(struct.pack(">h",p5))
param6 = bytearray(struct.pack(">h",p6))
param7 = bytearray(struct.pack(">h",p7))
param8 = bytearray(struct.pack(">h",p8))
message = base+param1+param2+param3+param4+param5+param6+param7+param8
#print(message)
if SERIAL_PORT:
try:
ser.write(message)
except:
print("Could not send message (serial port)!")
else:
try:
LaserPointerSock.sendto(message,(IP,PORT))
except:
print("Could not send message (Wifi)!")
def send_position():
global az, alt, az_deg,alt_deg
c = coords.get()
print("Coords: %s" % (c))
try:
az = c.split(" ")[0].strip()
alt = c.split(" ")[1].strip()
print("Azimuth: %s" % (az))
print("Altitud: %s" % (alt))
deg, minutes, seconds,other = re.split('[°\'"]', az)
az_deg = (float(deg) + float(minutes)/60 + float(seconds)/(60*60))
if (az_deg>180):
az_deg =az_deg-360
print(deg,minutes,seconds,"=>deg:",az_deg)
deg, minutes, seconds,other = re.split('[°\'"]', alt)
alt_deg = (float(deg) + float(minutes)/60 + float(seconds)/(60*60))
if (alt_deg>180):
alt_deg = alt_deg-360
print(deg,minutes,seconds,"=>deg:",alt_deg)
sendCommand(b'JJAM',int(az_deg*100),int(alt_deg*100))
except:
print("Error on input!")
def sendAngles(new_a1,new_a2):
global a1,a2
try:
#print("a1:",a1,"a2:",a2)
a1 = new_a1
a2 = new_a2
sendCommand(b'JJAM',int(a1*100),int(a2*100))
Rtext_a1.set('{:.2f}'.format(a1))
Rtext_a2.set('{:.2f}'.format(a2))
except:
print("Error!")
def sendAngles2(new_a1,new_a2,s=0.5):
global a1,a2
try:
#print("a1:",a1,"a2:",a2)
a1 = new_a1
a2 = new_a2
sendCommand(b'JJAM',int(a1*100),int(a2*100))
time.sleep(s)
text_a1.set('{:.2f}'.format(a1))
text_a2.set('{:.2f}'.format(a2))
except:
print("Error!")
def sendAnglesKey(event=None):
try:
sendAngles(float(angle1.get()),float(angle2.get()))
print("a1:",a1,"a2:",a2)
except:
print("Error!")
def leftKey(event=None):
sendAngles(a1-1,a2)
def rightKey(event=None):
#sendAngles(a1+4,a2)
#time.sleep(0.1)
#sendAngles(a1-3,a2)
sendAngles(a1+1,a2)
def upKey(event=None):
sendAngles(a1,a2+1)
def downKey(event=None):
sendAngles(a1,a2-1)
def CleftKey(event=None):
sendAngles(a1-0.02,a2)
def CrightKey(event=None):
sendAngles(a1+0.02,a2)
def CupKey(event=None):
sendAngles(a1,a2+0.02)
def CdownKey(event=None):
sendAngles(a1,a2-0.02)
def SleftKey(event=None):
sendAngles(a1-4,a2)
def SrightKey(event=None):
sendAngles(a1+4,a2)
def SupKey(event=None):
sendAngles(a1,a2+4)
def SdownKey(event=None):
sendAngles(a1,a2-4)
################# MAIN ###########################
# platform angles
a1=0
a2=0
# Try to detect a GAMEPAD
try:
pygame.quit()
pygame.init()
pad_count = pygame.joystick.get_count()
if pad_count > 0:
myPAD = pygame.joystick.Joystick(0)
myPAD.init()
gamepad_ready = True
if "XBOX" in myPAD.get_name():
XBOX_Pad = True
else:
XBOX_Pad = False
print("GAMEPAD detected ", myPAD.get_name())
else:
gamepad_ready = False
print("GAMEPAD NOT detected")
except:
gamepad_ready = False
master = tk.Tk()
master.title("JJROBOTS 2Axis Laser pointer control app")
master.bind('<Return>', sendAnglesKey)
master.bind('<Left>', leftKey)
master.bind('<Right>', rightKey)
master.bind('<Up>', upKey)
master.bind('<Down>', downKey)
master.bind('<Control-Left>', CleftKey)
master.bind('<Control-Right>', CrightKey)
master.bind('<Control-Up>', CupKey)
master.bind('<Control-Down>', CdownKey)
master.bind('<Shift-Left>', SleftKey)
master.bind('<Shift-Right>', SrightKey)
master.bind('<Shift-Up>', SupKey)
master.bind('<Shift-Down>', SdownKey)
if SERIAL_PORT:
try:
ser = serial.Serial(COM_PORT, COM_BAUD, timeout=1)
tk.Label(master,text="Connected to COM port...").grid(row=0)
except:
print("Could not connect to serial port...",COM_PORT)
tk.Label(master,text="Could not connect to serial port...").grid(row=0)
else:
# WIFI conection
try:
print("Opening socket...")
LaserPointerSock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
LaserPointerSock.sendto(b'JJAH0000000000000000',(IP,PORT))
print("Connected to Laser Pointer via Wifi... ")
tk.Label(master,text="Connected to Robot via Wifi...").grid(row=0)
except:
print("! Could not connect to laser pointer!. Check you are connected to the robot Wifi network (JJROBOTS_xx)")
tk.Label(master,text="Could not connect to robot wifi").grid(row=0)
tk.Label(master,text="Coordenadas Stellarium:").grid(row=1)
tk.Label(master,text="Az/Alt:").grid(row=2)
coords = tk.Entry(master)
coords.config(width=30)
coords.grid(row=2, column=1,padx=10)
tk.Button(master,text=' Go! ',
command=send_position).grid(row=3,column=0,
sticky=tk.W,padx=20,pady=12)
tk.Label(master,text="Angle1:").grid(row=4)
tk.Label(master,text="Angle2:").grid(row=5)
text_a1 = tk.StringVar()
text_a1.set("0")
angle1 = tk.Entry(master,textvariable=text_a1)
angle1.grid(row=4, column=1,padx=10)
text_a2 = tk.StringVar()
angle2 = tk.Entry(master,textvariable=text_a2)
text_a2.set("0")
angle2.grid(row=5, column=1,padx=10)
tk.Button(master,text=' Go! ',
command=sendAnglesKey).grid(
row=6,column=0,sticky=tk.W,padx=20,pady=12)
separator = ttk.Separator(master, orient='horizontal')
separator.grid(row=7,column=0,columnspan=4, ipadx=200)
tk.Label(master,text="Robot angle1:").grid(row=8)
tk.Label(master,text="Robot angle2:").grid(row=9)
Rtext_a1 = tk.StringVar()
Rtext_a1.set("0")
Rangle1 = tk.Label(master,textvariable=Rtext_a1)
Rangle1.grid(row=8, column=1,padx=0)
Rtext_a2 = tk.StringVar()
Rangle2 = tk.Label(master,textvariable=Rtext_a2)
Rtext_a2.set("0")
Rangle2.grid(row=9, column=1,padx=0,pady=10)
# Reduce accelerations and speed
##sendCommand(b'JJAS',10,0,25,0) # 10% speed, 25% accel
##sendAngles(3,0)
##time.sleep(1.0)
##sendAngles(3,3)
##time.sleep(1.0)
##sendAngles(0,3)
##time.sleep(1.0)
##sendAngles(0,0)
##time.sleep(1.0)
sendCommand(b'JJAS',50,0,70,0) # 50% speed, 70% accel
#########################################
### CUSTOM MAIN LOOP
#########################################
running = True
X_gp_step = 0
Y_gp_step = 0
old_time = time.time()
while running:
if gamepad_ready:
for event in pygame.event.get():
if event.type == JOYAXISMOTION:
#axes = myPAD.get_numaxes()
X_gp_step = round(myPAD.get_axis(2),2)
Y_gp_step = round(myPAD.get_axis(3),2)
if abs(X_gp_step)<0.2:
X_gp_step=0
if abs(Y_gp_step)<0.2:
Y_gp_step=0
#sendAngles(a1+X_gp_step,a2+Y_gp_step)
#print("JOY",X_gp_step,Y_gp_step)
if event.type == JOYBUTTONDOWN:
print("Joystick button!",myPAD.get_button(0),myPAD.get_button(1),myPAD.get_button(2),myPAD.get_button(3))
# Send gamepad command at 10Hz
if (time.time()-old_time)>0.02:
old_time = time.time()
if (X_gp_step!=0 or Y_gp_step!=0):
#print("joy command")
# Non linear joystick adjust
if X_gp_step<0:
a1_step = -X_gp_step*X_gp_step*3
else:
a1_step = X_gp_step*X_gp_step*3
if Y_gp_step<0:
a2_step = -Y_gp_step*Y_gp_step*3
else:
a2_step = Y_gp_step*Y_gp_step*3
sendAngles(a1+a1_step,a2+a2_step)
master.update_idletasks()
master.update()
try:
ser.close()
except:
print("Could not close serial port...")
print("End...")