Explanation
Opened this issue · 7 comments
I have the same Odrive controller, but I am building 6-wheeled robot so I have three of them and I am using an Arduino and Jetson Nano as the brain of the robot. I can not publish Odom from Arduino, so I am sending pos0, pos1 and nh.now() to c++ node running on Jetson and the node on Jetson fix the rest exactly as u do. I get everything working fine but the robot move very fast while it is moving very slow in Rviz and I believe that is because u are using different wheel size and so on. But I can not see where u input ur wheel size and wheel separation and so on. I am wondering if you can point out where u do that and if you can explain quickly what is the values 83466,15091 and 83.44 respectively in lines 112, 115 and 135.
It's in the Arduino code based on the encoder counts - basically the lines you pointed out.
I put all the details & calcs here: https://github.com/XRobots/ReallyUsefulRobot/blob/main/wheelReduction.txt
Thanks for the quick response. I have been checking and testing with the calculation you refered to. But it I am doing something wrong which I can not figure out. The robot is going into the same directions in RVIZ but not with same amount. The real robot might go for 5 meters which should correspond to 5 squares in RVIZ but in RVIZ , it is showing so much less than that in RVIZ. I have hoverboard motors which are the wheels by them self. Wheel diameter is 6.5 inches =165.1 mm. And the ROBOT_WIDTH is 480 mm, the cpr is 90.
Here is my Arduino code part:
/*A node which subscribing /cmd_vel coming from keyboard(teleop node)
Publishing /pos containing info about the position(encoder) of left/right rear motors.
*/
#include <HardwareSerial.h>
#include <ODriveArduino.h>
#include <Wire.h>
#include <ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/Twist.h>
#include <ros/time.h>
#define ROBOT_WIDTH (480) //MILLI METER
#define WHEEL_DIAMETER (330.2) // MILLI METER
#define WHEEL_CIRC (518.676947108) //circumference OF THE WHEEL
#define CPR_ENCODER (90)
/*
Driving forward
1 revolution= WHEEL_CIRC = 518.676947108 mm
Each mm travelled is CPR_ENCODER/WHEEL_CIRC = 90/518.676947108 =0.1735184116 counts per mm
Then each meter travelled is 0.1735184116*1000=173.5184116 counts per meter
Then each meter/sec= 173.5184116 counts/sec
Turning:
2pi radians = 360' and pi radians = 180'
distance each wheel has to go to travel 180' = (ROBOT_WIDTH*pi)/2=753.982236862 mm=pi radians
753.982236862 mm=1.45366444579 revolution=130.829800121 encoder counts=pi radians
1 radian = 130.829800121/pi=41.644418786 encoder counts
*/
HardwareSerial& odrive_serial1 = Serial1;
HardwareSerial& odrive_serial2 = Serial2;
HardwareSerial& odrive_serial3 = Serial3;
//axis0(M0)-left
//axis1(M1)-right
ODriveArduino odrive1(odrive_serial1);
ODriveArduino odrive2(odrive_serial2);
ODriveArduino odrive3(odrive_serial3);
// cmd_vel variables to be received to drive with
float demandx;
float demandz;
// timers for the sub-main loop
unsigned long currentMillis;
long previousMillis = 0; // set up timers
float loopTime = 10;
long forward0;
long forward1;
long turn0;
long turn1;
// position and velocity variables read from the ODrive
long pos0;
long pos1;
long pos0_old;
long pos1_old;
long pos0_diff;
long pos1_diff;
float pos0_mm_diff;
float pos1_mm_diff;
ros::NodeHandle nh;
//function that will be called when receiving command from host
void handle_cmd (const geometry_msgs::Twist& cmd_vel) {
demandx = 0.02*cmd_vel.linear.x; //decrease by 98%
demandz = 0.02*cmd_vel.angular.z; //decrease by 98%
}
ros::Subscriber<geometry_msgs::Twist> cmd_vel("cmd_vel", handle_cmd); //create a subscriber to ROS topic for velocity commands (will execute "handle_cmd" function when receiving data)
geometry_msgs::Vector3Stamped pos_msg; //create a "pos_msg" ROS message
ros::Publisher pos_pub("pos", &pos_msg); //create a publisher to ROS topic "pos" using the "pos_msg" type
void setup() {
nh.initNode(); //init ROS node
nh.getHardware()->setBaud(115200); //set baud for ROS serial communication
nh.subscribe(cmd_vel); //suscribe to ROS topic for velocity commands
nh.advertise(pos_pub); //prepare to publish speed in ROS topic
odrive_serial1.begin(115200);
odrive_serial2.begin(115200);
odrive_serial3.begin(115200);
odrive1.SetVelocity(0,0);
odrive1.SetVelocity(1,0);
odrive2.SetVelocity(0,0);
odrive2.SetVelocity(1,0);
odrive3.SetVelocity(0,0);
odrive3.SetVelocity(1,0);
odrive1.SetPosition(0,0.0);
odrive1.SetPosition(1,0.0);
odrive2.SetPosition(0,0.0);
odrive2.SetPosition(1,0.0);
odrive3.SetPosition(0,0.0);
odrive3.SetPosition(1,0.0);
Serial.begin(115200);
while (!Serial) ;
}
//_________________________________________________________________________
void loop() {
nh.spinOnce();
currentMillis = millis();
if (currentMillis - previousMillis >= loopTime) { // run a loop every 10ms
previousMillis = currentMillis; // reset the clock to time it
float modifier_lin = 1.03; // scaling factor because the wheels are squashy / there is wheel slip etc.
float modifier_ang = 0.92; // scaling factor because the wheels are squashy / there is wheel slip etc.
forward0 = demandx * (173.5184116 * modifier_lin) ; // convert m/s into counts/s
forward1 = demandx * (173.5184116 * modifier_lin); // convert m/s into counts/s
turn0 = demandz * (41.644418786 * modifier_ang); // convert rads/s into counts/s
turn1 = demandz * (41.644418786 * modifier_ang); // convert rads/s into counts/s
forward1 = forward1*-1; // one motor and encoder is mounted facing the other way
odrive1.SetVelocity(1, forward0 + turn0); //-1 the motors are on opposite direction
odrive1.SetVelocity(0, forward1 + turn1);
odrive2.SetVelocity(1, forward0 + turn0); //-1 the motors are on opposite direction
odrive2.SetVelocity(0, forward1 + turn1);
odrive3.SetVelocity(1, forward0 + turn0); //-1 the motors are on opposite direction
odrive3.SetVelocity(0, forward1 + turn1);
// get positions from ODrive
pos1 = (((odrive1.GetPosition(0)) *-1)+((odrive2.GetPosition(0)) *-1)+((odrive3.GetPosition(0)) *-1))/3.0;
pos0 = (odrive1.GetPosition(1)+odrive2.GetPosition(1)+odrive3.GetPosition(1))/3.0;
// work out the difference on each loop, and bookmark the old value
pos0_diff = pos0 - pos0_old;
pos1_diff = pos1 - pos1_old;
pos0_old = pos0;
pos1_old = pos1;
// calc mm from encoder counts
pos0_mm_diff = pos0_diff /0.1735184116;
pos1_mm_diff = pos1_diff /0.1735184116;
publishPos();
}
}
void publishPos() {
pos_msg.header.stamp = nh.now(); //timestamp for odometry data
pos_msg.vector.x = pos0_mm_diff;
pos_msg.vector.y = pos1_mm_diff;
pos_pub.publish(&pos_msg);
nh.spinOnce();
}
Maybe multiply the number by 5 then?
Does not seems as a solution for the problem but another question is that : "forward0 = demandx * (173.5184116 * modifier_lin) ; // convert m/s into counts/s". So according to my understading u assume that setVelocity() takes ticks per second because that is what you are sending. While according to "https://docs.odriverobotics.com/ascii-protocol" the velocity is "the desired velocity in [turns/s]" which is different to the ticks/s you sending. Is not turn/s the revolution/s?
turns/s is in the new ODrive firmware that's just been released. I'm using the old firmware which is counts/s
Ok, but then that is why I have problems with it. Do you recommend me to use an old firmware or should I change the code to work with turns/s instead? Does the getposition in old firmware returns counts? becuase the new one returns turns which means 1.3 revloution is detected as one revolution and so on.
Yes it returns counts. It's up to you I guess, I haven't tried using the new firmware yet.