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])