int main(int argc, char* argv[])
{
printf("[i] Start... \n");
Serial serial;
#if defined(WIN32)
serial.open(SERIAL_PORT_NAME, SERIAL_PORT_RATE, true);
#elif defined(LINUX)
serial.open(SERIAL_PORT_NAME, SERIAL_PORT_RATE);
#endif
if(!serial.connected())
{
printf("[!] Cant open port!\n");
return -1;
}
char c = 'y';
int res = 12;
char buf[BUF_SIZE1];
memset(buf, 0, BUF_SIZE1);
while(true)
{
serial.write( &c, 1 );
#if 0
if(res = serial.available()){
if( res = serial.Read(buf, res) ){
printf("%d %s\n", res, buf);
}
}
#else
if(serial.waitInput(1000)==0)
printf("[i] timeout!\n");
else
{
if(res = serial.available())
{
res = serial.read(buf, res);
printf("%d %s\n", res, buf);
}
}
#endif
}
serial.close();
printf("[i] End.\n");
return 0;
}
void setup() {
m_tagDetector = new AprilTags::TagDetector(m_tagCodes);
// prepare window for drawing the camera images
if (m_draw) {
cv::namedWindow(windowName, 1);
}
// optional: prepare serial port for communication with Arduino
if (m_arduino) {
m_serial.open("/dev/ttyACM0");
}
}
int main() {
Serial serial;
serial.open("/dev/ttyUSB0", 38400); // might need to change to your USB port
// read and parse one line at a time
while (true) {
string s = serial.readBytesUntil('\n');
parse(s);
}
return 0;
}
int main(int argc, char **argv){
cout<<"Connecting to the k3 robot..."<<endl;
try {
k3_ser.open();
}
catch(std::exception e) {
std::stringstream output;
output<<"Failed to open serial port "<< k3_ser.getPort() << "error: " << e.what() <<endl;
}
if(k3_ser.isOpen()){
cout<<"k3_ser is Connected on Port: "<<k3_ser.getPort()<<" at Baudrate: "<<k3_ser.getBaudrate()<<endl;
}
else{
ROS_ERROR("serial port not openened, verify the k3 robot selector position");
}
//test serial command
k3_ser.write("D,0,0\n");
ros::init(argc, argv, "k3");
cout<<"hello ros";
ros::NodeHandle n;
cmdVelSubscriber = n.subscribe("/robot_motors", 10, handlerVelocity);
ros::Rate loop_rate(10);
while(ros::ok()){
k3Motors();
//k3Cmd();
ros::spinOnce(); //Allow ROS to check for new ROS Messages
loop_rate.sleep(); //Sleep for some amount of time determined by loop_rate
}
return 0;
}
TEST(Serial, WriteCOM) {
Serial serial;
serial.open("COM7", 9600);
if(!serial.connected()){
FAIL()<<"cant open serial port!";
}
char buf[1024];
int res = 0;
while(1){
char wbuf[] = {0xff, 0xff, 0x00, 0x00, 0x00, 0x00 ,0xff};
serial.write(wbuf, sizeof(wbuf));
if( serial.waitInput(1000) > 0){
if( (res = serial.available()) > 0 ){
if( res = serial.read(buf, res) ){
printf("[i] read data(%d): \n", res);
for(int i=0; i<res; i++){
printf("%02X ", (unsigned char)buf[i]);
if(i>0 && (i+1)%16 == 0) {
printf("\t");
for(int j=i-15; j<=i; j++){
printf("%c", buf[j]);
}
printf("\n");
}
}
printf("\n");
res = 0;
}
}
}
Sleep(1000);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "robot_4wd_node");
ros::NodeHandle n;
drive_telemetry_pub = n.advertise<ros_4wd_driver::drive_telemetry_4wd>("drive_telemetry_4wd", 50);
sensors_telemetry_pub = n.advertise<ros_4wd_driver::sensors_telemetry_4wd>("sensors_telemetry_4wd", 50);
imu_raw_data_pub = n.advertise<ros_4wd_driver::imu_raw_data>("imu_raw_data", 50);
motor_service = n.advertiseService("motor_write", motor_write);
odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
cmd_vel_sub = n.subscribe<geometry_msgs::Twist>("cmd_vel", 1, cmd_vel_received);
ROS_INFO("Start");
ROS_INFO("Load params");
if (n.hasParam("/robot_4wd_node/drive_port")) {
ROS_INFO("Load param: drive_port");
n.getParam("/robot_4wd_node/drive_port", drive_serial_name);
}
if (n.hasParam("/robot_4wd_node/sensor_port")) {
ROS_INFO("Load param: sensor_port");
n.getParam("/robot_4wd_node/sensor_port", sensors_serial_name);
}
if (n.hasParam("/robot_4wd_node/baud")) {
ROS_INFO("Load param: baud");
n.getParam("/robot_4wd_node/baud", serial_rate);
}
n.param("/robot_4wd_node/wheel_diameter", wheel_diameter, wheel_diameter);
n.param("/robot_4wd_node/wheel_track", wheel_track, wheel_track);
n.param("/robot_4wd_node/gear_reduction", gear_reduction, gear_reduction);
n.param("/robot_4wd_node/encoder_resolution", encoder_resolution, encoder_resolution);
ticks_per_meter = encoder_resolution * gear_reduction / (wheel_diameter * M_PI);
ROS_INFO("drive_port: %s", drive_serial_name.c_str());
ROS_INFO("sensor_port: %s", sensors_serial_name.c_str());
ROS_INFO("baud: %d", serial_rate);
ROS_INFO("ticks_per_meter: %.2f", ticks_per_meter);
ROS_INFO("Open ports");
if( drive_serial.open(drive_serial_name.c_str(), serial_rate) ) {
ROS_ERROR("Cant open port: %s:%d", drive_serial_name.c_str(), serial_rate);
return -1;
}
if( sensors_serial.open(sensors_serial_name.c_str(), serial_rate) ) {
ROS_WARN("Cant open port: %s:%d", sensors_serial_name.c_str(), serial_rate);
}
#if 0
orcp2::ORCP2 orcp(drive_serial);
for(int i=0; i<7; i++) {
int val = i%2;
printf("%d\n", val);
orcp.digitalWrite(13, val);
orv::time::sleep(500);
}
#endif
boost::thread drv_thread(&drive_thread);
boost::thread snsr_thread(&sensors_thread);
while (ros::ok()) {
ros::spin();
}
global_stop = true;
drv_thread.join();
snsr_thread.join();
drive_serial.close();
sensors_serial.close();
ROS_INFO("End");
return 0;
}
SCSAPI_RESULT scs_telemetry_init(const scs_u32_t version, const scs_telemetry_init_params_t *const params)
{
if (version != SCS_TELEMETRY_VERSION_1_00) {
return SCS_RESULT_unsupported;
}
const scs_telemetry_init_params_v100_t *const version_params = static_cast<const scs_telemetry_init_params_v100_t *>(params);
game_log = version_params->common.log;
game_log(SCS_LOG_TYPE_message, "Plugin initialising");
std::string cwd;
get_cwd(cwd);
game_log(SCS_LOG_TYPE_message, (std::string("Plugin CWD: ") + cwd).c_str());
std::string option_filepath(cwd + "\\plugins\\dash_plugin.txt");
if (!option_file.read_file(option_filepath))
{
game_log(SCS_LOG_TYPE_error, (std::string("Error reading settings file: ") + option_filepath).c_str());
return SCS_RESULT_generic_error;
}
const std::string com_port = option_file.get_option_string("comport", "COM3");
game_log(SCS_LOG_TYPE_message, (std::string("Using serial port: ") + com_port).c_str());
// Open COM port
std::string errmsg;
if (!serial_port.open(com_port, errmsg))
{
game_log(SCS_LOG_TYPE_error, errmsg.c_str());
return SCS_RESULT_generic_error;
}
send_empty_packet();
// Register for in game events
bool registered =
(version_params->register_for_event(
SCS_TELEMETRY_EVENT_frame_end, telemetry_frame_end, NULL) == SCS_RESULT_ok) &&
(version_params->register_for_event(
SCS_TELEMETRY_EVENT_configuration, telemetry_configuration, NULL) == SCS_RESULT_ok);
// Register for truck channels
#define REG_CHAN(CHANNEL, TYPE) \
registered &= (version_params->register_for_channel( \
SCS_TELEMETRY_TRUCK_CHANNEL_ ## CHANNEL, SCS_U32_NIL, SCS_VALUE_TYPE_ ## TYPE, \
SCS_TELEMETRY_CHANNEL_FLAG_none, telemetry_store_ ## TYPE, &telemetry.CHANNEL) == SCS_RESULT_ok)
REG_CHAN(speed, float);
REG_CHAN(engine_rpm, float);
REG_CHAN(engine_gear, s32);
REG_CHAN(parking_brake, bool);
REG_CHAN(motor_brake, bool);
REG_CHAN(brake_air_pressure, float);
REG_CHAN(brake_air_pressure_warning, bool);
REG_CHAN(brake_air_pressure_emergency, bool);
REG_CHAN(brake_temperature, float);
REG_CHAN(fuel, float);
REG_CHAN(fuel_warning, bool);
REG_CHAN(fuel_average_consumption, float);
REG_CHAN(oil_pressure, float);
REG_CHAN(oil_pressure_warning, bool);
REG_CHAN(oil_temperature, float);
REG_CHAN(water_temperature, float);
REG_CHAN(water_temperature_warning, bool);
REG_CHAN(battery_voltage, float);
REG_CHAN(battery_voltage_warning, bool);
REG_CHAN(electric_enabled, bool);
REG_CHAN(engine_enabled, bool);
REG_CHAN(light_lblinker, bool);
REG_CHAN(light_rblinker, bool);
REG_CHAN(light_parking, bool);
REG_CHAN(light_low_beam, bool);
REG_CHAN(light_high_beam, bool);
REG_CHAN(light_brake, bool);
REG_CHAN(light_reverse, bool);
REG_CHAN(odometer, float);
if (!registered)
{
game_log(SCS_LOG_TYPE_error, "Unable to register callbacks");
return SCS_RESULT_generic_error;
}
memset(&telemetry, 0, sizeof(telemetry));
return SCS_RESULT_ok;
}
int main(int argc, char **argv) {
std::string port("/dev/ttyUSB0");
if ( argc > 1 ) {
std::string arg(argv[1]);
if ( arg == "--help" ) {
print_usage();
return 0;
} else {
port = argv[1];
}
}
std::cout << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << " Serial Timeouts" << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << "* For demo'ing low latency read timeouts on posix systems." << std::endl;
std::cout << "* Timeouts < 100ms use a custom loop, > 100ms use termios." << std::endl;
std::cout << "* Hook this up to a serial cable to test (actual" << std::endl;
std::cout << "* connection not important)." << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << std::endl;
Serial serial;
try {
serial.open(port,BaudRate_115200,DataBits_8,StopBits_1,NoParity);
} catch (StandardException &e ) {
std::cout << "[ERROR] : error opening " << port << std::endl;
std::cout << std::endl;
print_usage();
return 0;
}
TimeStamp time, pre_read_time;
char buffer[256];
std::cout << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << " 100 ms timeouts" << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << "* This will use termios to scan." << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << std::endl;
serial.block(100);
for ( unsigned int i = 0; i < 10; ++i ) {
pre_read_time.stamp();
long result = serial.read(buffer,256);
time.stamp();
if ( result > 0 ) {
std::cout << "[INFO] : read " << result << " bytes." << std::endl;
} else if ( result == 0 ) {
std::cout << "[INFO] : timed out [" << (time - pre_read_time) << "]." << std::endl;
} else {
std::cout << "[INFO] : error " << result << "." << std::endl;
}
}
std::cout << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << " 50 ms timeouts" << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << "* This will internally scan with 5ms loops." << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << std::endl;
serial.block(50);
// uncomment this to test reading without a timeout on a loopback connection.
// buffer[0] = 'a';
// buffer[1] = 'b';
// serial.write(buffer,2);
for ( unsigned int i = 0; i < 10; ++i ) {
pre_read_time.stamp();
long result = serial.read(buffer,256);
time.stamp();
if ( result > 0 ) {
std::cout << "[INFO] : read " << result << " bytes." << std::endl;
} else if ( result == 0 ) {
std::cout << "[INFO] : timed out [" << (time - pre_read_time) << "]." << std::endl;
} else {
std::cout << "[INFO] : error " << result << "." << std::endl;
}
}
std::cout << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << " 20 ms timeouts" << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << "* This will internally scan with 2ms loops." << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << std::endl;
serial.block(20);
for ( unsigned int i = 0; i < 10; ++i ) {
pre_read_time.stamp();
long result = serial.read(buffer,256);
time.stamp();
if ( result > 0 ) {
std::cout << "[INFO] : read " << result << " bytes." << std::endl;
} else if ( result == 0 ) {
std::cout << "[INFO] : timed out [" << (time - pre_read_time) << "]." << std::endl;
} else {
std::cout << "[INFO] : error " << result << "." << std::endl;
}
}
std::cout << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << " 5 ms timeouts" << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << "* This will internally scan with 1ms loops." << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << std::endl;
serial.block(5);
for ( unsigned int i = 0; i < 10; ++i ) {
pre_read_time.stamp();
long result = serial.read(buffer,256);
time.stamp();
if ( result > 0 ) {
std::cout << "[INFO] : read " << result << " bytes." << std::endl;
} else if ( result == 0 ) {
std::cout << "[INFO] : timed out [" << (time - pre_read_time) << "]." << std::endl;
} else {
std::cout << "[INFO] : error " << result << "." << std::endl;
}
}
return 0;
}