Explanation

[mazoxBahy]

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.

[XRobots]

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

[mazoxBahy]

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();
}

[XRobots]

Maybe multiply the number by 5 then?

[mazoxBahy]

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?

[XRobots]

turns/s is in the new ODrive firmware that's just been released. I'm using the old firmware which is counts/s

[mazoxBahy]

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.

[XRobots]

Yes it returns counts. It's up to you I guess, I haven't tried using the new firmware yet.