144 lines
4.3 KiB
Python
144 lines
4.3 KiB
Python
'''
|
|
Created on Nov 11, 2012
|
|
|
|
@author: attero
|
|
'''
|
|
import serial
|
|
import glob
|
|
import threading
|
|
import math
|
|
|
|
def mutex(m):
|
|
def wrapper(self, *args, **kwargs):
|
|
self.s.acquire()
|
|
m()
|
|
self.s.release()
|
|
return wrapper
|
|
|
|
class CannonController :
|
|
def __init__(self):
|
|
self.s = threading.Semaphore()
|
|
print("cannon controller started")
|
|
self.position = [0, 0]
|
|
self.fired = False
|
|
self.laser = False
|
|
ports = glob.glob("/dev/ttyACM*")
|
|
|
|
if(len(ports) == 1):
|
|
try:
|
|
self.ser = serial.Serial(ports[0], 115200, timeout=1)
|
|
except serial.SerialException:
|
|
raise Exception("Could not open serial connction (busy?)")
|
|
self.connected_to_serial = True
|
|
else:
|
|
self.ser = False
|
|
self.connected_to_serial = False
|
|
if len(ports) > 1 :
|
|
raise Exception("To many devices to handle")
|
|
elif len(ports) == 0 :
|
|
print Exception("No device is connected")
|
|
|
|
def set_position(self, point):
|
|
self.position = point
|
|
|
|
def translate_yaw(self, yaw):
|
|
return yaw
|
|
|
|
def fire(self):
|
|
self.fired = True
|
|
|
|
def enable_laser(self):
|
|
self.laser = True
|
|
|
|
def disable_laser(self):
|
|
self.laser = False
|
|
|
|
def move_left(self, change):
|
|
if(self.position[0] >= change):
|
|
self.position[0] -= change
|
|
|
|
def move_right(self, change):
|
|
if(self.position[0] < 255 - change):
|
|
self.position[0] += change
|
|
|
|
def move_up(self, change):
|
|
if(self.position[1] < 255 - change):
|
|
self.position[1] += change
|
|
|
|
def move_down(self, change):
|
|
if(self.position[1] >= change):
|
|
self.position[1] -= change
|
|
|
|
def check_validity_of_data(self):
|
|
if(abs(self.position[0]) > 255):
|
|
return False
|
|
if (abs(self.position[1]) > 255):
|
|
return False
|
|
return True
|
|
|
|
def get_data(self):
|
|
if self.check_validity_of_data():
|
|
data = []
|
|
data.extend(self.position)
|
|
if(self.fired):
|
|
data.append(255)
|
|
self.fired = False
|
|
else:
|
|
data.append(0)
|
|
if(self.laser):
|
|
data.append(255)
|
|
else:
|
|
data.append(0)
|
|
return data
|
|
else:
|
|
return [0, 0, 0, 0]
|
|
|
|
def get_data_to_send(self):
|
|
data = self.get_data()
|
|
data[0] = self.translate_yaw(data[0])
|
|
return [chr(x) for x in data]
|
|
|
|
def send_data(self):
|
|
if(self.connected_to_serial):
|
|
data = self.get_data_to_send()
|
|
self.ser.write('a')
|
|
self.ser.write(data[0])
|
|
print "wrote %i" % ord(data[0])
|
|
self.ser.write('b')
|
|
self.ser.write(data[1])
|
|
self.ser.write('c')
|
|
self.ser.write(data[2])
|
|
self.ser.write('d')
|
|
self.ser.write(data[3])
|
|
else:
|
|
print "Cannot send data, not connected..."
|
|
|
|
class HackWAWCannonController(CannonController):
|
|
AXIS_DISTANCE = 20
|
|
ARM_LENGTH = 1
|
|
def translate_yaw(self, yaw):
|
|
return yaw
|
|
try:
|
|
turret_angle = (yaw * math.pi) / 256 - (math.pi / 2)
|
|
servo_angle = math.asin((self.ARM_LENGTH * math.tan(turret_angle)) / (self.AXIS_DISTANCE - self.ARM_LENGTH))
|
|
except:
|
|
return 0 if yaw == 0 else 255
|
|
return int(((servo_angle + (math.pi / 2)) * 256) / math.pi)
|
|
|
|
class Gunpoint :
|
|
def __init__(self, point1, point2, point3):
|
|
self.vertical_angle = point3[1] - point1[1]
|
|
self.horizontal_angle = point2[0] - point1[0]
|
|
self.beginnig_horizontal = point1[0]
|
|
self.beginnig_vertical = point1[1]
|
|
|
|
def calibrate(self, point1, point2, point3):
|
|
self.vertical_angle = point3[1] - point1[1]
|
|
self.horizontal_angle = point2[0] - point1[0]
|
|
self.beginnig_horizontal = point1[0]
|
|
self.beginnig_vertical = point1[1]
|
|
|
|
def aim(self, point):
|
|
horizontal = math.ceil(self.beginnig_horizontal + self.horizontal_angle*point[0])
|
|
vertical = math.ceil(self.beginnig_vertical + self.vertical_angle*point[1])
|
|
return [horizontal, vertical] |