ROS-KINETIC
Hi, I'm trying make a code to a little robot with Arduino using ROSSERIAL, but sometimes when I change my code, this error happen, and i don't know why.
Let me explain.
On this code.
#include
#include
#include
#include
#include
#include
#include
//#include
#include
ros::NodeHandle nh;
rosserial_arduino::Adc adc_msg;
ros::Publisher p("adc", &adc_msg);
ros::Subscriber subCmdLeft("cmd_left_wheel", moveLeftMotorCB);
ros::Publisher left_wheel_vel_pub("/left_wheel_velocity", &left_wheel_vel);
ros::Subscriber subCmdRight("cmd_right_wheel", moveRightMotorCB);
ros::Publisher right_wheel_vel_pub("/right_wheel_velocity", &right_wheel_vel);
ros::Subscriber subCmdVel("cmd_vel", cmdVelCB);
ros::Publisher sensor_vel_pub("/sensor_velocity", &sensor_vel);
void setup()
{
setupEncoders();
setupMotors();
Timer3.initialize(LOOP_TIME); /// Periodic Timer interrupt for control tasks.
Timer3.attachInterrupt(controlLoop);
nh.initNode();
nh.advertise(p);
nh.subscribe(subCmdLeft);
nh.advertise(left_wheel_vel_pub);
nh.subscribe(subCmdRight);
nh.advertise(right_wheel_vel_pub);
nh.subscribe(subCmdVel);
nh.advertise(sensor_vel_pub);
bipGen(800, 200, 333, 3); // Startup bips. It takes 1s.
}
void loop()
{
if (loop_time < millis())
{
adc_msg.adc0 = averageAnalog(0);
adc_msg.adc1 = averageAnalog(1);
adc_msg.adc2 = averageAnalog(2);
adc_msg.adc3 = averageAnalog(3);
adc_msg.adc4 = averageAnalog(4);
adc_msg.adc5 = averageAnalog(5);
p.publish(&adc_msg);
loop_time = millis() + 1000; // 1 Hz analog data publishing.
}
nh.spinOnce();
}
void controlLoop()
{
Timer3.detachInterrupt(); //stop the timer
left_wheel_vel.data = float(left_encoder_position) * 2 * pi * left_wheel_radius * 1000000 / loop_time / gear_relationship / encoder_cpr;
left_wheel_vel_pub.publish(&left_wheel_vel);
right_wheel_vel.data = float(right_encoder_position) * 2 * pi * right_wheel_radius * 1000000 / loop_time / gear_relationship / encoder_cpr;
right_wheel_vel_pub.publish(&right_wheel_vel);
sensor_vel.linear.x = (left_wheel_vel.data * left_wheel_radius + right_wheel_vel.data * right_wheel_radius)/2;
sensor_vel.linear.y = 0;
sensor_vel.linear.z = 0;
sensor_vel.angular.x = 0;
sensor_vel.angular.y = 0;
sensor_vel.angular.z = (left_wheel_vel.data * left_wheel_radius + right_wheel_vel.data * right_wheel_radius)/l_wheels;
sensor_vel_pub.publish(&sensor_vel);
left_encoder_position = 0;
right_encoder_position = 0;
Timer3.attachInterrupt(controlLoop); //enable the timer
}
If I take off, the variable loop_time of this part, the code generates the error, but if I keep it there, don't. And don't understand why. But the problem that I need take off.
left_wheel_vel.data = float(left_encoder_position) * 2 * pi * left_wheel_radius * 1000000 / loop_time / gear_relationship / encoder_cpr;
How can I solve it?
IF I KEEP THERE, THE CODE WORKS FINE AND I CAN READ THE TOPICS FROM ARDUINO.
↧