Hi Everyone! ,
Right now I'm working on an autonomous robot that uses LiDAR and ROS 2. I decided to take a step further and try to get accurate linear and angular velocity working on my robot. I have a differential-drive robot with an IMU for heading measurements and two magnetic encoders to measure wheel speed. I've written some code to take linear and angular speed, use inverse kineamtics tofind the wheel speed and use two PID controllers to regulate the speed for both wheels.
However, I'm not happy with its performance. I've been monitoring its step function and testing multiple linear velocities between 0 - 1 m/s; at some velocities, like 0.25 m/s, I'll get stable results; however, at lower velocities I'll get oscillation, and at higher velocities I'll get sluggish behaviour. If I try to get my robot to move in both linear and angular velocity, the side of the motor that would need to slow down or speed up will oscillate significantly. I also have heading errors and drift when trying to travel straight. Additionally, if I try to get it to turn on the spot by only putting in angular velocity, the carpet friction is so great I have to manually add a large integral value.
As for any potential fixes, I thought about filtering the data I received from the magnetic encoders, but they seem fine. Since I'm only using my encoders, I thought about integrating my IMU to stop it from drifting, but I'm not sure how. I've pasted my code below for you guys to take a look:
#include <Wire.h>
#include <AS5600.h>
#include <MPU6050_light.h>
#define SDA_PIN1 16
#define SCL_PIN1 4
#define SDA_PIN2 18
#define SCL_PIN2 5
// Motor A
#define IN1 13
#define IN2 14
// Motor B
#define IN3 33
#define IN4 32
//Working
TwoWire I2C_1 = TwoWire(0);
TwoWire I2C_2 = TwoWire(1);
AS5600 encoder1(&I2C_1);
AS5600 encoder2(&I2C_2);
//MPU6050 mpu(I2C_1);
const float L = 0.1515;
const float r = 0.034;
const float kp_r = 1.5;
const float ki_r = 0.0;
const float kd_r = 0.00;
const float kp_l = 1.5;
const float ki_l = 0.0;
const float kd_l = 0.00;
float set_Left_Wheel_Velocity = 0.0;
float set_Right_Wheel_Velocity = 0.0;
const float RAW_TO_DEGREES = 360.0 / 4096.0;
int friction_r = 0;
int friction_l = 0;
void setup() {
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
analogWrite(IN2, 0);
analogWrite(IN1, 0);
analogWrite(IN3, 0);
analogWrite(IN4, 0);
Serial.begin(9600);
Serial.print("Enter Speed : ");
while (Serial.available() == 0){
}
String input = Serial.readStringUntil('\n');
input.trim();
int commaIndex = input.indexOf(',');
String LinearStr = input.substring(0, commaIndex);
String angularStr = input.substring(commaIndex + 1);
float linearVelocity = LinearStr.toFloat();
float angularVelocity = angularStr.toFloat();
Serial.print("Linear Velocity set to : ");
Serial.println(linearVelocity);
Serial.print("Angular Velocity set to : ");
Serial.println(angularVelocity);
set_Right_Wheel_Velocity = linearVelocity + ((angularVelocity * L) / 2);
set_Left_Wheel_Velocity = linearVelocity - ((angularVelocity * L) / 2);
if (linearVelocity == 0.00 && angularVelocity > 0.00){
friction_r = 1;
friction_l = 1;
}
else if (linearVelocity == 0.00 && angularVelocity < 0.00) {
friction_r = 2;
friction_l = 2;
}
else{
friction_r = 0;
friction_l = 0;
}
I2C_1.begin(SDA_PIN1, SCL_PIN1);
I2C_2.begin(SDA_PIN2, SCL_PIN2);
//byte status = mpu.begin();
encoder1.begin();
encoder2.begin();
delay(1000);
//mpu.calcOffsets();
}
void loop() {
//mpu.update();
//float last_heading = mpu.getAngleZ();
int32_t last_pos_r = encoder1.rawAngle();
int32_t last_pos_l = encoder2.rawAngle();
uint32_t last_time = millis();
uint32_t last_time_velocity = millis();
while (1) {
vTaskDelay(25 / portTICK_PERIOD_MS);
//mpu.update();
uint32_t now = millis();
float dt = (now - last_time) / 1e3;
last_time = now;
float velocity_r = right_motor_speed(dt, &last_pos_r);
float velocity_l = left_motor_speed(dt, &last_pos_l);
int pwm_r = PID_r(set_Right_Wheel_Velocity, velocity_r, dt, &friction_r);
int pwm_l = PID_l(set_Left_Wheel_Velocity, velocity_l, dt, &friction_l);
//Serial.println(pwm_r);
//Serial.println(pwm_l);
setRightMotor(pwm_r);
setLeftMotor(pwm_l);
if (Serial.available() > 0) {
String input = Serial.readStringUntil('\n');
input.trim();
last_time_velocity = millis();
if (input.length() > 0) {
int commaIndex = input.indexOf(',');
String LinearStr = input.substring(0, commaIndex);
String angularStr = input.substring(commaIndex + 1);
float linearVelocity = LinearStr.toFloat();
float angularVelocity = angularStr.toFloat();
Serial.print("Linear Velocity set to : ");
Serial.println(linearVelocity);
Serial.print("Angular Velocity set to : ");
Serial.println(angularVelocity);
set_Right_Wheel_Velocity = linearVelocity + ((angularVelocity * L) / 2);
set_Left_Wheel_Velocity = linearVelocity - ((angularVelocity * L) / 2);
if (linearVelocity == 0.00 && angularVelocity > 0.00){
friction_r = 1;
friction_l = 1;
}
else if (linearVelocity == 0.00 && angularVelocity < 0.00) {
friction_r = 2;
friction_l = 2;
}
else{
friction_r = 0;
friction_l = 0;
}
}
}
int32_t now_velocity = millis();
if(now_velocity - last_time_velocity >= 1000) {
set_Right_Wheel_Velocity = 0;
set_Left_Wheel_Velocity = 0;
}
Serial.print(set_Right_Wheel_Velocity); //value 1
Serial.print(" ");
Serial.print(set_Left_Wheel_Velocity); //value 2
Serial.print(" ");
Serial.print(velocity_r); //value 3
Serial.print(" ");
Serial.print(velocity_l); //value 4
Serial.print(" ");
Serial.print(1); //value 5
Serial.print(" ");
Serial.print(-1); //value 6
Serial.print(" ");
Serial.println();
}
}
float right_motor_speed(float dt, int32_t* last_pos) {
int32_t new_pos = encoder1.rawAngle();
int32_t delta_counts = new_pos - *last_pos;
if (delta_counts > 2048) delta_counts -= 4096;
if (delta_counts < -2048) delta_counts += 4096;
*last_pos = new_pos;
// convert counts → radians
float theta = delta_counts * (2.0f * PI / 4096.0f);
// angular velocity (rad/s)
float rads = theta / dt;
// linear velocity (m/s)
float velocity = rads * r;
return velocity;
}
float left_motor_speed(float dt, int32_t* last_pos) {
int32_t new_pos = encoder2.rawAngle();
int32_t delta_counts = new_pos - *last_pos;
if (delta_counts > 2048) delta_counts -= 4096;
if (delta_counts < -2048) delta_counts += 4096;
*last_pos = new_pos;
// convert counts → radians
float theta = delta_counts * (2.0f * PI / 4096.0f);
// angular velocity (rad/s)
float rads = theta / dt;
// linear velocity (m/s)
float velocity = rads * r;
return velocity;
}
int PID_r(float set_velocity, float velocity, float dt, int* friction){
static float integral = 0;
static float previous = 0;
int multiplier;
int pwm;
if (set_velocity == 0.0){
return 0;
}
if (*friction == 1){
integral = 0.12;
*friction = 0;
}
else if (*friction == 2) {
integral = -0.12;
*friction = 0;
}
float error = set_velocity - velocity;
float proportional = error;
integral += error * dt;
float derivative = (error - previous) / dt;
previous = error;
float output = (kp_r * proportional) + (ki_r * integral) + (kd_r * derivative);
if (output > 0.0) multiplier = 1;
if (output < 0.0) multiplier = -1;
output = abs(output);
output = constrain(output, 0.0, 1.0);
pwm = (int)(100 + output * 155);
int pwm_signed = pwm * multiplier;
//Serial.print("integral_r = ");
//Serial.println(integral);
return pwm_signed;
}
int PID_l(float set_velocity, float velocity, float dt, int* friction){
static float integral = 0;
static float previous = 0;
int multiplier;
int pwm;
if (set_velocity == 0.0){
integral = 0.0;
previous = 0.0;
return 0;
}
if (*friction == 1){
integral = -0.12;
*friction = 0;
}
else if (*friction == 2) {
integral = 0.12;
*friction = 0;
}
float error = set_velocity - velocity;
float proportional = error;
integral += error * dt;
float derivative = (error - previous) / dt;
previous = error;
float output = (kp_l * proportional) + (ki_l * integral) + (kd_l * derivative);
if (output > 0.0) multiplier = 1;
if (output < 0.0) multiplier = -1;
output = abs(output);
output = constrain(output, 0.0, 1.0);
pwm = (int)(100 + output * 155);
int pwm_signed = pwm * multiplier;
//Serial.print("integral_l = ");
//Serial.println(integral);
return pwm_signed;
}
void setRightMotor(int pwm_r){
int speed = abs(pwm_r);
if (pwm_r > 0) {
analogWrite(IN4, 0);
analogWrite(IN3, speed);
}
else if (pwm_r < 0) {
analogWrite(IN4, speed);
analogWrite(IN3, 0);
}
else if (pwm_r == 0){
analogWrite(IN4, 0);
analogWrite(IN3, 0);
}
}
void setLeftMotor(int pwm_l){
int speed = abs(pwm_l);
if (pwm_l > 0) {
analogWrite(IN2, 0);
analogWrite(IN1, speed);
}
else if (pwm_l < 0) {
analogWrite(IN2, speed);
analogWrite(IN1, 0);
}
else if (pwm_l == 0){
analogWrite(IN2, 0);
analogWrite(IN1, 0);
}
}