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;
}
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;
}
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;
}
void quitSequence(int sig){
int status=1;
cout<<"- SetCommand(_GO, 2, 0)...";
while((status != RQ_SUCCESS)){
if((device.SetCommand(_GO, 1, 0)!=RQ_SUCCESS)||(device.SetCommand(_GO,2,0)!=RQ_SUCCESS)){
status=16;
}
else{
status=RQ_SUCCESS;
}
if(status==RQ_SUCCESS){
cout<<"succeeded."<<endl;;
}
else{
cout<<"failed --> "<<status<<endl;
}
}
device.Disconnect();
kill(getpid(),SIGKILL);
}
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;
}
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;
}
