-
Notifications
You must be signed in to change notification settings - Fork 1
/
ctrls_adapted_Ubuntu.py
152 lines (126 loc) · 4.23 KB
/
ctrls_adapted_Ubuntu.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
#Python3
#Created for Mission Critial Drone Project for Ericsson
#Code to be run on Computer to connect to Drone
#Need to connect pygame compatiable controller. May need to change axis layout if new controller is used
#Use "ControllerFile.py" to test
#Austin Greisman
import pygame
import socket
import time
import subprocess
import os
import signal
#Inital Connection set up -> Drone static IP over Hai's PIC VPN
host = '10.8.0.2'
#Static port
port = 6970
#Variable used to remain in sync with the drone during data transimission
heartbeat = 0
s = socket.socket()
s.connect((host, port))
print("Connected to Drone...")
#If connection is lost for more than 5 seconds, connection dropped
s.settimeout(5)
pygame.init()
print("yes")
#Loop until the user clicks the close button.
done = False
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
# Initialize the joysticks
pygame.joystick.init()
#Variables
pitch = 0
roll = 0
yaw = 0
throttle = 0
#Calibration Settings
low_cal = 3807
mid_cal = 5246
max_cal = 6685
#python2 on Drone
#low_cal = 240
#mid_cal = 330
#max_cal = 420
Canny = 1
#Activated by pressing start button. However, needs to be calibrated again
def StartYourEngines(heartbeat, s):
if heartbeat:
print("Sending start sequence to drone...")
message = "%d,%d,%d,%d " % (3807, 3807, 3807, 3807) #Change!
print(message)
for i in range(10):
s.send(message.encode('utf-8'))
heartbeat = 0
s.recv(5)
print("Start sequence sent...")
# -------- Main Program Loop -----------
while done==False:
# EVENT PROCESSING STEP
for event in pygame.event.get(): # User did something
if event.type == pygame.QUIT: # If user clicked close
done=True # Flag that we are done so we exit this loop
#Count Joystick
joystick_count = pygame.joystick.get_count()
# For each joystick:
for i in range(joystick_count):
joystick = pygame.joystick.Joystick(i)
joystick.init()
# Usually axis run in pairs, up/down for one, and left/right for
# the other.
axes = joystick.get_numaxes()
for i in range( axes ):
axis = joystick.get_axis( i )
# Minimum jump value implemented to remove wobble
min_jump = 0.05
#CUSTOM FOR UBUNTU CONTROL
if i == 2 and abs(axis-yaw) > min_jump:
yaw = axis
if i == 3 and abs(axis-throttle) > min_jump:
throttle = (axis)*-1
if i == 0 and abs(axis-roll) > min_jump:
roll = axis
if i == 1 and abs(axis-pitch) > min_jump:
pitch = (axis) * -1
# Grabs value for start button on Logitech controller (For StartYourEngines())
engine_button = joystick.get_button(0)
hover_button = joystick.get_button(1)
# Hat switch. All or nothing for direction, not like joysticks.
# Value comes back in an array.
#hats = joystick.get_numhats()
#for i in range( hats ):
# hat = joystick.get_hat( i )
####Prepare to send, currently hardcoded to call settings - Custom
send_pitch = int(mid_cal + 1439*pitch)
send_yaw = int(mid_cal + 1439*yaw)
send_roll =int(mid_cal + 1439*roll)
send_throttle = int(mid_cal + 1439*throttle)
#python2 on Drone
'''send_pitch = int(mid_cal + 90*pitch) - 3
send_yaw = int(mid_cal + 90*yaw) - 2
send_roll =int(mid_cal + 90*roll) - 3
send_throttle = int(mid_cal + 90*throttle)'''
#####Send commands out
# To remain in sync with drone
try:
heartbeat = s.recv(5)
except:
print("Lost connection...")
break
# If start button(s) pressed
if engine_button:
StartYourEngines(heartbeat, s)
if hover_button:
print("AutoHover Engagged")
if heartbeat:
#print("Sending Commands...")
message = "%d,%d,%d,%d "%(send_roll, send_pitch, send_throttle, send_yaw)
#print(message)
s.send(message.encode('utf-8'))
#print("Message sent...")
time.sleep(0.01) #was 0.1
heartbeat = 0
# Close the window and quit.
# If you forget this line, the program will 'hang'
# on exit if running from IDLE.
pygame.quit ()