void Irp6otmM2J::updateHook() {
if (RTT::NewData == port_desired_motor_position_.read(motor_position_)) {
mp2i(&motor_position_(0), &joint_position_(0));
port_joint_position_.write(joint_position_);
desired_motor_pos_initiated_ = true;
} else if ((!desired_motor_pos_initiated_)
&& (RTT::NewData == port_motor_position_.read(motor_position_))) {
mp2i(&motor_position_(0), &joint_position_(0));
port_joint_position_.write(joint_position_);
}
}
bool IRP6pServo::startHook()
{
try
{
for (unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
hi_.insertSetValue(i, 0);
hi_.readWriteHardware();
if (hi_.isRobotSynchronized())
{
for (unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
motor_pos_old_[i] = ((double)hi_.getPosition(i) / (ENC_RES[i]/(2*M_PI)));
if(!checkMotorPosition(motor_pos_old_))
std::cout << "current motor state out of range !!!" << std::endl;
mp2i(motor_pos_old_, joint_pos_);
for (unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
{
dsrJntPos_[i] = joint_pos_[i];
}
cmdJntPos_ = dsrJntPos_;
state_ = SERVOING;
}
else
{
std::cout << "robot not synchronized" << std::endl;
if(autoSynchronize_)
{
std::cout << "synchronizing .. " << std::endl;
synchro_state_ = MOVE_TO_SYNCHRO_AREA;
state_ = SYNCHRONIZING;
synchro_drive_ = 0;
} else
{
state_ = NOT_SYNCHRONIZED;
}
}
}
catch (const std::exception& e)
{
std::cout << e.what() << std::endl;
return false;
}
for(unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
{
reg_[i].reset();
pos_inc_[i] = 0.0;
}
return true;
}
void SarkofagM2J::updateHook() {
if (RTT::NewData == port_motor_position_.read(motor_position_)) {
mp2i(&motor_position_(0), &joint_position_(0));
port_joint_position_.write(joint_position_);
}
}
void IRP6pServo::updateHook()
{
double motor_pos[NUMBER_OF_DRIVES];
double pwm[NUMBER_OF_DRIVES];
switch (state_)
{
case NOT_SYNCHRONIZED :
for (unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
pos_inc_[i] = 0.0;
break;
case SERVOING :
if (dsrJntPos_port_.read(dsrJntPos_) == RTT::NewData)
{
double joint_pos_new[NUMBER_OF_DRIVES];
double motor_pos_new[NUMBER_OF_DRIVES];
for(unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
joint_pos_new[i] = dsrJntPos_[i];
if(i2mp(joint_pos_new, motor_pos_new))
{
for(unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
{
pos_inc_[i] = (motor_pos_new[i] - motor_pos_old_[i]) * ((double)ENC_RES[i]/(2*M_PI));
motor_pos_old_[i] = motor_pos_new[i];
}
} else
{
std::cout << "setpoint out of motor range !!! " << std::endl;
for(unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
pos_inc_[i] = 0.0;
}
cmdJntPos_ = dsrJntPos_;
}
else
{
for (unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
pos_inc_[i] = 0.0;
}
break;
case SYNCHRONIZING :
switch (synchro_state_)
{
case MOVE_TO_SYNCHRO_AREA :
if (hi_.isInSynchroArea(synchro_drive_))
{
std::cout << "[servo " << synchro_drive_ << " ] MOVE_TO_SYNCHRO_AREA cmp" << std::endl;
pos_inc_[synchro_drive_] = 0.0;
delay_ = 500;
synchro_state_ = STOP;
std::cout << "[servo " << synchro_drive_ << " ] STOP start" << std::endl;
}
else
{
pos_inc_[synchro_drive_] = SYNCHRO_STEP_COARSE[synchro_drive_] * (ENC_RES[synchro_drive_]/(2*M_PI));
}
break;
case STOP :
if(delay_-- < 0)
{
std::cout << "[servo " << synchro_drive_ << " ] STOP cmp" << std::endl;
synchro_state_ = MOVE_FROM_SYNCHRO_AREA;
hi_.startSynchro(synchro_drive_);
std::cout << "[servo " << synchro_drive_ << " ] MOVE_FROM_SYNCHRO_AREA start" << std::endl;
}
break;
case MOVE_FROM_SYNCHRO_AREA :
if (hi_.isImpulseZero(synchro_drive_) && isInSynchroArea(synchro_drive_))
{
std::cout << "[servo " << synchro_drive_ << " ] MOVE_FROM_SYNCHRO_AREA cmp " << std::endl;
hi_.finishSynchro(synchro_drive_);
hi_.resetPosition(synchro_drive_);
reg_[synchro_drive_].reset();
pos_inc_[synchro_drive_] = 0.0;
motor_pos_old_[synchro_drive_] = ((double)hi_.getPosition(synchro_drive_) / (ENC_RES[synchro_drive_]/(2*M_PI)));
if(++synchro_drive_ < NUMBER_OF_DRIVES)
{
synchro_state_ = MOVE_TO_SYNCHRO_AREA;
} else
{
synchro_state_ = SYNCHRO_END;
}
}
else
{
pos_inc_[synchro_drive_] = SYNCHRO_STEP_FINE[synchro_drive_] * (ENC_RES[synchro_drive_]/(2*M_PI));
}
break;
case SYNCHRO_END:
if(!checkMotorPosition(motor_pos_old_))
std::cout << "current motor state out of range !!!" << std::endl;
mp2i(motor_pos_old_, joint_pos_);
for (unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
{
dsrJntPos_[i] = joint_pos_[i];
}
state_ = SERVOING;
break;
}
break;
}
for(unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
{
int64_t increment = hi_.getIncrement(i);
if(abs(increment) > 400)
{
std::cout << "!!!! increment > 400 !!!!" << std::endl;
increment = 0;
}
if(fabs(pos_inc_[i]) > 400)
{
std::cout << "!!!! pos_inc > 400 !!!!" << std::endl;
pos_inc_[i] = 0;
}
pwm[i] = reg_[i].doServo(pos_inc_[i], increment);
}
try
{
for(unsigned int i = 0; i < 6; i++)
hi_.insertSetValue(i, pwm[i]);
hi_.readWriteHardware();
}
catch (const std::exception& e)
{
std::cout << e.what() << std::endl;
}
if(state_ == SERVOING)
{
for (unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
motor_pos[i] = ((double)hi_.getPosition(i) / (ENC_RES[i]/(2*M_PI)));
if(!checkMotorPosition(motor_pos))
std::cout << "current motor state out of range !!!" << std::endl;
mp2i(motor_pos, joint_pos_);
for (unsigned int i = 0; i < NUMBER_OF_DRIVES; i++)
{
msrJntPos_[i] = joint_pos_[i];
}
msrJntPos_port_.write(msrJntPos_);
cmdJntPos_port_.write(cmdJntPos_);
}
}
void Irp6otmM2J::updateHook() {
port_motor_position_.read(motor_position_);
mp2i(&motor_position_(0), &joint_position_(0));
port_joint_position_.write(joint_position_);
}
