Hey all,
Still getting to grips with V+,
currently not having ANY luck with talking to any of the 3 global serial ports on the SIO board.
Whenever I try to attach using the code below I get *Device not ready*, I tried different SERIAL ports to no avail. Am I missing something basic?
Thanks!
Code
.PROGRAM ros.srv.fdb()
;
; ABSTRACT: Serial server for sending joint feedback to ROS.
;
; INPUTS: None.
;
; OUTPUTS: None.
;
; SIDE EFFECTS: None.
;
; DATA STRUCT: Server streams <LENGTH(bytes), <10>, <1(COMM_TYPE - TOPIC)>, <0 (REPLY_TYPE - N/A)>,
; <UNUSED <JOINT_DATA[10] (in rads (floats))>
;
AUTO idx, slun, status
AUTO REAL jts[10]
AUTO $tmp.str, $msg.len, $msg.type, $cmd.type, $msg.reply, $msg.jts[10], $msg.unused
AUTO LOC #cur.loc
AUTO $msg
$tmp.str = ""
; Initialize joints
FOR idx = 1 TO 10
jts[idx] = 0
END
; Open a Serial port
ATTACH (slun, 11) "SERIAL:1"
status = IOSTAT(slun)
IF (status < 0) THEN
TYPE "Error opening Serial port: "+$ERROR(status)
GOTO 100
END
; Server loop
WHILE run DO
; Format a message to send to ROS
CALL ros.msg.create(56, 10, 1, 0, 0, $msg)
; Get the current joint position
HERE #cur.loc
DECOMPOSE jts[1] = #cur.loc
; Format the joint data and append it to the message
FOR idx = 1 TO 10
jts[idx] = ((jts[idx]*PI)/180)
$tmp.str = $FLTB(jts[idx])
CALL ros.rev.bytes($tmp.str, $msg.jts[idx])
$msg = $msg+$msg.jts[idx]
END
; Send the message to ROS
WRITE (slun) $msg, /S
; Loop delay
WAIT.EVENT , delay.fdb
END
; Close the Serial port
DETACH (slun)
; Return
100 RETURN
.END
Display More
The serials are configured without flow control or DTR as per
Code
GLOBAL_PORT 1 = "/DRIVER SERIAL /SPEED 38400 /BYTE_LENGTH 8 /PARITY NONE
/STOP_BITS 1 /FLOW NONE /FLUSH OFF /DTR OFF /MULTIDROP OFF
/BUFFER_SIZE 200"
GLOBAL_PORT 2 = "/DRIVER SERIAL /SPEED 38400 /BYTE_LENGTH 8 /PARITY NONE
/STOP_BITS 1 /FLOW NONE /FLUSH OFF /DTR OFF /MULTIDROP OFF
/BUFFER_SIZE 200"
GLOBAL_PORT 3 = "/DRIVER SERIAL /SPEED 38400 /BYTE_LENGTH 8 /PARITY NONE
/STOP_BITS 1 /FLOW NONE /FLUSH OFF /DTR OFF /MULTIDROP OFF
/BUFFER_SIZE 200"