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;
}
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;
}