bool SarkofagJ2M::i2mp(const double* joints, double* motors)
{
// Obliczanie kata obrotu walu silnika napedowego kolumny
motors[0] = GEAR[0] * joints[0] + SYNCHRO_JOINT_POSITION[0];
return checkMotorPosition(motors);
}
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;
}
bool Irp6otmJ2M::i2mp(const double* joints, double* motors) {
// Niejednoznacznosc polozenia dla 4-tej osi (obrot kisci < 180).
const double joint_4_revolution = M_PI;
// Niejednoznacznosc polozenia dla 5-tej osi (obrot kisci > 360).
const double axis_5_revolution = 2 * M_PI;
// Obliczanie kata obrotu walu silnika napedowego toru
motors[0] = GEAR[0] * joints[0] + SYNCHRO_JOINT_POSITION[0];
// Obliczanie kata obrotu walu silnika napedowego kolumny
motors[1] = GEAR[1] * joints[1] + SYNCHRO_JOINT_POSITION[1];
// Obliczanie kata obrotu walu silnika napedowego ramienia dolnego
motors[2] = GEAR[2]
* sqrt(sl123 + mi2 * cos(joints[2]) + ni2 * sin(-joints[2]))
+ SYNCHRO_JOINT_POSITION[2];
// Obliczanie kata obrotu walu silnika napedowego ramienia gornego
motors[3] = GEAR[3]
* sqrt(
sl123 + mi3 * cos(joints[3] + joints[2] + M_PI_2)
+ ni3 * sin(-(joints[3] + joints[2] + M_PI_2)))
+ SYNCHRO_JOINT_POSITION[3];
// Obliczanie kata obrotu walu silnika napedowego obotu kisci T
// jesli jest mniejsze od -pi/2
double joints_tmp4 = joints[4] + joints[3] + joints[2] + M_PI_2;
if (joints_tmp4 < -M_PI_2)
joints_tmp4 += joint_4_revolution;
motors[4] = GEAR[4] * (joints_tmp4 + THETA[4]) + SYNCHRO_JOINT_POSITION[4];
// Obliczanie kata obrotu walu silnika napedowego obrotu kisci V
motors[5] = GEAR[5] * joints[5] + SYNCHRO_JOINT_POSITION[5] + motors[4];
// Ograniczenie na obrot.
while (motors[5] < LOWER_MOTOR_LIMIT[5])
motors[5] += axis_5_revolution;
while (motors[5] > UPPER_MOTOR_LIMIT[5])
motors[5] -= axis_5_revolution;
// Obliczanie kata obrotu walu silnika napedowego obrotu kisci N
motors[6] = GEAR[6] * joints[6] + SYNCHRO_JOINT_POSITION[6];
return checkMotorPosition(motors);
}
bool IRP6pServo::i2mp(const double* joints, double* motors)
{
const double sl123 = 7.789525e+04;
const double mi1 = 6.090255e+04;
const double ni1 = -2.934668e+04;
const double mi2 = -4.410000e+04;
const double ni2 = -5.124000e+04;
// Niejednoznacznosc polozenia dla 3-tej osi (obrot kisci < 180).
const double joint_3_revolution = M_PI;
// Niejednoznacznosc polozenia dla 4-tej osi (obrot kisci > 360).
const double axis_4_revolution = 2 * M_PI;
// Obliczanie kata obrotu walu silnika napedowego kolumny
motors[0] = GEAR[0] * joints[0] + SYNCHRO_JOINT_POSITION[0];
// Obliczanie kata obrotu walu silnika napedowego ramienia dolnego
motors[1] = GEAR[1] * sqrt(sl123 + mi1 * cos(joints[1]) + ni1
* sin(-joints[1])) + SYNCHRO_JOINT_POSITION[1];
// Obliczanie kata obrotu walu silnika napedowego ramienia gornego
motors[2] = GEAR[2] * sqrt(sl123 + mi2 * cos(joints[2] + joints[1] + M_PI_2) + ni2
* sin(-(joints[2] + joints[1] + M_PI_2))) + SYNCHRO_JOINT_POSITION[2];
// Obliczanie kata obrotu walu silnika napedowego obotu kisci T
// jesli jest mniejsze od -pi/2
double joints_tmp3 = joints[3] + joints[2] + joints[1] + M_PI_2;
if (joints_tmp3 < -M_PI_2)
joints_tmp3 += joint_3_revolution;
motors[3] = GEAR[3] * (joints_tmp3 + THETA[3]) + SYNCHRO_JOINT_POSITION[3];
// Obliczanie kata obrotu walu silnika napedowego obrotu kisci V
motors[4] = GEAR[4] * joints[4] + SYNCHRO_JOINT_POSITION[4]
+ motors[3];
// Ograniczenie na obrot.
while (motors[4] < -80)
motors[4] += axis_4_revolution;
while (motors[4] > 490)
motors[4] -= axis_4_revolution;
// Obliczanie kata obrotu walu silnika napedowego obrotu kisci N
motors[5] = GEAR[5] * joints[5] + SYNCHRO_JOINT_POSITION[5];
return checkMotorPosition(motors);
}
void checkMotors()
{
// Syncronize this method (called every 5ms)
static bool polling = false;
if (polling) return;
polling = true;
// A total of 4 timers are available
for (byte i = 0; i < 4; i++) {
// Only interested in active timers
if (!motorCtrl[i].active) continue;
// Stop motor(s) and timer when time interval has been reached
checkMotorTimer(i);
// When using an encoder check if the specified position has been reached
checkMotorPosition(i);
}
polling = false;
}
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_);
}
}
bool Irp6tfgJ2M::i2mp(const double* joints, double* motors) {
// Obliczenie kata obrotu walu silnika napedowego chwytaka.
motors[0] = inv_a_7 * sqrt(inv_b_7 + inv_c_7 * joints[0]) + inv_d_7;
return checkMotorPosition(motors);
}
