So i was trying to run Dynamixel Xm430 motor with Operating mode position via DynamixelDriver component.
Upon understanding the code it was clear that there is no implementation for Operating mode : KPosition.
I had to write the code for controlling the motor via Position mode and here is the code
bool DynamixelDriver::writeCommand(TensorConstView1d command) {
// Send command to motors
DynamixelMode mode = get_control_mode();
LOG_DEBUG(" Dynamixel Mode is %d ",mode );
if(mode == DynamixelMode::kVelocity){
double max_speed = get_max_speed();
if (max_speed < 0.0) {
reportFailure("Invalid maximum speed (%f) - sending zero speeds.", max_speed);
LOG_DEBUG("Invalid maximum speed (%f) - sending zero speeds.", max_speed);
max_speed = 0.0;
}
bool success = true;
for (size_t i = 0; i < servo_ids_.size(); i++) {
const int servo_id = servo_ids_[i];
// Get and clamp speed
double servo_speed = command(i); //Return speed of servo at index i
LOG_DEBUG(" Servo Speed : %f and command(%d) is %f",servo_speed,i,command(i));
if (std::abs(servo_speed) > max_speed) {
servo_speed = std::copysign(max_speed, servo_speed);
}
// Write to motor
LOG_DEBUG("Running in Velocity Mode. %f" , servo_speed);
const bool failed = dynamixel_->writeRegister(servo_id, RegisterKey::MOVING_SPEED,
dynamixel_->getAngularSpeedToTicks(servo_speed));
success &= failed;
// Show with sight
const std::string key = "motor_" + std::to_string(i + 1);
show(key + ".command", command(i));
show(key + ".fail", failed ? 1 : 0);
}
// if a write failed, check the error and disable the dynamixels if necessary
if (!success) {
const byte status = dynamixel_->getStatus();
const bool serious_error =
(status & kStatusOverheating) || (status & kStatusOverloaded);
if (serious_error) {
reportFailure("Dynamixel serious error detected; disabling servos");
return false;
}
// do not log other errors, since the dynamixel API has already logged the error
}
return true;
}
else if(mode == DynamixelMode::kPosition){
LOG_DEBUG(" Dynamixel Mode is %d ",mode );
bool success = true;
for (size_t i = 0; i < servo_ids_.size(); i++) {
const int servo_id = servo_ids_[i];
LOG_DEBUG(" VALUE OF COMMAND IS %i ", command(i));
int current_position = dynamixel_->readRegister(servo_id, RegisterKey::CURRENT_POSITION);
LOG_DEBUG("CURRENT POSITION VALUE %d", current_position);
unsigned int goal_position = command(i); //Value between 0 - 4095
//Write to motor
LOG_DEBUG("Running in Position Mode.");
const bool failed = dynamixel_->writeRegister(servo_id, RegisterKey::GOAL_POSITION,
dynamixel_->getAngleToTicks(goal_position));
success &= failed;
// Show with sight
const std::string key = "motor_" + std::to_string(i + 1);
show(key + ".command", command(i));
show(key + ".fail", failed ? 1 : 0);
}
// if a write failed, check the error and disable the dynamixels if necessary
if (!success) {
const byte status = dynamixel_->getStatus();
const bool serious_error =
(status & kStatusOverheating) || (status & kStatusOverloaded);
if (serious_error) {
reportFailure("Dynamixel serious error detected; disabling servos");
return false;
}
// do not log other errors, since the dynamixel API has already logged the error
}
return true;
}
else {
LOG_ERROR(" Unknown Operation mode Selected");
reportFailure(" Dynamixel Unkown Mode Error");
return false;
}
}
I was able to move the motor and had plugged DEBUG logs various places to see the content of what is happening where.
This is the log i got.
2021-02-02 15:07:34.576 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@196: Dynamixel Mode is 0
2021-02-02 15:07:34.576 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@243: Dynamixel Mode is 0
2021-02-02 15:07:34.576 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@249: VALUE OF COMMAND IS -1894479680
2021-02-02 15:07:34.584 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@251: CURRENT POSITION VALUE 722
2021-02-02 15:07:34.584 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@256: Running in Position Mode.
2021-02-02 15:07:34.600 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@308: INSIDE _______________________________--
2021-02-02 15:07:34.616 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@314: Current Position Back 686
2021-02-02 15:07:34.616 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@316: Actual Position -1894479680
2021-02-02 15:07:34.776 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@196: Dynamixel Mode is 0
2021-02-02 15:07:34.776 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@243: Dynamixel Mode is 0
2021-02-02 15:07:34.776 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@249: VALUE OF COMMAND IS -1894479680
2021-02-02 15:07:34.792 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@251: CURRENT POSITION VALUE 851
2021-02-02 15:07:34.792 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@256: Running in Position Mode.
2021-02-02 15:07:34.808 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@308: INSIDE _______________________________--
2021-02-02 15:07:34.824 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@314: Current Position Back 898
2021-02-02 15:07:34.824 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@316: Actual Position -1894479680
2021-02-02 15:07:34.976 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@196: Dynamixel Mode is 0
2021-02-02 15:07:34.976 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@243: Dynamixel Mode is 0
2021-02-02 15:07:34.976 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@249: VALUE OF COMMAND IS -1894479680
^C2021-02-02 15:07:34.984 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@251: CURRENT POSITION VALUE 1161
2021-02-02 15:07:34.984 DEBUG packages/dynamixel/components/DynamixelDriver.cpp@256: Running in Position Mode.
There are values which i do not understand like these -1894479680
I know Position values can be between 0- 4095.
Can anyone help me understand the content of what is happening and where i am going wrong.