How to use cmd_vel data in navigation stack?
I'm trying to convert geometry_msgs::Twist message received from cmd_vel topic to something that can make my arduino robot move. I know that from geometry_msgs::Twist message i'll receive linear velocity and angular velocity. My motors are actually controlled by calling functions like that:
void RobotControl::goAhead(int PWM){
digitalWrite(_IN1, LOW); digitalWrite(_IN2, HIGH);
digitalWrite(_IN3, LOW);
digitalWrite(_IN4, HIGH);
analogWrite(_ENB, PWM);
analogWrite(_ENA, PWM); }
in this case, PWM is a value between 0 and 200 and sets speed for my motors in this range.
The main question is:
Is there a way to use received cmd_vel data and convert this to instructions that can make my robot move by simpy call goAhead() function? If my goAhead function has not been implemented in the right way, how to implement a funciton that solve the problem?
Thank u!
--- EDIT--- ok, let's see if i understand this:
velCallback(Twist &twistmsg) {
vel_x = twistmsg.linear.x;
vel_th = twistmsg.angular.z;
if(vel_x == 0){
right_vel = vel_th * width_robot / 2.0;
left_vel = (-1) * right_vel; }
else if(vel_th == 0){
left_vel = right_vel = vel_x;
}
else{
left_vel = vel_x - vel_th / 2.0;
right_vel = vel_x + vel_th / 2.0;
}
now if everything is right i can calcolate RPM:
RPMleft = ((60 * left_vel) / (diameter * PI))
RPMright = ((60 * right_vel) / (diameter * PI))
now i should calcalate PWM, i googled for almost one hour but i can't find anything. Do you know how can i do it?