23 #include "controller_kni.h"
25 #include "exception.h"
27 #include <common/MathHelperFunctions.h>
28 #include <core/exceptions/software.h>
42 : cfg_device_(
"/dev/ttyS0"), cfg_kni_conffile_(
"/etc/kni3/hd300/katana6M180.cfg")
44 cfg_read_timeout_ = 100;
45 cfg_write_timeout_ = 0;
47 gripper_last_pos_.clear();
48 gripper_last_pos_.resize(2);
51 phi_ = theta_ = psi_ = 0.;
72 std::string &kni_conffile,
73 unsigned int read_timeout,
74 unsigned int write_timeout)
77 cfg_kni_conffile_ = kni_conffile;
78 cfg_read_timeout_ = read_timeout;
79 cfg_write_timeout_ = write_timeout;
86 TCdlCOMDesc ccd = {0, 57600, 8,
'N', 1, (int)cfg_read_timeout_, (
int)cfg_write_timeout_};
87 device_.reset(
new CCdlCOM(ccd, cfg_device_.c_str()));
89 protocol_.reset(
new CCplSerialCRC());
90 protocol_->init(device_.get());
93 katana_->create(cfg_kni_conffile_.c_str(), protocol_.get());
94 katbase_ = katana_->GetBase();
95 sensor_ctrl_ = &katbase_->GetSCT()->arr[0];
103 motor_init_.resize(katana_->getNumberOfMotors());
104 for (
unsigned int i = 0; i < motor_init_.size(); i++) {
105 motor_init_.at(i) = *(katbase_->GetMOT()->arr[i].GetInitialParameters());
107 }
catch ( ::Exception &e) {
116 katana_->setRobotVelocityLimit((
int)vel);
117 }
catch ( ::Exception &e) {
125 if (active_motors_.size() < 1)
129 for (
unsigned int i = 0; i < active_motors_.size(); i++) {
130 final &= motor_final(active_motors_.at(i));
132 cleanup_active_motors();
151 katana_->calibrate();
161 katana_->freezeRobot();
162 }
catch ( ::Exception &e) {
171 katana_->switchRobotOn();
172 }
catch ( ::Exception &e) {
181 katana_->switchRobotOff();
182 }
catch ( ::Exception &e) {
191 katana_->getCoordinates(x_, y_, z_, phi_, theta_, psi_, refresh);
192 }
catch ( ::Exception &e) {
201 if (active_motors_.size() == (
unsigned short)katana_->getNumberOfMotors()) {
205 const TKatMOT *mot = katbase_->GetMOT();
206 for (
unsigned int i = 0; i < active_motors_.size(); i++) {
207 mot->arr[active_motors_.at(i)].recvPVP();
210 }
catch ( ::Exception &e) {
219 sensor_ctrl_->recvDAT();
220 }
catch ( ::ParameterReadingException &e) {
231 katana_->openGripper(blocking);
232 }
catch ( ::Exception &e) {
236 active_motors_.clear();
237 active_motors_.resize(1);
238 active_motors_[0] = katbase_->GetMOT()->cnt - 1;
240 gripper_last_pos_.clear();
241 gripper_last_pos_[0] = katbase_->GetMOT()->arr[active_motors_[0]].GetPVP()->pos;
242 gripper_last_pos_[1] = 0;
249 katana_->closeGripper(blocking);
250 }
catch ( ::Exception &e) {
254 active_motors_.clear();
255 active_motors_.resize(1);
256 active_motors_[0] = katbase_->GetMOT()->cnt - 1;
258 gripper_last_pos_.clear();
259 gripper_last_pos_[0] = katbase_->GetMOT()->arr[active_motors_[0]].GetPVP()->pos;
260 gripper_last_pos_[1] = 0;
272 cleanup_active_motors();
275 katana_->moveRobotTo(x_, y_, z_, phi_, theta_, psi_, blocking);
276 }
catch (KNI::NoSolutionException &e) {
282 for (
short i = 0; i < katana_->getNumberOfMotors(); ++i) {
290 cleanup_active_motors();
293 katana_->moveRobotToEnc(encoders);
298 for (
unsigned short i = 0; i < encoders.size(); ++i) {
306 std::vector<int> encoders;
309 for (
unsigned int i = 0; i < angles.size(); i++) {
310 encoders.push_back(KNI_MHF::rad2enc((
double)angles.at(i),
311 motor_init_.at(i).angleOffset,
312 motor_init_.at(i).encodersPerCycle,
313 motor_init_.at(i).encoderOffset,
314 motor_init_.at(i).rotationDirection));
316 }
catch ( ::Exception &e) {
329 cleanup_active_motors();
332 katana_->moveMotorToEnc(
id, enc);
337 add_active_motor(
id);
346 cleanup_active_motors();
349 katana_->moveMotorTo(
id, angle);
354 add_active_motor(
id);
363 cleanup_active_motors();
366 katana_->moveMotorByEnc(
id, enc);
371 add_active_motor(
id);
380 cleanup_active_motors();
383 katana_->moveMotorBy(
id, angle);
388 add_active_motor(
id);
435 const TSctDAT *sensor_data = sensor_ctrl_->GetDAT();
437 const int num_sensors = (size_t)sensor_data->cnt;
439 to.resize(num_sensors);
441 for (
int i = 0; i < num_sensors; ++i) {
442 to[i] = sensor_data->arr[i];
444 }
catch ( ::Exception &e) {
453 std::vector<int> encoders = katana_->getRobotEncoders(refresh);
466 std::vector<int> encoders = katana_->getRobotEncoders(refresh);
469 for (
unsigned int i = 0; i < encoders.size(); i++) {
470 to.push_back(KNI_MHF::enc2rad(encoders.at(i),
471 motor_init_.at(i).angleOffset,
472 motor_init_.at(i).encodersPerCycle,
473 motor_init_.at(i).encoderOffset,
474 motor_init_.at(i).rotationDirection));
476 }
catch ( ::Exception &e) {
482 KatanaControllerKni::motor_oor(
unsigned short id)
484 return id > (
unsigned short)katana_->getNumberOfMotors();
488 KatanaControllerKni::motor_final(
unsigned short id)
490 CMotBase mot = katbase_->GetMOT()->arr[id];
491 if (mot.GetPVP()->msf == MSF_MOTCRASHED)
495 unsigned short gripper_not_moved = 0;
496 if (
id == katbase_->GetMOT()->cnt - 1) {
497 if (gripper_last_pos_[0] == mot.GetPVP()->pos) {
498 gripper_last_pos_[1] += 1;
500 gripper_last_pos_[0] = mot.GetPVP()->pos;
501 gripper_last_pos_[1] = 0;
503 gripper_not_moved = gripper_last_pos_[1];
506 return (std::abs(mot.GetTPS()->tarpos - mot.GetPVP()->pos) < 10) or (gripper_not_moved > 3);
510 KatanaControllerKni::cleanup_active_motors()
512 for (
unsigned int i = 0; i < active_motors_.size(); ++i) {
513 if (motor_final(active_motors_.at(i))) {
514 active_motors_.erase(active_motors_.begin() + i);
521 KatanaControllerKni::add_active_motor(
unsigned short id)
523 for (
unsigned int i = 0; i < active_motors_.size(); ++i) {
524 if (active_motors_.at(i) ==
id) {
528 active_motors_.push_back(
id);