portx = "/dev/ttyUSB0"
bps = 115200
res = None
import threading
from multiprocessing import Value,Lock,Manager
import multiprocessing
import serial
import time
import struct
a=[0,0,0,0]
begin=b'\xFF\xFF'
end=b'\xFE'
global LL
value1 = 0.0
serial = serial.Serial(portx, int(bps), timeout=0.2, parity=serial.PARITY_NONE, stopbits=1)
time.sleep(1.5)
import os, struct, array
#serial_connection
class JoyStick:
def __init__(self):
print('avaliable devices')
for fn in os.listdir('/dev/input'):
if fn.startswith('js'):
print('/dev/input/%s' % fn)
self.fn = '/dev/input/js0'
self.x_axis = 0;
def open(self):
self.jsdev = open(self.fn, 'rb')
def read(self):
self.evbuf = self.jsdev.read(8)
return struct.unpack('IhBB', self.evbuf)
def type(self,type_):
if type_ & 0x01:
return "button";
if type_ & 0x02:
return "axis";
def button_state(self):
return 1;
def get_x_axis(self):
time, value, type_, number = struct.unpack('IhBB', self.evbuf)
if number == 1:
fvalue = value / 32767
return fvalue;
js=JoyStick()
def joystick_thread(num,model,sudo,ki):
js.open()
while True:
time, value, type_, number = js.read()
if js.type(type_) == "button":
print("button:{} state: {}".format(number, value))
if number == 4 and value == 1:
model = 1
print("start")
#logger.start()
if number == 5 and value == 1:
model = 0
print("end")
#logger.stop()
if number == 6 and value == 1:
sudo+= 0.001
print("KP+",sudo)
if number == 7 and value == 1:
if sudo > 0 :
sudo-= 0.001
print("KP-",sudo)
if number == 0 and value == 1:
ki+= 0.00001
print("KI+",ki)
if number == 2 and value == 1:
if ki > 0 :
ki-= 0.00001
print("KI-",ki)
if number == 8 and value == 1:
model = 2
print("break")
if number == 3 and value == 1:
num+= 0.00001
print("KD+",num)
if number == 1 and value == 1:
if num>0:
num -= 0.00001
print("KD-",num)
if number ==12 and value == 1:
model = 3 #按下mode 拍照
if number == 9 and value ==1:
model = 4
value=struct.pack('f',num)
model1=model.to_bytes(1,"little")
sudo1=struct.pack('f',sudo)
ki2 = struct.pack('f',ki)
value1=begin+model1+value+sudo1+ki2+end#2+1+4+4+1
serial.write(value1)
if model == 2:
break
if __name__ == "__main__":
num = 0.00145 # KD
model= 0 # moshi
sudo = 0.021 #KP
ki = 0.000000 #KI
joystick_thread(num,model,sudo,ki)單線程接收與發送
