Commit 015e13cd authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Implemented the custom printf and scanf functions to use the virtual serial port when it exists.

parent 1e56e008
......@@ -24,5 +24,16 @@
<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
output="screen"/>
<node name="gazebo_image_to_video_dev"
pkg="gazebo_image_to_video_dev"
type="gazebo_image_to_video_dev"
output="screen">
<remap from="~/camera_in/image_raw"
to="/bioloid/camera/image_raw"/>
<remap from="~/camera_in/camera_info"
to="/bioloid/camera/camera_info"/>
<param name="video_dev" value="/dev/video0"/>
</node>
</launch>
......@@ -18,7 +18,7 @@ bioloid:
exp_gpio3_frame: right_front_fwd_ir_link
exp_gpio4_frame: right_front_dwn_ir_link
exp_gpio5_frame: right_lateral_dwn_ir_link
serial_console_device: /dev/ttyS100
serial_console_device: /dev/pts/18
joints:
- j_shoulder_r
- j_shoulder_l
......
......@@ -20,7 +20,7 @@ bioloid:
exp_gpio5_frame: right_lateral_dwn_ir_link
pan_servo_id: 19
tilt_servo_id: 20
serial_console_device: /dev/pts/19
serial_console_device: /dev/pts/18
joints:
- j_shoulder_r
- j_shoulder_l
......
......@@ -92,7 +92,7 @@ catkin_package(
###########
SET(BIOLOID_FW_PATH ~/humanoids/cm510_controller_fw)
SET(ProjectPath ~/humanoids/cm510_controller_fw/examples/pan_tilt)
SET(ProjectPath ~/humanoids/cm510_controller_fw/examples/vision)
## Specify additional locations of header files
## Your package locations should be listed before other locations
......@@ -130,7 +130,7 @@ add_library(${PROJECT_NAME} src/bioloid_controller_cm510.cpp
# communications modules
${BIOLOID_FW_PATH}/communications/src/dynamixel.c
# main application module
${ProjectPath}/pan_tilt.c
${ProjectPath}/cm510_vision.c
# AVR simulation modules
src/sim/avr_delay.c
src/sim/avr_registers.c
......
......@@ -6,8 +6,11 @@
#include "rs232.h"
extern "C" int vsprintf (char * s, const char * format, va_list arg );
extern "C" int vfprintf ( FILE * stream, const char * format, va_list arg );
extern "C" int vfprintf (FILE * stream, const char * format, va_list arg );
extern "C" int vsscanf (const char * s, const char * format, va_list arg );
extern "C" int vfscanf ( FILE * stream, const char * format, va_list arg );
extern FILE *stdout;
extern FILE *stdin;
// private variables
CRS232 serial_console("bioloid_serial_port");
......@@ -39,12 +42,11 @@ int serial_console_getchar(FILE *dev)
return byte;
}
/*int printf(const char *fmt, ...)
int cm510_printf(const char *fmt, ...)
{
va_list ap;
int i;
std::cout << "printf" << std::endl;
if(rs232_enabled)
{
char out_buff[256];
......@@ -60,7 +62,44 @@ int serial_console_getchar(FILE *dev)
va_end(ap);
}
return i;
}*/
}
int cm510_scanf(const char *fmt, ...)
{
va_list ap;
int done,num;;
if(rs232_enabled)
{
char *out_buff=NULL;
CEventServer *event_server=CEventServer::instance();
std::list<std::string> events;
try{
events.push_back(serial_console.get_rx_event_id());
event_server->wait_first(events,1000);
num=serial_console.get_num_data();
out_buff=new char[num];
serial_console.read((unsigned char *)out_buff,num);
va_start(ap,fmt);
done=vsscanf(out_buff,fmt,ap);
va_end(ap);
delete[] out_buff;
}catch(CException &e){
std::cout << e.what() << std::endl;
if(out_buff!=NULL)
delete[] out_buff;
}
}
else
{
va_start (ap, fmt);
done = vfscanf (stdin, fmt, ap);
va_end (ap);
}
return done;
}
/* pubic functions */
void serial_console_init(uint32_t baudrate)
......@@ -80,3 +119,13 @@ void serial_console_init(uint32_t baudrate)
rs232_enabled=false;
}
}
uint8_t serial_console_get_num_data(void)
{
if(rs232_enabled)
return serial_console.get_num_data();
else
{
return 0;
}
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment