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); }