python-lib/keyboardControl.py at master · Humhu/python-lib · GitHub

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
importtime, sys
fromstructimport*
fromserialimport*
fromxbeeimportXBee
fromlibimportnetwork_const
fromlib.dictionariesimport*
fromlib.command_interfaceimportCommandInterface
fromlib.network_coordinatorimportNetworkCoordinator

THRUST_INCREMENT=0.05
YAW_INCREMENT=-0.5
ELEVATOR_INCREMENT=0.2

THRUST_UPPER_LIMIT=1.0
THRUST_LOWER_LIMIT=0.0
YAW_UPPER_LIMIT=1.0
YAW_LOWER_LIMIT=-1.0
ELEVATOR_UPPER_LIMIT=1.0
ELEVATOR_LOWER_LIMIT=-1.0

deftxCallback(dest, packet):
globalxb
xb.tx(dest_addr=dest, data=packet)

defloop():

DEFAULT_COM_PORT='COM7'
DEFAULT_BAUD_RATE=57600
DEFAULT_ADDRESS='\x10\x21'
DEFAULT_PAN='\x10\x01'


iflen(sys.argv) ==1:
com=DEFAULT_COM_PORT
baud=DEFAULT_BAUD_RATE
addr=DEFAULT_ADDRESS
eliflen(sys.argv) ==4:
com=sys.argv[1]
baud=int(sys.argv[2])
addr=pack('>H', int(sys.argv[3], 16))
else:
print"Wrong number of arguments. Must be: COM BAUD ADDR"
sys.exit(1)

coord=CommandInterface(addr, txCallback)
coord.enableDebug()

ser=Serial(port=com, baudrate=baud)
xb=XBee(ser)
print"Setting PAN ID to "+hex(DEFAULT_PAN)
xb.at(command='ID', parameter=pack('>H', DEFAULT_PAN))

thrust=0.0
yaw=0.0
elevator=0.0

whileTrue:

try:

c=msvcrt.getch()

ifc=='w':
thrust=thrust+THRUST_INCREMENT
elifc=='a':
yaw=yaw+YAW_INCREMENT
elifc=='s':
thrust=thrust-THRUST_INCREMENT
elifc=='d':
yaw=yaw-YAW_INCREMENT
elifc=='r':
elevator=elevator+ELEVATOR_INCREMENT
elifc=='f':
elevator=elevator-ELEVATOR_INCREMENT
elifc=='q':
thrust=0.0
yaw=0.0
elevator=0.0
elifc=='e':
break
elifc=='t':
coord.setRegulatorMode(RegulatorStates['Remote Control'])

ifthrust>THRUST_UPPER_LIMIT:
thrust=THRUST_UPPER_LIMIT
elifthrust<THRUST_LOWER_LIMIT:
thrust=THRUST_LOWER_LIMIT

ifyaw>YAW_UPPER_LIMIT:
yaw=YAW_UPPER_LIMIT
elifyaw<YAW_LOWER_LIMIT:
yaw=YAW_LOWER_LIMIT

ifelevator>ELEVATOR_UPPER_LIMIT:
elevator=ELEVATOR_UPPER_LIMIT
elifelevator<ELEVATOR_LOWER_LIMIT:
elevator=ELEVATOR_LOWER_LIMIT

coord.setRemoteControlValues(thrust, yaw, elevator)

except:

print"Exception: ", sys.exc_info()[0]
break

xb.halt()
ser.close()

if__name__=='__main__':

try:
loop()

except:
pass