Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Changed constrained velocity to match motor max better #322

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Changed constrained velocity to match motor max better #322

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions ...ncr_arduino/opencr/libraries/turtlebot3_ros2/include/turtlebot3/turtlebot3_motor_driver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ class Turtlebot3MotorDriver
bool read_present_velocity(int32_t &left_value, int32_t &right_value);
bool read_present_current(int16_t &left_value, int16_t &right_value);
bool read_profile_acceleration(uint32_t &left_value, uint32_t &right_value);
bool read_max_velocity(int16_t &left_value, int16_t &right_value);

bool write_velocity(int32_t left_value, int32_t right_value);
bool write_profile_acceleration(uint32_t left_value, uint32_t right_value);
Expand All @@ -64,6 +65,8 @@ class Turtlebot3MotorDriver
uint8_t left_wheel_id_;
uint8_t right_wheel_id_;
bool torque_;
int16_t right_velocity_limit_;
int16_t left_velocity_limit_;
};

#endif // TURTLEBOT3_MOTOR_DRIVER_H_
33 changes: 27 additions & 6 deletions ...pencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3_motor_driver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,10 +16,13 @@

#include "../../include/turtlebot3/turtlebot3_motor_driver.h"

// Limit values (XM430-W210-T and XM430-W350-T)
// MAX RPM is 77 when DXL is powered 12.0V
// Limit values (XM430-W210-T and XM430-250-T)
// Need to set for lower value since motor will ignore values past limit
// MAX RPM for W210 (Waffle) is 77 RPM @12.0V
// MAX RPM for W250 (Burger) is 61 RPM @12.0V
// 77 / 0.229 (RPM) = 336.24454...
const uint16_t LIMIT_X_MAX_VELOCITY = 337;
// 61 / 0.229 (RPM) = 266.3755...
const uint16_t LIMIT_X_MAX_VELOCITY = 265;
// V = r * w = r * (RPM * 0.10472)
// = 0.033 * (0.229 * Goal_Velocity) * 0.10472
// Goal_Velocity = V * 1263.632956882
Expand All @@ -42,7 +45,9 @@ Dynamixel2Arduino dxl(Serial3, OPENCR_DXL_DIR_PIN);
Turtlebot3MotorDriver::Turtlebot3MotorDriver()
: left_wheel_id_(DXL_MOTOR_ID_LEFT),
right_wheel_id_(DXL_MOTOR_ID_RIGHT),
torque_(false)
torque_(false),
right_velocity_limit_(LIMIT_X_MAX_VELOCITY),
left_velocity_limit_(LIMIT_X_MAX_VELOCITY)
{
}

Expand Down Expand Up @@ -73,6 +78,7 @@ bool Turtlebot3MotorDriver::init(void)

// Enable Dynamixel Torque
set_torque(true);
read_max_velocity(this->left_velocity_limit_, this->right_velocity_limit_);

return true;
}
Expand Down Expand Up @@ -169,7 +175,21 @@ bool Turtlebot3MotorDriver::read_present_current(int16_t &left_value, int16_t &r

return ret;
}
bool Turtlebot3MotorDriver::read_max_velocity(int16_t &left_value, int16_t &right_value)
{
bool ret = false;

sync_read_param.addr = 44;
sync_read_param.length = 4;

if(dxl.syncRead(sync_read_param, read_result)){
memcpy(&left_value, read_result.xel[LEFT].data, read_result.xel[LEFT].length);
memcpy(&right_value, read_result.xel[RIGHT].data, read_result.xel[RIGHT].length);
ret = true;
}

return ret;
}
bool Turtlebot3MotorDriver::read_profile_acceleration(uint32_t &left_value, uint32_t &right_value)
{
bool ret = false;
Expand Down Expand Up @@ -230,8 +250,9 @@ bool Turtlebot3MotorDriver::control_motors(const float wheel_separation, float l
wheel_velocity[LEFT] = lin_vel - (ang_vel * wheel_separation / 2);
wheel_velocity[RIGHT] = lin_vel + (ang_vel * wheel_separation / 2);

wheel_velocity[LEFT] = constrain(wheel_velocity[LEFT] * VELOCITY_CONSTANT_VALUE, -LIMIT_X_MAX_VELOCITY, LIMIT_X_MAX_VELOCITY);
wheel_velocity[RIGHT] = constrain(wheel_velocity[RIGHT] * VELOCITY_CONSTANT_VALUE, -LIMIT_X_MAX_VELOCITY, LIMIT_X_MAX_VELOCITY);

wheel_velocity[LEFT] = constrain(wheel_velocity[LEFT] * VELOCITY_CONSTANT_VALUE, (float)-this->left_velocity_limit_, (float) this->left_velocity_limit_);
wheel_velocity[RIGHT] = constrain(wheel_velocity[RIGHT] * VELOCITY_CONSTANT_VALUE, (float) -this->right_velocity_limit_, (float) this->right_velocity_limit_);

dxl_comm_result = write_velocity((int32_t)wheel_velocity[LEFT], (int32_t)wheel_velocity[RIGHT]);
if (dxl_comm_result == false)
Expand Down