.PROGRAM alt.alt() ;-------------------------------------------------------- ; Input params: None ; Output params: None ; Description: ; Revision History ; 9/19/01 - wbg - started with simple shell ;-------------------------------------------------------- ; program to run at task level 0 GLOBAL tasknum_alt, tasknum_tcp, tasknum_move GLOBAL step[] GLOBAL num_axes GLOBAL alt_loop, stop_move_loop GLOBAL data_ary[,], index LOCAL i ; Intialize values tasknum_alt = 0 tasknum_tcp = 1 tasknum_move = 2 num_axes = 4 FOR i = 0 TO (num_axes-1) step[i] = 0 END alt_loop = TRUE ok2alter = FALSE altindex = 1 ; start up other tasks ; this task is on task 0 TYPE "%% alt.alt (task 0) initializing, starting other tasks" DETACH () ; release control of the robot EXECUTE tasknum_tcp alt.tcp(), -1 EXECUTE tasknum_move alt.move(), -1 TYPE "%% alt.alt (task 0 ) - waiting for a set.event to start alter loop" WAIT.EVENT 1 ; wait for set event from alt.move process WHILE alt_loop DO WAIT ; have the alter command execute at the ; start of the next servo cycle IF ok2alter == TRUE THEN ; TYPE "%% .alt - step = ", step[0], " ", step[1], " ", step[2], ", ", step[3] data_ary[altindex,1] = qnx_time data_ary[altindex,2] = step[0] data_ary[altindex,3] = step[1] data_ary[altindex,4] = step[2] data_ary[altindex,5] = step[3] data_ary[altindex,6] = des_ws[0] data_ary[altindex,7] = 0 altindex = altindex+1 ALTER () step[0], step[1], step[2], 0, 0, step[3] ok2alter = FALSE END END TYPE "%% alt.alt (task 0) - terminating" HALT .END .PROGRAM alt.move() ;-------------------------------------------------------- ; Input params: None ; Output params: None ; Description: ; Revision History ; 9/19/01 - wbg - started with simple shell ;-------------------------------------------------------- ; program to run at task level 2 GLOBAL tasknum_alt, tasknum_tcp, tasknum_move GLOBAL alt_loop, stop_move_loop GLOBAL xorigin, yorigin, zorigin GLOBAL yaworigin, pitchorigin, rollorigin GLOBAL soft_size[], hard_size[], soft_range[] GLOBAL num_axes AUTO home AUTO st2_val2_ms LOCAL i ; Intialize values xorigin = 493.5 yorigin = 0 zorigin = 679.25 yaworigin = 0 pitchorigin = 180 rollorigin = 0 hard_size[0] = 158 ; x-axis hard_size[1] = 454 ; y-axis hard_size[2] = (775-583.5)/2 ; z-axis = 95.75 hard_size[3] = 180 ; yaw-axis soft_range[0] = 50 soft_range[1] = 50 soft_range[2] = 25 soft_range[3] = 21 FOR i = 0 TO (num_axes-1) soft_size[i] = hard_size[i]-soft_range[i] END st2_val2_ms = 2 ;see STATE description stop_move_loop = FALSE ; attach the robot and enable the alter command ATTACH () ALTON () 3 ; send the robot home TYPE "%% alt.move (task 2) - sending the robot home" SPEED 20, 20 ACCEL 20, 20 CPON ALWAYS SET home = TRANS(xorigin,yorigin,zorigin,yaworigin,pitchorigin,rollorigin) MOVES home DO ; do nothing until the robot has made it home UNTIL STATE(2) == st2_val2_ms TYPE "%% alt.move (task 2) - starting MOVES HERE loop" SPEED 80, 40 ALWAYS ACCEL 40, 20 MOVES HERE SET.EVENT tasknum_alt ; start the alter commands DO WAIT.EVENT , 1 ; wait one second UNTIL stop_move_loop ALTOFF alt_loop = FALSE TYPE "%% alt.move (task 2) - terminating" HALT .END .PROGRAM alt.tcp() ;-------------------------------------------------------- ; Input params: None ; Output params: None ; Description: This program is used in conjunction with ; the alt.XXX programs. This program ; will do the tcp/ip communication with artoo ; Revision History ; 9/19/01 - wbg - started with cforce shell ; removed much of the move stuff ;-------------------------------------------------------- GLOBAL tasknum_alt, tasknum_tcp, tasknum_move GLOBAL soft_size[], hard_size[], soft_range[] GLOBAL step[] GLOBAL alt_loop, stop_move_loop GLOBAL ok2alter GLOBAL data_ary[,] GLOBAL qnx_time GLOBAL index GLOBAL num_axes AUTO handle, lun, status, valid_comm, comm_loop, first_time AUTO keynum AUTO des_step[6] GLOBAL deltamax[], deltamax_temp[] GLOBAL des_ws[] AUTO stored[6] LOCAL i ; Intialize values write_data = FALSE lun = 7 ; wait_val = 0 ; causes the read operation to wait until it is complete comm_loop = TRUE first_time = TRUE ; sets the parameters of x,y & z based on the Adept world coordinates deltamax[0] = 12 ; x-axis, thus a max speed of about .75 m/s deltamax[1] = 12 ; y-axis deltamax[2] = 6 ; z-axis deltamax[3] = 10 ; yaw, deg thus a max speed of about 625 deg/sec FOR i = 0 TO (num_axes-1) stored[i] = 0 des_ws[i] = 0 END index = 1 ; Attach the TCP device (mode 4 means next available LUN) TYPE "%% alt.tcp (task 1) - setting up communcation" ATTACH (lun, 4) "TCP" status = IOSTAT(lun) ;Check status of ATTACH IF status < 0 THEN ; get the heck of out of dodge TYPE "Error from ATTACH:", $ERROR(status) GOTO 100 END ; Set up a socket with local port #2001 FOPEN (lun, 16) "/local_port 2001 /clients 5 /buffer_size 20480" status = IOSTAT(lun) ;check status of ATTACH IF status < 0 THEN ; get the heck out of dodge TYPE "Error from FOPEN", $ERROR(status) GOTO 100 END TYPE "%% alt.tcp (task 1) - starting comm loop" WHILE comm_loop DO ; main while loop that runs until: a TCP error occurs, or QNX ; tells the Adept to stop ; comm_loop makes controls the loop operation. ; tells QNX to send some data $out_str = "abc" WRITE (lun, handle) $out_str IF first_time == TRUE THEN ; when the socket connection is created the string sent ; is not the same as the data string READ (lun, handle, wait_val) $in.str TYPE "%% alt.tcp (task 1) - initial string received =", $in.str first_time = FALSE ELSE READ (lun, handle, wait_val) des_step[0], des_step[1], des_step[2], des_step[3], qnx_time, keynum END ; reads the data coming from QNX status = IOSTAT(lun) CASE status OF ; handles result of call to IOSTAT VALUE 1: ;Success ; Constrain values to workspace IF keynum == 1.234 THEN ; in this case, the data is mapped to the space, and the ; move is performed valid_comm = TRUE ELSE ; prevents any motion if there is some communication error valid_comm = FALSE IF keynum == 99 THEN ; 99 is the keynum value sent by QNX to cause Adept to ; exit nicely TYPE "%% alt.tcp (task 1) - adept received exit order from QNX. Exiting!" GOTO 100 ELSE TYPE "%% alt.tcp (task 1) - keynum failed, quiting" GOTO 100 END ; inner if END ; if/else IF valid_comm THEN ; step 1 add the current des step to the stored value ; step 2 check to see if stored value is > or < zero ; step 3 if > then take min of stored val and the deltamax ; if < then take max of stored val and the deltamax ; and set that value to the global value ; step 4 update the stored value by subtracting the new step FOR i = 0 TO (num_axes-1) stored[i] = stored[i]+des_step[i] IF stored[i] >= 0 THEN step[i] = MIN(deltamax[i],stored[i]) ELSE step[i] = MAX(-deltamax[i],stored[i]) END stored[i] = stored[i]-step[i] END IF write_data == TRUE THEN ; data_ary[index,1] = qnx_time ; data_ary[index,2] = des_y_step ; data_ary[index,3] = y_step ; data_ary[index,4] = stored_y ; data_ary[index,5] = des_x_step ; data_ary[index,6] = x_step ; data_ary[index,7] = stored_x index = index+1 END ok2alter = TRUE ; this prevents the last good value ; being altered over and over if there ; is a delay ELSE ; we might want to set this to the last value ; but for now set it to zero FOR i = 0 TO num_axes step[i] = 0 END END ; valid_comm if VALUE 100: ;New connection TYPE "%% alt.tcp (task 1) - new connection established, handle =", handle VALUE 101: ;Connection closed TYPE "%% alt.tcp (task 1) - connection closed, handle = ", handle TYPE "*******************************", /C2 FCMND (lun, 600) $INTB(handle) ; Deallocates handle status = IOSTAT(lun) IF status < 0 THEN TYPE "Error from FCMND: ", $ERROR(status) END GOTO 100 VALUE -526: ; No data received TYPE /C2, "No data received = -526", /C2 ANY ; some other error TYPE "Error during read: ", $ERROR(status) GOTO 100 END ; of case statement ; this wait is to synch up things WAIT END ; of while loop ;close all open client connections, and release the logical units 100 TYPE "%% alt.tcp - shutting down" IF write_data == TRUE THEN CALL file.save(data_ary, "test1.txt", index) END FCLOSE (lun) DETACH (lun) stop_move_loop = TRUE HALT .END .PROGRAM file.save(data_ary, $qnx_datafile, counter) ; ---------------------------------------- ; Abstract: stores data from calling program to a file ; Input params: data_ary - the array containing input and altered position ; values ; $qnx_datafile - the place to store the data ; counter - to know size of the array ; Output params: None ;-------------------------------------------------------- AUTO disk_lun ; LUN of disk. Adept assigns automatically AUTO from_qnx, internal, whilenum whilenum = 1 from_qnx = 1 ; array index of values from QNX internal = 2 ; array index of values as set by Adept ATTACH (disk_lun, 4) "DISK" ; mode = 4 forces Adept to assign ; correct LUN FOPENA (disk_lun, 0, 1) $qnx_datafile ; record length = 0, mode = 1 to cause file to only be accessed ; when a specific command is issued. FOPENA allows read-write-append. IF IOSTAT(disk_lun) < 0 THEN TYPE "Error opening file", /C1 HALT END ; TYPE "whilenum @ beginning = ", whilenum WHILE ((IOSTAT(disk_lun) > 0) AND (whilenum < counter)) DO ; loop while no errors occur, and while still within bounds of array ; write the from_qnx values WRITE (disk_lun) data_ary[whilenum,1], ", ", /S WRITE (disk_lun) data_ary[whilenum,2], ", ", /S WRITE (disk_lun) data_ary[whilenum,3], ", ", /S WRITE (disk_lun) data_ary[whilenum,4], ", ", /S WRITE (disk_lun) data_ary[whilenum,5], ", ", /S WRITE (disk_lun) data_ary[whilenum,6], ", ", /S WRITE (disk_lun) data_ary[whilenum,7] ; WRITE (disk_lun) data_ary[whilenum,from_qnx,1], ", ", /S ; WRITE (disk_lun) data_ary[whilenum,from_qnx,2], ", ", /S ; WRITE (disk_lun) data_ary[whilenum,from_qnx,3], ", ", data_ary[whilenum,from_qnx,4], ", ", /S ; WRITE (disk_lun) data_ary[counter,from_qnx,5], ", ", data_ary[counter,from_qnx,6] ; write the internal values ; WRITE (disk_lun) data_ary[whilenum,internal,1], ", ", data_ary[whilenum,internal,2], ", ", /S ; WRITE (disk_lun) data_ary[whilenum,internal,3], ", ", data_ary[whilenum,internal,4], ", ", /S ; WRITE (disk_lun) data_ary[whilenum,internal,5], ", ", data_ary[counter,internal,6] ; WRITE (disk_lun) "" ; force a space between records whilenum = whilenum+1 END ; clean up - fclose closes the file, detach detaches from the disk device FCLOSE (disk_lun) DETACH (disk_lun) RETURN ; that's all folx .END .LOCATIONS .END .REALS .END .DOUBLES .END .STRINGS .END