cos_verticalPAngle = m.sqrt( m.pow(abs_deltaX, 2) + m.pow(abs_deltaX * tanY, 2)) / pixelPen
# Calibrate the right and left direction in terms of the North/South poles of the Earth
# Need a better automated approach for calibration
if(X_Degree < 140 or X_Degree > 320):
deltaX = - pixelPen * cos_verticalPAngle * m.cos(m.radians(Y_Degree))
else:
deltaX = pixelPen * cos_verticalPAngle * m.cos(m.radians(Y_Degree))
deltaY = pixelPen * cos_verticalPAngle * m.sin(m.radians(-Y_Degree))
################################################Main Thread Below##################################
# Start calibration for pixelsPerMetric
pixelsPerMetric = calibrator(42)
# Initialize reading from Arduino
# arduinoData = serial.Serial('com3', 115200)
telnet = TelnetWorkers('192.168.0.103', '23')
# define range of purple color in HSV
lower_blue = np.array([85,50,150])
upper_blue = np.array([110,200,255])
# Create empty deque array to store object locations for trajectory drawing
# Note, the upper right corner is the Origin
points_draw_original = deque(maxlen=4)
points_draw_cal = deque(maxlen=4)
# Create empty array list to store object locations for plotting
import pyvjoy
import time
from pyfirmata import Arduino, util
from scipy.interpolate import interp1d
import calibration
board = Arduino('COM5')
ster = board.get_pin('a:0:p')
gas = board.get_pin('a:1:i')
brake = board.get_pin('a:2:i')
iter = util.Iterator(board)
min_gas, min_brake = calibration.calibrator(board, iter, gas, brake)
interpol_gas = interp1d([min_gas, 1], [0, 16384*2])
interpol_brake = interp1d([min_brake, 1], [0, 16384*2])
interpol_steer = interp1d([0, 1], [0, 16384*2])
j = pyvjoy.VJoyDevice(1)
board.pass_time(0.2)
while True:
if gas.read() >= min_gas and brake.read() >= min_brake:
# if gas.read() >= 0.52:
# if brake.read() >= 0.52:
gas_val = int(interpol_gas(gas.read()))
brake_val = int(interpol_brake(brake.read()))
j.data.wAxisZRot = brake_val
j.data.wAxisZ = gas_val
print('Gas: {}, Brake: {}'.format(gas_val, brake_val), end = '\r')
cv2.imshow("new", new)
cv2.imshow("newpoints", points_n)
cv2.waitKey()
sortedCal = sorted(pointsList_c, key=lambda point: point.y, reverse=False)
sortedNew = sorted(pointsList_n, key=lambda point: point.y, reverse=False)
longer = max(len(sortedNew), len(sortedCal))
for i in range(0, longer):
if len(sortedNew) > i:
sortedNew[i].x = 160 - sortedNew[i].x
sortedNew[i].y = 120 - sortedNew[i].y
cal = calibrator()
print(type(cal))
cal.calibrate(sortedCal)
getDepth(pointsList_c, pointsList_n)
for i in pointsList_c:
print(i.X, i.Y, i.Z)
print("----------------------------------------------------")
print("3D:")
for i in pointsList_n:
print(i.X, i.Y, i.Z)
print(counter)
cv2.waitKey()
if (not all(x in chambers for x in [0, 1])) and (not all(x in chambers for x in [2, 3])): skip=True
else:
if 3 not in chambers or len(chambers)<3: skip=True
for chamber in chambers:
if len(selectedhits[selectedhits.chamber==chamber].layer.unique()) < 3: skip=True
if len(selectedhits[selectedhits.chamber==chamber].chamber.tolist()) > 7: skip=True
return skip
# Analyze the input file
with open(args.input,"r") as csvfile:
# the plotter
plotter = plotter()
# the calibrator
calibrator = calibrator()
# the residual estimator
estimator = residuals_estimator()
# read the preporcessed csv file
reader=csv.reader(csvfile, delimiter=' ', quoting=csv.QUOTE_NONNUMERIC)
counter=0
for event in reader:
counter+=1
if counter%100==0: print (counter, "lines processed")
if args.num_events>0 and counter>=args.num_events: break
eventID=event[0]
nhits=event[1]