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;
}
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[])
{
///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;
}
