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;
}
void motorcommand(){
	float pid = (PID_RESPONSE_SPEED*P*err_new + I*erri + D*errd);
	if (pid < (-20)) pid = -20;
	if (pid > 20) pid = 20;
	int _max = 1000; 
	device.GetConfig(_MXRPM, _max); 
	float max = _max;
	
	w = pid*correction_factor_ang*DEG_TO_RAD*(Lr/2)*(60/CIRC)*(float)turn_direction*(1000/max)*RED; 
	
	// v0 = vel*(1-fabs(w)/1000);
			
	v = vel*correction_factor_vel*(60/CIRC)*(float)power_direction*(1000/max)*RED;                    		
	cout<<v<<" "<<w<<endl;		
	if( (((v+w)*power_direction)>2000) || (((v-w)*power_direction ) > 2000) ){ 
		device.SetCommand(_G, 1, 0); device.SetCommand(_G, 2, 0);
	}
	
	status = device.SetCommand(_G, 1, int(w));	
	status = device.SetCommand(_G, 2, int(v));
	
	v0 = 0;
}
예제 #3
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;
}