int main(int argc, char* argv[]){	
	
	ros::init(argc, argv, "trajec_design");
	ros::NodeHandle n;
	ros::Rate loop_rate(10);
		
	ROS_INFO("\n\n--- Roboteq Motor Controller Request Gateway Server ---\n");
	ROS_INFO("Initializing...");
	
	status = device.Connect("/dev/ttyACM0");

	while (status != RQ_SUCCESS && ros::ok())
	{
		ROS_INFO("Error connecting to device: %d\n", status);
		ROS_INFO("Attempting server restart...");
		usleep(1000000);
		device.Disconnect();

		status = device.Connect("/dev/ttyACM0");
		if (status == 0) {
			ROS_INFO("Connection re-established...");
		}
	}
	
	int mode=0;
	device.GetConfig(_MXMD, mode);
	ROS_INFO("Operating mode: %d", mode);
	ros::Duration(1.0).sleep();
	
	geometry_msgs::Twist my_twist;
	
	device.SetConfig(_MXRPM, 1, 5000);
	device.SetConfig(_MXRPM, 2, 5000);
	
	ros::Subscriber heading = n.subscribe("/vn100_monitor", 1, PID);
	ros::Subscriber desired_heading = n.subscribe("/desired_bearing", 1, deshead);
	ros::Subscriber lessDist=n.subscribe("/message", 1, no_obs);
	
	ros::Publisher pub=n.advertise<geometry_msgs::Twist>("/husky/cmd_vel",1);
	
	cin>>vel;

	while(ros::ok()){
		ros::spinOnce();
		my_twist.linear.x=v;
		my_twist.angular.z=w;
		pub.publish(my_twist);
		//ROS_INFO("%f", my_twist.linear.x);
		//ROS_INFO("%f", my_twist.angular.z);
		loop_rate.sleep();
	}
	
	return 0;
}
Esempio n. 2
0
int main(int argc, char *argv[])
{
        int i;
	string response = "";
	if(argc<2){
	  cout<<"Need Port";
	  return -1;}
	int status = device.Connect(argv[1]);

	if(status != RQ_SUCCESS)
	{
		cout<<"Error connecting to device: "<<status<<"."<<endl;
		return 1;
	}
	
	for(i=0;i<1;i++){
	cout<<"- SetCommand(_GO, 2, 100)...";
	if((status = (device.SetCommand(_GO, 1, 300)+device.SetCommand(_GO,2,300))) != RQ_SUCCESS)
		cout<<"failed --> "<<status<<endl;
	else
		cout<<"succeeded."<<endl;
	}
	signal(SIGTSTP,quitSequence);
	while(1);
	return 0;
}
Esempio n. 3
0
int main()
{
	string response = "";
	RoboteqDevice device;
	int status = device.Connect("/dev/ttyACM0");

	if(status != RQ_SUCCESS)
	{
		cout<<"Error connecting to device: "<<status<<"."<<endl;
		return 1;
	}

	device.Disconnect();
	return 0;
}
Esempio n. 4
0
int main(int argc, char *argv[])
{   
    int status = device.Connect("/dev/ttyACM1");
    if(status != RQ_SUCCESS)
    {
        cout<<"Error connecting to device: "<<status<<"."<<endl;
        return 1;
    }

    device.SetConfig(_RWD, 1, -1);
    device.SetConfig(_RWD, 2, -1);
    
    ros::init(argc, argv, "move_script");
    ros::NodeHandle n;
    
    n.getParam("odom_from_enc/odom_mode", ODOMETRY_MODE);
    n.getParam("odom_from_enc/pub_tf", PUB_TF);
    n.getParam("odom_from_enc/pub_odom", PUB_ODOM);
    n.getParam("odom_from_enc/max_speed_lim", MAX_SPEED_LIMIT);
    
    ROS_INFO("Odometry Mode : %d", ODOMETRY_MODE);
    ROS_INFO("Publish TF : %d", PUB_TF);
    ROS_INFO("Publish Odometry Messages : %d", PUB_ODOM);
    ROS_INFO("Max Speed %% Limit : %f", MAX_SPEED_LIMIT);
    prev_time = ros::Time::now();
    
    imu_prev_time = ros::Time::now();
    
    ros::Duration(0.1).sleep();
    odom_broadcaster = new tf::TransformBroadcaster;
    odom_pub = n.advertise<nav_msgs::Odometry > ("/odom", 10);
    pose_pub2 = n.advertise<geometry_msgs::PoseStamped>("/poseStamped",5);
    ros::Subscriber enc_sub = n.subscribe("/encoders", 100, encoderCallback);
    ros::Subscriber encoder_tick_sub = n.subscribe("/encoder_ticks", 100, encoderTickCallback);
    //ROS_INFO("- SetConfig(_DINA, 1, 1)...");
    if((status = device.SetConfig(_DINA, 1, 1)) != RQ_SUCCESS)
      cout<<"failed --> "<<status<<endl;
    else
      //ROS_INFO("succeeded.");
      
    ros::Duration(0.01).sleep(); //sleep for 10 ms

    int result;
    int MAX_RPM;
    enc_loop_time = ros::Time::now();
    enc_loop_time_2 = ros::Time::now();
    //ROS_INFO("- GetConfig(_DINA, 1)...");
    if((status = device.GetConfig(_DINA, 1, result)) != RQ_SUCCESS)
      cout<<"failed --> "<<status<<endl;
    else
      cout<<"returned --> "<<result<<endl;
    
    //ROS_INFO("Reading RPM");
    if((status = device.GetConfig(_MXRPM, 1, MXRPM)) !=RQ_SUCCESS)
        cout<<"reading rpm M1 failed -->"<<status<<endl;
    else
        cout<<"MOTOR 1 MAXRPM : "<<MXRPM<<endl;
        
    if((status = device.GetConfig(_MXRPM, 2, MXRPM)) !=RQ_SUCCESS)
        cout<<"reading rpm M2 failed -->"<<status<<endl;
    else
        cout<<"MOTOR 2 MAXRPM : "<<MXRPM<<endl;
    
    encoder_cpr = encoder_ppr*4;
    //SET ACCELERATION
    /* 
    //ROS_INFO("SETTING ACCELERATION");
    if((status = device.SetConfig(_AC, 1, ACCELERATION)) !=RQ_SUCCESS)
        cout<<"failed -->"<<result<<endl;
    else
        cout<<"MOTOR 1 ACCEL : "<<ACCELERATION<<endl;
    
    if((status = device.SetConfig(_AC, 2, ACCELERATION)) !=RQ_SUCCESS)
        cout<<"failed -->"<<result<<endl;
    else
        cout<<"MOTOR 2 ACCEL : "<<ACCELERATION<<endl;
    
    
    //ROS_INFO("Roboteq -- Successfull setup of the Roboteq SDC2130");
    */
    printf ("Sek Operational\n\n");
    ros::Duration(0.01).sleep(); //sleep for 10 ms
    while (ros::ok())
    {
        ros::spinOnce();
        //current_time = ros::Time::now();
        //if ((current_time.toSec() - enc_loop_time.toSec())>=0.2)
        //{
        //    readEnc();
        //    calcOdom();
        //    enc_loop_time = current_time;
        //}
    }
    device.Disconnect();
    return 0;
}
int main(int argc, char* argv[])
{	
	
	///ROS Initializations
	ros::init(argc, argv, "Roboteq_Channel_Tests");
	ros::NodeHandle n;
	ros::Publisher publisher = n.advertise<nav_msgs::Odometry>("/encoder_data", 1);
	ros::Rate loop_rate(20);
	
	
	
	///ROBOTEQ Connection
	ROS_INFO("\n\n--- Roboteq Motor Controller Request Gateway Server ---\n");
	ROS_INFO("Initializing...");
	usleep(500000);

	status = device.Connect("/dev/ttyACM0");

	while (status != RQ_SUCCESS && ros::ok())
	{
		ROS_ERROR("Error connecting to device: %d\n", status);
		ROS_INFO("Attempting server restart...");
		usleep(999999);
		device.Disconnect();

		status = device.Connect("/dev/ttyAMC0");
		if (status == 0) {
			ROS_INFO("Connection re-established...");
		}
	}
	
	///Initialization and Fixed Data
	output.pose.pose.position.x=0; output.pose.pose.position.y=0; output.pose.pose.position.z=0;
	output.pose.pose.orientation.x=1; output.pose.pose.orientation.y=0; output.pose.pose.orientation.z=0; output.pose.pose.orientation.w=0;
	for(int i=0; i<36; i++){
		if((i/6)==(i%6)) output.pose.covariance[i]=99999;
		else output.pose.covariance[i]=0;
	}
	output.twist.twist.linear.x = 0; output.twist.twist.linear.y = 0; output.twist.twist.linear.z = 0;
	output.twist.twist.angular.x = 0; output.twist.twist.angular.y = 0; output.twist.twist.angular.y = 0;
	for(int i=0; i<36; i++){
		if((i/6)==(i%6)) output.twist.covariance[i]=99999;
		else output.twist.covariance[i]=0;
	}

	///The Publishing Loop
	device.SetCommand(_C, 1, 0);
	device.SetCommand(_C, 2, 0);
	int ch1_rpm_value = 0, ch2_rpm_value = 0;
	localization::roboteq_msg unfiltered;

	output.header.frame_id = "odom_combined";
	output.child_frame_id = "base_footprint";
	
	while(ros::ok()){
		/*Gathering Data from Device*/
			device.GetValue(_S, 1, ch1_rpm_value);
			device.GetValue(_S, 2, ch2_rpm_value);
	
		/*Preparing Data for Low-Pass Filter*/
			//Time
			unfiltered.header.stamp = ros::Time::now();
			//RPM Stuff
			unfiltered.rpm_1 = ch1_rpm_value; unfiltered.rpm_2 = ch2_rpm_value;
		
		/*LowPass Filtering*/
		low_pass(unfiltered);
		
		/*Calculating 'v' and 'w' From Filtered Data*/
			//Calculate 'v'
			v = v_factor*(filtered.rpm_2 - filtered.rpm_1); //rpm_1 is negatived because when the wheel turns forwards, rpm_1 is negative
			//Calculate 'w'
			w = w_factor*(filtered.rpm_2 + filtered.rpm_1); //positive is left, negative is right (rpm_2 refers to right wheel)

		/*Data to Output Message*/
			//Header Data
			output.header.stamp = filtered.header.stamp;
			output.header.seq++;
			//Twist Data
			output.twist.twist.linear.x = v;
			output.twist.twist.angular.z = w;
			//Covariances
			output.twist.covariance[0] = linearVelocityVariance;
			output.twist.covariance[35] = angularVelocityVariance;
		
		/*Publishing*/
		publisher.publish(output);
		loop_rate.sleep();
	}
	
	return 0;
}