181 lines
6.8 KiB
Python
181 lines
6.8 KiB
Python
|
import math
|
||
|
|
import sys
|
||
|
|
import serial
|
||
|
|
import time
|
||
|
|
from struct import pack
|
||
|
|
|
||
|
|
# Transforming x, y, z to r, a, h
|
||
|
|
def xyz2arh( x, y, z ):
|
||
|
|
r = int(round(math.sqrt(pow(x, 2) + pow(y, 2))))
|
||
|
|
a = int(round(math.degrees(math.atan2(y, x))))
|
||
|
|
# atan2 gives negative results for -y --> subtract it from 360
|
||
|
|
if a < 0:
|
||
|
|
a = 360 + a
|
||
|
|
print a
|
||
|
|
h = z
|
||
|
|
return [a, r, h]
|
||
|
|
|
||
|
|
def xyz2a( x, y ):
|
||
|
|
a = int(round(math.degrees(math.atan2(y, x))))
|
||
|
|
# atan2 gives negative results for -y --> subtract it from 360
|
||
|
|
if a < 0:
|
||
|
|
a = 360 + a
|
||
|
|
return a
|
||
|
|
|
||
|
|
def xyz2r( x, y ):
|
||
|
|
r = int(round(math.sqrt(pow(x, 2) + pow(y, 2))))
|
||
|
|
return r
|
||
|
|
|
||
|
|
def xyz2h( z ):
|
||
|
|
h = z
|
||
|
|
return h
|
||
|
|
|
||
|
|
def sendBin( dec , ser):
|
||
|
|
print ("SENT POSITION: " + str(dec))
|
||
|
|
x = pack('i', dec)
|
||
|
|
ser.write(x[1])
|
||
|
|
ser.write(x[0])
|
||
|
|
|
||
|
|
def sendPositions( pos , ser):
|
||
|
|
sendBin(pos[0], ser)
|
||
|
|
sendBin(pos[1], ser)
|
||
|
|
sendBin(pos[2], ser)
|
||
|
|
|
||
|
|
# Calculate the shelf-number (1. digit of track)
|
||
|
|
def shelfNr( track ):
|
||
|
|
return int(track/10)
|
||
|
|
|
||
|
|
# Calculate the track-number (2. digit of track)
|
||
|
|
def trackNr( track ):
|
||
|
|
return track % 10
|
||
|
|
|
||
|
|
# Create and send instructions to move a ball from given x, y to track
|
||
|
|
def create_instr( x, y, track):
|
||
|
|
ser = serial.Serial(port='/dev/ttyACM0', # Change name if necessary. Find out name with ls /dev/tty* (1x without, 1x with USB connected)
|
||
|
|
baudrate=9600,
|
||
|
|
parity=serial.PARITY_NONE,
|
||
|
|
stopbits=serial.STOPBITS_ONE,
|
||
|
|
bytesize=serial.EIGHTBITS,
|
||
|
|
timeout=1)
|
||
|
|
time.sleep(1) # TODO DELETE Arduino resets, so wait for some time
|
||
|
|
|
||
|
|
signature = chr(0b01010010) # from raspberry
|
||
|
|
priority = chr(0b00000000) # add to end of queue
|
||
|
|
moveJ = chr(0b00000001) # binary 1
|
||
|
|
moveL = chr(0b00000010) # binary 2
|
||
|
|
moveL = moveJ
|
||
|
|
moveA = chr(0b00000100) # binary 4
|
||
|
|
pause = chr(0b00000111) # binary 7
|
||
|
|
dontChange = 32767
|
||
|
|
eot = chr(0b11111110) + chr(0b11111110) # end of transmission
|
||
|
|
magOn = chr(0b00000001)
|
||
|
|
magOff = chr(0b00000000)
|
||
|
|
|
||
|
|
heights = (80, 258, 187, 56) # heights of shelfs in mm (1. = Vorposition-Hoehe; 2. = Unterstes Brett Hoehe)
|
||
|
|
angles = (90, 63, 55, 47, 40, 32, 0) # angles of tracks in degrees + 0/6 = prePos
|
||
|
|
trackPosR = (170, 160, 173, 170) # radius of final pos in track in mm (2. = Oberste)
|
||
|
|
trackPrePosR = (trackPosR[0]-10, trackPosR[1]-10, trackPosR[2]-10, trackPosR[3]-10) # radius of pre pos of track
|
||
|
|
trackPreShelfR = trackPrePosR # radius of pre pos of shelf
|
||
|
|
shelfPrePosR = 155 # save bottom-position
|
||
|
|
shelfPrePosHOffset = 5
|
||
|
|
ballPosPreZOffset = 40 # height of pre pos of ballpos in mm (above the ball)
|
||
|
|
ballPosFinH = 10
|
||
|
|
magnetPauseTime = 1000
|
||
|
|
speedSlow = 20
|
||
|
|
speedFast = 50
|
||
|
|
|
||
|
|
higherOffsetBalls = [[-17.48, -59.11], # 34
|
||
|
|
[-54.32, -72.05], # 39
|
||
|
|
[-24.68, -85.12], # 40
|
||
|
|
[-58.33, -109.74]] # 56
|
||
|
|
higherhigherOffsetBalls = [[-131.86, -11.8]] # 53
|
||
|
|
|
||
|
|
# Create header
|
||
|
|
header = signature + priority
|
||
|
|
|
||
|
|
# Transform the given arguments so the program can work with them
|
||
|
|
x, y = y, x # change x and y to suit robot coordinate system
|
||
|
|
x = float(x)
|
||
|
|
y = float(y)
|
||
|
|
track = int(track)
|
||
|
|
|
||
|
|
#for i in range(len(higherOffsetBalls)):
|
||
|
|
# if x == higherOffsetBalls[i][1] and y == higherOffsetBalls[i][0]:
|
||
|
|
# ballPosFinH = 13
|
||
|
|
# break
|
||
|
|
|
||
|
|
#for i in range(len(higherhigherOffsetBalls)):
|
||
|
|
# if x == higherhigherOffsetBalls[i][1] and y == higherhigherOffsetBalls[i][0]:
|
||
|
|
# ballPosFinH = 15
|
||
|
|
# break
|
||
|
|
|
||
|
|
actShelfNr = shelfNr(track)
|
||
|
|
instr =[[moveJ, xyz2a(x, y), xyz2r(x, y), xyz2h(ballPosPreZOffset), speedFast], # To Ball-Pre-Position
|
||
|
|
[moveL, xyz2a(x, y), xyz2r(x, y), xyz2h(ballPosFinH), speedSlow], # To Ball-Position
|
||
|
|
[moveA, dontChange, dontChange, dontChange, magOn], # Switch on Magnet
|
||
|
|
[pause, magnetPauseTime],
|
||
|
|
[moveL, xyz2a(x, y), xyz2r(x, y), xyz2h(ballPosPreZOffset), speedSlow], # To Ball-Pre-Position again
|
||
|
|
[moveJ, angles[0], shelfPrePosR, heights[0], speedFast], # To Shelf-Pre-Pre-Position
|
||
|
|
# TODO Left or right pre-position?
|
||
|
|
[moveJ, angles[0], trackPreShelfR[actShelfNr], heights[actShelfNr]+shelfPrePosHOffset, speedFast], # To Shelf-Pre-Position
|
||
|
|
[moveJ, angles[trackNr(track)], trackPrePosR[actShelfNr], heights[actShelfNr]+shelfPrePosHOffset, speedSlow], # To Track-Pre-Position
|
||
|
|
[moveL, angles[trackNr(track)], trackPosR[actShelfNr], heights[actShelfNr], speedSlow], # To Track-Goal-Position
|
||
|
|
[moveA, dontChange, dontChange, dontChange, magOff], # Switch off Magnet
|
||
|
|
[pause, magnetPauseTime],
|
||
|
|
[moveL, angles[trackNr(track)], trackPrePosR[actShelfNr], heights[actShelfNr]+shelfPrePosHOffset, speedSlow], # To Track-Pre-Position
|
||
|
|
[moveJ, angles[0], trackPreShelfR[actShelfNr], heights[actShelfNr]+shelfPrePosHOffset, speedSlow], # To Shelf-Pre-Position
|
||
|
|
[moveJ, angles[0], shelfPrePosR, heights[0], speedFast] # To Shelf-Pre-Pre-Position
|
||
|
|
]
|
||
|
|
|
||
|
|
# MoveJ, MoveL: A, R, H
|
||
|
|
# MoveA: R1, P, R2
|
||
|
|
|
||
|
|
# TODO: Is robot in safe mode?
|
||
|
|
|
||
|
|
# Send instructions
|
||
|
|
#time.sleep(5)
|
||
|
|
|
||
|
|
for i in range(len(instr)):
|
||
|
|
ser.write(header)
|
||
|
|
ser.write(instr[i][0]) # send move-type
|
||
|
|
|
||
|
|
if instr[i][0] == pause:
|
||
|
|
sendBin(instr[i][1], ser)
|
||
|
|
if instr[i][0] == moveJ or instr[i][0] == moveL:
|
||
|
|
sendBin(instr[i][1], ser)
|
||
|
|
sendBin(instr[i][2], ser)
|
||
|
|
sendBin(instr[i][3], ser)
|
||
|
|
#sendBin(instr[i][4], ser) # send speed
|
||
|
|
if instr[i][0] == moveA:
|
||
|
|
sendBin(instr[i][1], ser)
|
||
|
|
sendBin(instr[i][2], ser)
|
||
|
|
sendBin(instr[i][3], ser)
|
||
|
|
ser.write(instr[i][4]) # send magOn / magOff
|
||
|
|
|
||
|
|
ser.write(eot)
|
||
|
|
|
||
|
|
# DEBUG
|
||
|
|
print("SENT INSTR" + str(i))
|
||
|
|
time.sleep(1.0)
|
||
|
|
|
||
|
|
# Debug: Send pause
|
||
|
|
# ser.write(header)
|
||
|
|
# ser.write(pause)
|
||
|
|
# sendBin(1500,ser)
|
||
|
|
# ser.write(eot)
|
||
|
|
|
||
|
|
while ser.inWaiting():
|
||
|
|
print(ser.readline())
|
||
|
|
|
||
|
|
while 1 == 1:
|
||
|
|
print(ser.readline())
|
||
|
|
|
||
|
|
ser.close() # close serial connection
|
||
|
|
# Only for debugging
|
||
|
|
#file = open("testfile.txt", "w")
|
||
|
|
#file.write(str(x) + str(y) + str(track));
|
||
|
|
|
||
|
|
# Get the information from php with arguments -> if the file create_instructions is called directly with arguments
|
||
|
|
if __name__ == "__main__":
|
||
|
|
create_instr(sys.argv[1], sys.argv[2], sys.argv[3])
|