$search
00001 /********************************************************************************** 00002 * Katana Native Interface - A C++ interface to the robot arm Katana. 00003 * Copyright (C) 2005-2008 Neuronics AG 00004 * Check out the AUTHORS file for detailed contact information. 00005 * This program is free software; you can redistribute it and/or modify 00006 * it under the terms of the GNU General Public License as published by 00007 * the Free Software Foundation; either version 2 of the License, or 00008 * (at your option) any later version. 00009 * This program is distributed in the hope that it will be useful, 00010 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 * GNU General Public License for more details. 00013 * You should have received a copy of the GNU General Public License 00014 * along with this program; if not, write to the Free Software 00015 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00016 **********************************************************************************/ 00018 // control.cpp 00019 // demo program for KNI libraries 00021 #include "kniBase.h" 00022 #include <iostream> 00023 #include <cstdio> 00024 #include <memory> 00025 #include <vector> 00026 #include <fstream> 00027 #include <pthread.h> 00029 #ifdef WIN32 00030 # include <conio.h> 00031 #else //LINUX 00032 # include "keyboard.h" 00033 #endif 00034 #define LEFT false 00035 #define RIGHT true 00036 00037 //Thread structs: 00038 pthread_mutex_t mutex; 00039 00040 struct TPoint { 00041 double X, Y, Z; 00042 double phi, theta, psi; 00043 }; 00044 00045 struct TCurrentMot { 00046 int idx; 00047 bool running; 00048 bool dir; 00049 }; 00050 00051 struct Tpos{ 00052 static std::vector<int> x,y,z,u,v,w; 00053 static const int xArr[], yArr[], zArr[], uArr[], vArr[], wArr[]; 00054 }; 00055 //Katana obj. 00056 std::auto_ptr<CLMBase> katana; 00057 //std::vector<int> Foo::vec(array, array + sizeof(array)/sizeof(*array)); 00058 //positionen, hard-coded. Use values from file instead 00059 const int Tpos::xArr[] = {30206, -23393, -3066, 14454, 30000, 30000}; 00060 const int Tpos::yArr[] = {24327, -7837, -16796, 5803, 30290, 31000}; 00061 const int Tpos::zArr[] = {24327, -7837, -16796, 5802, 30290, 10924}; 00062 const int Tpos::uArr[] = {5333, -13791, -9985, 11449, 30996, 12063}; 00063 const int Tpos::vArr[] = {-3799, -5703, -11676, 8210, 30995, 12063}; 00064 const int Tpos::wArr[] = {-3799, -5703, -11676, 8210, 30995, 30992}; 00065 std::vector<int> Tpos::x(xArr, xArr + sizeof(xArr)/sizeof(*xArr)); 00066 std::vector<int> Tpos::y(yArr, yArr + sizeof(yArr)/sizeof(*yArr)); 00067 std::vector<int> Tpos::z(zArr, zArr + sizeof(zArr)/sizeof(*zArr)); 00068 std::vector<int> Tpos::u(uArr, uArr + sizeof(uArr)/sizeof(*uArr)); 00069 std::vector<int> Tpos::v(vArr, vArr + sizeof(vArr)/sizeof(*vArr)); 00070 std::vector<int> Tpos::w(wArr, wArr + sizeof(wArr)/sizeof(*wArr)); 00071 std::vector<TPoint> points(0); 00072 void StartPointlistMovement(); 00073 void StartProgram(int index); 00074 pthread_t tid; 00075 void* RunProgram(void*); 00076 pid_t threadPid; 00077 int retVal = 0; 00078 bool progRunning = false; 00079 const double PI = 3.14159265358979323846; 00081 void DisplayHelp() { 00082 std::cout << "------------------------------------------- \n"; 00083 std::cout << "?: Display this help\n"; 00084 std::cout << "c: Calibrate the Katana\n"; 00085 std::cout << "b: Read current controller types\n"; 00086 std::cout << "e: Read the current encoder values\n"; 00087 std::cout << "E: Read the current force values\n"; 00088 std::cout << "o: Switch motors off/on (Default: On)\n"; 00089 std::cout << "r: Switch angle format: Radian/Degree (Default: Rad)\n"; 00090 std::cout << "x: Read the current position\n"; 00091 std::cout << "v: Set the velocity limits for all motors seperately\n"; 00092 std::cout << "V: Set the velocity limits for all motors (or for the TCP if in linear movement mode)\n"; 00093 std::cout << "a: Set the acceleration limits for all motors seperately\n"; 00094 std::cout << "A: Set the acceleration limits for all motors\n"; 00095 std::cout << ",: Set the force limits for all motors\n"; 00096 std::cout << "w: Read the velocity limits of all motors \n"; 00097 std::cout << "W: Read the acceleration limits of all motors \n"; 00098 std::cout << "q: Read the Sensors\n"; 00099 std::cout << "y: Set a new position using IK\n"; 00100 std::cout << "l: Switch on/off linear movements\n"; 00101 std::cout << "<: Add a point to the point list\n"; 00102 std::cout << ">: Move to a specific point\n"; 00103 std::cout << " : (space) Move to the next point in the point list\n"; 00104 std::cout << "=: write pointlist to file\n"; 00105 std::cout << "(: calculate DK from any encoders\n"; 00106 std::cout << "f: read pointlist from file\n"; 00107 std::cout << "g: Open Gripper\n"; 00108 std::cout << "h: Close Gripper\n"; 00109 std::cout << "n: Set the speed collision limit for all motors seperately\n"; 00110 std::cout << "N: Set the speed collision limit for all motors\n"; 00111 std::cout << "s: Set the position collision limit for all motors seperately\n"; 00112 std::cout << "S: Set the position collision limit for all motors\n"; 00113 std::cout << "t: Switch collision limit on\n"; 00114 std::cout << "T: Switch collision limit off\n"; 00115 std::cout << "u: Unblock motors after crash\n"; 00116 std::cout << "d: Move motor to degrees\n"; 00117 std::cout << "z: Set TCP offset\n\n\n"; 00118 std::cout << "Keyboard of Katana, use the following keys:\n\n"; 00119 std::cout << "1: Move motor1 left\n"; 00120 std::cout << "2: Move motor1 right\n"; 00121 std::cout << "3: Move motor2 left\n"; 00122 std::cout << "4: Move motor2 right\n"; 00123 std::cout << "5: Move motor3 left\n"; 00124 std::cout << "6: Move motor3 right\n"; 00125 std::cout << "7: Move motor4 left\n"; 00126 std::cout << "8: Move motor4 right\n"; 00127 std::cout << "9: Move motor5 left\n"; 00128 std::cout << "0: Move motor5 right\n"; 00129 std::cout << "/: Move motor6 left\n"; 00130 std::cout << "*: Move motor6 right\n"; 00131 std::cout << ".: Toggle Step mode\n"; 00132 std::cout << "+: Increase step size\n"; 00133 std::cout << "-: Decrease step size\n\n"; 00134 std::cout << "$: Start/Stop Program\n"; 00135 std::cout << "p: Start/Stop movement through points list\n\n"; 00136 } 00138 int main(int argc, char *argv[]) { 00139 00140 if (argc != 3) { 00141 std::cout << "---------------------------------\n"; 00142 std::cout << "usage: control CONFIGFILE IP_ADDR\n"; 00143 std::cout << "---------------------------------\n"; 00144 return 0; 00145 } 00146 00147 std::cout << "--------------------\n"; 00148 std::cout << "CONTROL DEMO STARTED\n"; 00149 std::cout << "--------------------\n"; 00150 00151 //----------------------------------------------------------------// 00152 //open device: a serial port is opened in this case 00153 //----------------------------------------------------------------// 00154 00155 00156 std::auto_ptr<CCdlSocket> device; 00157 std::auto_ptr<CCplSerialCRC> protocol; 00158 00159 try { 00160 00161 int port = 5566; 00162 device.reset(new CCdlSocket(argv[2], port)); 00163 00164 std::cout << "-------------------------------------------\n"; 00165 std::cout << "success: port " << port << " open\n"; 00166 std::cout << "-------------------------------------------\n"; 00167 00168 //--------------------------------------------------------// 00169 //init protocol: 00170 //--------------------------------------------------------// 00171 00172 protocol.reset(new CCplSerialCRC()); 00173 std::cout << "-------------------------------------------\n"; 00174 std::cout << "success: protocol class instantiated\n"; 00175 std::cout << "-------------------------------------------\n"; 00176 protocol->init(device.get()); //fails if no response from Katana 00177 std::cout << "-------------------------------------------\n"; 00178 std::cout << "success: communication with Katana initialized\n"; 00179 std::cout << "-------------------------------------------\n"; 00180 00181 00182 //--------------------------------------------------------// 00183 //init robot: 00184 //--------------------------------------------------------// 00185 katana.reset(new CLMBase()); 00186 katana->create(argv[1], protocol.get()); 00187 00188 00189 } catch(Exception &e) { 00190 std::cout << "ERROR: " << e.message() << std::endl; 00191 return -1; 00192 } 00193 std::cout << "-------------------------------------------\n"; 00194 std::cout << "success: katana initialized\n"; 00195 std::cout << "-------------------------------------------\n"; 00196 00197 DisplayHelp(); 00198 00199 short counter = 0; 00200 bool loop = true; 00201 int pos = 0; 00202 bool DispInRad = true; 00203 bool IsOff = false; 00204 bool useLinearMode = false; 00205 double RadToDeg = 1.0; 00206 std::vector<int> encoders(katana->getNumberOfMotors(), 0); 00207 const TKatMOT* motors; 00208 TCurrentMot mot[6]; 00209 for (int i = 0; i< 6; i++){ 00210 mot[i].running = false; 00211 mot[i].idx = i; 00212 mot[i].dir = true; 00213 mot[i].dir = RIGHT; 00214 } 00215 bool stepMode = false; 00216 int stepSize = 100; 00217 bool runProgram = false; 00218 //pthread_mutex_init(&mutex, 0); 00219 //pthread_mutex_lock(&mutex); 00220 //pthread_mutex_unlock(&mutex); 00221 //set sensor controller values 00222 /*TSctDesc sctdesc[1] = {{15, 8, 16}}; //sctID,resol,count 00223 CSctBase* sctarr = new CSctBase[1]; //create sensor-ctrl class 00224 TKatSCT katsct = {1, sctarr, sctdesc}; //fill TKatSCT structure 00225 */ 00226 CSctBase* sensctrl = &katana->GetBase()->GetSCT()->arr[0]; 00227 int limit; 00228 00229 //set linear velocity to 60 00230 katana->setMaximumLinearVelocity(60); 00231 00232 while (loop) { 00233 double arr_pos[6]; 00234 00235 int input = _getch(); 00236 if(progRunning){ 00237 //Thread killen: 00238 progRunning = false; 00239 continue; 00240 } 00241 00242 try { 00243 00244 switch (input) { 00245 00246 /* case 'i': //VK_I: Test routine, connected P2P movement 00247 { 00248 double posX = points[counter].X; 00249 double posY = points[counter].Y; 00250 double posZ = points[counter].Z; 00251 double posPhi = points[counter].phi; 00252 double posTheta = points[counter].theta; 00253 double posPsi = points[counter].psi; 00254 katana->moveRobotTo(posX, posY, posZ, posPhi, posTheta, posPsi, false); 00255 counter++; 00256 counter = counter % ((short) points.size()); 00257 katana->movP2P(posX, posY, posZ, posPhi, posTheta, posPsi, points[counter].X, points[counter].Y, points[counter].Z, points[counter].phi, points[counter].theta, points[counter].psi, true, 20, true); 00258 counter++; 00259 counter = counter % ((short) points.size()); 00260 break; 00261 } 00262 */ 00263 case '(': 00264 { 00265 std::vector<double> pose_result(6, 0); 00266 std::vector<int> etc; 00267 etc.push_back(20000); 00268 etc.push_back(-20000); 00269 etc.push_back(-20000); 00270 etc.push_back(20000); 00271 etc.push_back(20000); 00272 etc.push_back(20000); 00273 katana->getCoordinatesFromEncoders(pose_result, etc); 00274 std::cout.precision(6); 00275 std::cout << "\n------------------------------------\n"; 00276 std::cout << "X: " << pose_result.at(0) << "\n"; 00277 std::cout << "Y: " << pose_result.at(1) << "\n"; 00278 std::cout << "Z: " << pose_result.at(2) << "\n"; 00279 std::cout << "phi: " << RadToDeg*pose_result.at(3) << "\n"; 00280 std::cout << "theta: " << RadToDeg*pose_result.at(4) << "\n"; 00281 std::cout << "psi: " << RadToDeg*pose_result.at(5) << "\n"; 00282 std::cout << "------------------------------------\n"; 00283 } 00284 break; 00285 00286 case '.': 00287 if(stepMode == false){ 00288 stepMode = true; 00289 std::cout << "Step Mode ON \n" << std::endl; 00290 } 00291 else{ 00292 stepMode = false; 00293 std::cout << "Step Mode OFF \n" << std::endl; 00294 } 00295 break; 00296 case '+': 00297 if(stepSize <= 3000){ 00298 stepSize += 300; 00299 std::cout << "Step Size = " << stepSize << std::endl; 00300 00301 } 00302 break; 00303 case '-': 00304 if(stepSize >= 330){ 00305 stepSize -= 300; 00306 std::cout << "Step Size = " << stepSize << std::endl; 00307 } 00308 break; 00309 case '1': 00310 if((stepMode == true) ||(mot[0].running == false || (mot[0].running == true && mot[0].dir == LEFT))){ 00311 mot[0].idx = 0; 00312 motors = katana->GetBase()->GetMOT(); 00313 pos = motors->arr[mot[0].idx].GetEncoderMinPos(); 00314 if(motors->arr[mot[0].idx].checkEncoderInRange(mot[0].idx)){ 00315 if(stepMode){ 00316 pos = katana->getMotorEncoders(0) + stepSize; 00317 } 00318 std::cout << "\nMoving to max encoder position " << pos << std::endl; 00319 katana->moveMotorToEnc(mot[0].idx, pos); 00320 mot[0].running = true; 00321 mot[0].dir = RIGHT; 00322 } 00323 else{ 00324 std::cout << "\nEncoder target value out of range! \n" << std::endl; 00325 } 00326 } 00327 else{ 00328 katana->freezeMotor(0); 00329 mot[0].running = false; 00330 } 00331 break; 00332 case '2': 00333 if((stepMode == true) ||(mot[0].running == false || (mot[0].running == true && mot[0].dir == RIGHT))){ 00334 mot[0].idx = 0; 00335 //motors = katana->GetBase()->GetMOT(); 00336 motors = katana->GetBase()->GetMOT(); 00337 pos = motors->arr[mot[0].idx].GetEncoderMaxPos(); 00338 if(motors->arr[mot[0].idx].checkEncoderInRange(mot[0].idx)){ 00339 if(stepMode){ 00340 pos = katana->getMotorEncoders(0) - stepSize; 00341 } 00342 std::cout << "\nMoving to max encoder position " << pos << std::endl; 00343 katana->moveMotorToEnc(mot[0].idx, pos); 00344 mot[0].running = true; 00345 mot[0].dir = LEFT; 00346 } 00347 else{ 00348 std::cout << "\nEncoder target value out of range! \n" << std::endl; 00349 } 00350 } 00351 else{ 00352 katana->freezeMotor(0); 00353 mot[0].running = false; 00354 } 00355 break; 00356 case '3': 00357 if((stepMode == true) ||(mot[1].running == false || (mot[1].running == true && mot[1].dir == LEFT))){ 00358 mot[1].idx = 1; 00359 motors = katana->GetBase()->GetMOT(); 00360 pos = motors->arr[mot[1].idx].GetEncoderMinPos(); 00361 if(motors->arr[mot[1].idx].checkEncoderInRange(mot[1].idx)){ 00362 if(stepMode){ 00363 pos = katana->getMotorEncoders(1) + stepSize; 00364 } 00365 std::cout << "\nMoving to max encoder position " << pos << std::endl; 00366 katana->moveMotorToEnc(mot[1].idx, pos); 00367 mot[1].running = true; 00368 mot[1].dir = RIGHT; 00369 } 00370 else{ 00371 std::cout << "\nEncoder target value out of range! \n" << std::endl; 00372 } 00373 } 00374 else{ 00375 katana->freezeMotor(1); 00376 mot[1].running = false; 00377 } 00378 break; 00379 case '4': 00380 if((stepMode == true) ||(mot[1].running == false || (mot[1].running == true && mot[1].dir == RIGHT))){ 00381 mot[1].idx = 1; 00382 //motors = katana->GetBase()->GetMOT(); 00383 motors = katana->GetBase()->GetMOT(); 00384 pos = motors->arr[mot[1].idx].GetEncoderMaxPos(); 00385 if(motors->arr[mot[1].idx].checkEncoderInRange(mot[1].idx)){ 00386 if(stepMode){ 00387 pos = katana->getMotorEncoders(1) - stepSize; 00388 } 00389 std::cout << "\nMoving to max encoder position " << pos << std::endl; 00390 katana->moveMotorToEnc(mot[1].idx, pos); 00391 mot[1].running = true; 00392 mot[1].dir = LEFT; 00393 } 00394 else{ 00395 std::cout << "\nEncoder target value out of range! \n" << std::endl; 00396 } 00397 } 00398 else{ 00399 katana->freezeMotor(1); 00400 mot[1].running = false; 00401 } 00402 break; 00403 case '5': 00404 if((stepMode == true) ||(mot[2].running == false || (mot[2].running == true && mot[2].dir == LEFT))){ 00405 mot[2].idx = 2; 00406 motors = katana->GetBase()->GetMOT(); 00407 pos = motors->arr[mot[2].idx].GetEncoderMinPos(); 00408 if(motors->arr[mot[2].idx].checkEncoderInRange(mot[2].idx)){ 00409 if(stepMode){ 00410 pos = katana->getMotorEncoders(2) + stepSize; 00411 } 00412 std::cout << "\nMoving to max encoder position " << pos << std::endl; 00413 katana->moveMotorToEnc(mot[2].idx, pos); 00414 mot[2].running = true; 00415 mot[2].dir = RIGHT; 00416 } 00417 else{ 00418 std::cout << "\nErunProgram =ncoder target value out of range! \n" << std::endl; 00419 } 00420 } 00421 else{ 00422 katana->freezeMotor(2); 00423 mot[2].running = false; 00424 } 00425 break; 00426 case '6': 00427 if((stepMode == true) ||(mot[2].running == false || (mot[2].running == true && mot[2].dir == RIGHT))){ 00428 mot[2].idx = 2; 00429 //motors = katana->GetBase()->GetMOT(); 00430 motors = katana->GetBase()->GetMOT(); 00431 pos = motors->arr[mot[2].idx].GetEncoderMaxPos(); 00432 if(motors->arr[mot[2].idx].checkEncoderInRange(mot[2].idx)){ 00433 if(stepMode){ 00434 pos = katana->getMotorEncoders(2) - stepSize; 00435 } 00436 std::cout << "\nMoving to max encoder position " << pos << std::endl; 00437 katana->moveMotorToEnc(mot[2].idx, pos); 00438 mot[2].running = true; 00439 mot[2].dir = LEFT; 00440 } 00441 else{ 00442 std::cout << "\nEncoder target value out of range! \n" << std::endl; 00443 } 00444 } 00445 else{ 00446 katana->freezeMotor(2); 00447 mot[2].running = false; 00448 } 00449 break; 00450 case '7': 00451 if((stepMode == true) ||(mot[3].running == false || (mot[3].running == true && mot[3].dir == LEFT))){ 00452 mot[3].idx = 3; 00453 motors = katana->GetBase()->GetMOT(); 00454 pos = motors->arr[mot[3].idx].GetEncoderMinPos(); 00455 if(motors->arr[mot[3].idx].checkEncoderInRange(mot[3].idx)){ 00456 if(stepMode){ 00457 pos = katana->getMotorEncoders(3) + stepSize; 00458 } 00459 std::cout << "\nMoving to max encoder position " << pos << std::endl; 00460 katana->moveMotorToEnc(mot[3].idx, pos); 00461 mot[3].running = true; 00462 mot[3].dir = RIGHT; 00463 } 00464 else{ 00465 std::cout << "\nEncoder target value out of range! \n" << std::endl; 00466 } 00467 } 00468 else{ 00469 katana->freezeMotor(3);runProgram = 00470 mot[3].running = false; 00471 } 00472 break; 00473 case '8': 00474 if((stepMode == true) ||(mot[3].running == false || (mot[3].running == true && mot[3].dir == RIGHT))){ 00475 mot[3].idx = 3; 00476 //motors = katana->GetBase()->GetMOT(); 00477 motors = katana->GetBase()->GetMOT(); 00478 pos = motors->arr[mot[3].idx].GetEncoderMaxPos(); 00479 if(motors->arr[mot[3].idx].checkEncoderInRange(mot[3].idx)){ 00480 if(stepMode){ 00481 pos = katana->getMotorEncoders(3) - stepSize; 00482 } 00483 std::cout << "\nMoving to max encoder position " << pos << std::endl; 00484 katana->moveMotorToEnc(mot[3].idx, pos); 00485 mot[3].running = true; 00486 mot[3].dir = LEFT; 00487 } 00488 else{ 00489 std::cout << "\nEncoder target value out of range! \n" << std::endl; 00490 } 00491 } 00492 else{ 00493 katana->freezeMotor(3); 00494 mot[3].running = false; 00495 } 00496 break; 00497 case '9': 00498 if((stepMode == true) ||(mot[4].running == false || (mot[4].running == true && mot[4].dir == LEFT))){ 00499 mot[4].idx = 4; 00500 motors = katana->GetBase()->GetMOT(); 00501 pos = motors->arr[mot[4].idx].GetEncoderMinPos(); 00502 if(motors->arr[mot[4].idx].checkEncoderInRange(mot[4].idx)){ 00503 if(stepMode){ 00504 pos = katana->getMotorEncoders(4) + stepSize; 00505 } 00506 std::cout << "\nMoving to max encoder position " << pos << std::endl; 00507 katana->moveMotorToEnc(mot[4].idx, pos); 00508 mot[4].running = true; 00509 mot[4].dir = RIGHT; 00510 } 00511 else{ 00512 std::cout << "\nEncoder target value out of range! \n" << std::endl; 00513 } 00514 } 00515 else{ 00516 katana->freezeMotor(4); 00517 mot[4].running = false; 00518 } 00519 break; 00520 case '0': 00521 if((stepMode == true) ||(mot[4].running == false || (mot[4].running == true && mot[4].dir == RIGHT))){ 00522 mot[4].idx = 4; 00523 //motors = katana->GetBase()->GetMOT(); 00524 motors = katana->GetBase()->GetMOT(); 00525 pos = motors->arr[mot[4].idx].GetEncoderMaxPos(); 00526 if(motors->arr[mot[4].idx].checkEncoderInRange(mot[4].idx)){ 00527 if(stepMode){ 00528 pos = katana->getMotorEncoders(4) - stepSize; 00529 } 00530 std::cout << "\nMoving to max encoder position " << pos << std::endl; 00531 katana->moveMotorToEnc(mot[4].idx, pos); 00532 mot[4].running = true; 00533 mot[4].dir = LEFT; 00534 } 00535 else{ 00536 std::cout << "\nEncoder target value out of range! \n" << std::endl; 00537 } 00538 } 00539 else{ 00540 katana->freezeMotor(4); 00541 mot[4].running = false; 00542 } 00543 break; 00544 case '/': 00545 if((stepMode == true) ||(mot[5].running == false || (mot[5].running == true && mot[5].dir == LEFT))){ 00546 mot[5].idx = 5; 00547 motors = katana->GetBase()->GetMOT(); 00548 pos = motors->arr[mot[0].idx].GetEncoderMinPos(); 00549 if(motors->arr[mot[0].idx].checkEncoderInRange(mot[5].idx)){ 00550 if(stepMode){ 00551 pos = katana->getMotorEncoders(5) + stepSize; 00552 std::cout << "\nMoving to max encoder position " << pos + 1000 << std::endl; 00553 katana->moveMotorToEnc(mot[5].idx, pos); 00554 } 00555 else{ 00556 katana->openGripper(); 00557 } 00558 mot[5].running = true; 00559 mot[5].dir = RIGHT; 00560 } 00561 else{ 00562 std::cout << "\nEncoder target value out of range! \n" << std::endl; 00563 } 00564 } 00565 else{ 00566 katana->freezeMotor(5); 00567 mot[5].running = false; 00568 } 00569 break; 00570 case '*': 00571 if((stepMode == true) ||(mot[5].running == false || (mot[5].running == true && mot[5].dir == RIGHT))){ 00572 mot[5].idx = 5; 00573 //motors = katana->GetBase()->GetMOT(); 00574 motors = katana->GetBase()->GetMOT(); 00575 pos = motors->arr[mot[5].idx].GetEncoderMaxPos(); 00576 if(motors->arr[mot[5].idx].checkEncoderInRange(mot[5].idx)){ 00577 if(stepMode){ 00578 pos = katana->getMotorEncoders(5) - stepSize; 00579 std::cout << "\nMoving to max encoder position " << pos << std::endl; 00580 katana->moveMotorToEnc(mot[5].idx, pos); 00581 } 00582 else{ 00583 katana->closeGripper(); 00584 } 00585 mot[5].running = true; 00586 mot[5].dir = LEFT; 00587 } 00588 else{ 00589 std::cout << "\nEncoder target value out of range! \n" << std::endl; 00590 } 00591 } 00592 else{ 00593 katana->freezeMotor(5); 00594 mot[5].running = false; 00595 } 00596 break; 00597 case '$': 00598 // loop = false; 00599 // runProgram = true; 00600 // StartProgram(0); 00601 std::cout << "\n Adjust hardcoded pointlist and program for your katana before running the program!!!\n" << std::endl; 00602 break; 00603 case '?': 00604 DisplayHelp(); 00605 break; 00606 00607 case 'r': 00608 if(DispInRad) { 00609 DispInRad = false; 00610 RadToDeg = 180.0/PI; 00611 std::cout << "\nAngles in DEGREE !! \n" << std::endl; 00612 } else { 00613 DispInRad = true; 00614 RadToDeg = 1.0; 00615 std::cout << "\nAngles in RADIAN !! \n" << std::endl; 00616 } 00617 break; 00618 00619 00620 case 'c': //VK_C (Calibration) 00621 std::cout << "\nCalibrating Katana, please wait for termination... \n" << std::flush; 00622 katana->calibrate(); 00623 break; 00624 00625 case 'b': //VK_B (get current controller type) 00626 std::cout << "\nController types: " << std::endl; 00627 for (int i = 0; i < katana->getNumberOfMotors(); ++i) { 00628 std::cout << " " << (katana->getCurrentControllerType(i+1) == 0 ? "position" : "current"); 00629 } 00630 std::cout << std::endl; 00631 break; 00632 00633 case 'e': //VK_E (read encoder values) 00634 std::cout << "\nEncoder values: " << std::endl; 00635 katana->getRobotEncoders(encoders.begin(), encoders.end()); 00636 for (std::vector<int>::iterator i= encoders.begin(); i != encoders.end(); ++i) { 00637 std::cout << *i << " "; 00638 } 00639 std::cout << std::endl; 00640 break; 00641 00642 case 'E': //VK_E (read force values) 00643 std::cout << "\nForce values:" << std::endl; 00644 for (int i = 0; i < katana->getNumberOfMotors(); ++i) { 00645 std::cout << " " << (int)katana->getForce(i+1); 00646 } 00647 std::cout << std::endl; 00648 break; 00649 00650 case 'o': //VK_O (motors off/on) 00651 if(IsOff) { 00652 katana->switchRobotOn(); 00653 IsOff = false; 00654 std::cout << "\nMotors on\n"; 00655 } else { 00656 katana->switchRobotOff(); 00657 IsOff = true; 00658 std::cout << "\nMotors off\n"; 00659 } 00660 break; 00661 00662 case 'x': //VK_O (DK to screen) 00663 { 00664 TPoint p; 00665 katana->getCoordinates(p.X, p.Y, p.Z, p.phi, p.theta, p.psi); 00666 00667 std::cout.precision(6); 00668 std::cout << "\n------------------------------------\n"; 00669 std::cout << "X: " << p.X << "\n"; 00670 std::cout << "Y: " << p.Y << "\n"; 00671 std::cout << "Z: " << p.Z << "\n"; 00672 std::cout << "phi: " << RadToDeg*p.phi << "\n"; 00673 std::cout << "theta: " << RadToDeg*p.theta << "\n"; 00674 std::cout << "psi: " << RadToDeg*p.psi << "\n"; 00675 std::cout << "------------------------------------\n"; 00676 00677 break; 00678 } 00679 case 'q': //read sensors: 00680 { 00681 std::cout << "\nCurrent Sensor values:" << std::endl; 00682 const TSctDAT* data = sensctrl->GetDAT(); 00683 bool* change = new bool[data->cnt]; 00684 byte* lastarr = new byte[data->cnt]; 00685 sensctrl->recvDAT(); 00686 for (int k=0; k<data->cnt; k++) { //init stuff 00687 change[k] = false; 00688 lastarr[k] = (byte) data->arr[k]; 00689 } 00690 //clock_t t = clock(); //init timer 00691 sensctrl->recvDAT(); //update sensor data 00692 for (int i=0; i<data->cnt; i++) { 00693 std::cout.width(5); 00694 std::cout << data->arr[i] << " "; //printout data 00695 } std::cout << "\n"; 00696 /* 00697 while (clock() - t < 25) {} //wait 25 millisec 00698 00699 for (int j=0; j<data->cnt; j++) { 00700 change[j] |= (data->arr[j] != lastarr[j]); 00701 lastarr[j] = data->arr[j]; 00702 } 00703 */ 00704 break; 00705 } 00706 case 'v': //VK_V (Set max vel) 00707 { 00708 short velocity; 00709 std::cout << "\n\nSet maximum velocity for motors to: \n"; 00710 for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) { 00711 std::cout << motorNumber+1 << ": "; 00712 std::cin >> velocity; 00713 katana->setMotorVelocityLimit(motorNumber, velocity); 00714 } 00715 } 00716 break; 00717 00718 case 'V': //VK_V (Set max vel) 00719 { 00720 if(useLinearMode) { 00721 double velocity; 00722 std::cout << "\n\nSet the TCP velocity to: "; 00723 std::cin >> velocity; 00724 katana->setMaximumLinearVelocity(velocity); 00725 katana->setRobotVelocityLimit(static_cast<short>(velocity)); 00726 } else { 00727 short velocity; 00728 std::cout << "\n\nSet maximum velocity for all motors to: "; 00729 std::cin >> velocity; 00730 katana->setRobotVelocityLimit(velocity); 00731 katana->setMaximumLinearVelocity(static_cast<double>(velocity)); 00732 } 00733 std::cout << std::endl; 00734 } 00735 break; 00736 00737 case 'a': //VK_A (Set max acc) 00738 { 00739 short acceleration; 00740 std::cout << "\n\nSet maximum velocity for motors to: \n"; 00741 for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) { 00742 std::cout << motorNumber+1 << ": "; 00743 std::cin >> acceleration; 00744 katana->setMotorAccelerationLimit(motorNumber, acceleration); 00745 } 00746 } 00747 break; 00748 case 'A': //VK_A (Set max acc) 00749 { 00750 short acceleration; 00751 std::cout << "\n\nSet maximum acceleration for all motors to: "; 00752 std::cin >> acceleration; 00753 std::cout << std::endl; 00754 katana->setRobotAccelerationLimit(acceleration); 00755 } 00756 break; 00757 case ',': //set force limits for all motors 00758 { 00759 short limit; 00760 std::cout << "\nSet force limit for all motors to (%): "; 00761 std::cin >> limit; 00762 std::cout << std::endl; 00763 katana->setForceLimit(0, limit); 00764 } 00765 break; 00766 00767 case 'w': //VK_W (Read current max vel) 00768 { 00769 std::cout << "\nCurrent velocity limits:" << std::endl; 00770 for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) 00771 std::cout << motorNumber+1 << ": " << katana->getMotorVelocityLimit(motorNumber) << std::endl; 00772 std::cout << "linear: " << katana->getMaximumLinearVelocity() << std::endl; 00773 break; 00774 } 00775 case 'W': //VK_W (Read current max acc) 00776 { 00777 std::cout << "\nCurrent acceleration limits:" << std::endl; 00778 for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) 00779 std::cout << motorNumber+1 << ": " << katana->getMotorAccelerationLimit(motorNumber) << std::endl; 00780 break; 00781 } 00782 00783 case 'l': //VK_L (switch linear mode) 00784 if(useLinearMode) { 00785 std::cout << "Switching to inverse kinematics movement mode\n"; 00786 } else { 00787 std::cout << "Switching to linear movement mode\n"; 00788 } 00789 useLinearMode = !useLinearMode; 00790 break; 00791 case 'y': //VK_Y (IKGoto) 00792 std::cout << "\n\nInsert cartesian parameters: \n"; 00793 std::cout << "X: "; 00794 std::cin >> arr_pos[0]; 00795 std::cout << "Y: "; 00796 std::cin >> arr_pos[1]; 00797 std::cout << "Z: "; 00798 std::cin >> arr_pos[2]; 00799 std::cout << "phi: "; 00800 std::cin >> arr_pos[3]; 00801 std::cout << "theta: "; 00802 std::cin >> arr_pos[4]; 00803 std::cout << "psi: "; 00804 std::cin >> arr_pos[5]; 00805 00806 arr_pos[3] = 1/RadToDeg*arr_pos[3]; 00807 arr_pos[4] = 1/RadToDeg*arr_pos[4]; 00808 arr_pos[5] = 1/RadToDeg*arr_pos[5]; 00809 00810 if(useLinearMode) { 00811 katana->moveRobotLinearTo(arr_pos[0], arr_pos[1], arr_pos[2], arr_pos[3], arr_pos[4], arr_pos[5]); 00812 } else { 00813 katana->moveRobotTo(arr_pos[0], arr_pos[1], arr_pos[2], arr_pos[3], arr_pos[4], arr_pos[5]); 00814 } 00815 break; 00816 00817 case '<': 00818 std::cout.precision(6); 00819 TPoint point; 00820 katana->getCoordinates(point.X, point.Y, point.Z, point.phi, point.theta, point.psi); 00821 std::cout << "Point: "; 00822 std::cout << " X=" << point.X; 00823 std::cout << ", Y=" << point.Y; 00824 std::cout << ", Z=" << point.Z; 00825 std::cout << ", phi=" << RadToDeg*point.phi; 00826 std::cout << ", theta="<< RadToDeg*point.theta; 00827 std::cout << ", psi=" << RadToDeg*point.psi; 00828 std::cout << " ... added to point list as number "; 00829 std::cout << points.size() << std::endl; 00830 points.push_back(point); 00831 break; 00832 00833 case '>': 00834 std::cout.width(10); 00835 std::cout.precision(3); 00836 std::cout << "\nMoving to point? "; 00837 unsigned int pointNumber; 00838 std::cin >> pointNumber; 00839 if(pointNumber >= points.size()) { 00840 std::cout << "Invalid point number. You have only " << points.size() << " points in your list" << std::endl; 00841 break; 00842 } 00843 std::cout.width(6); 00844 std::cout << " x=" << points[pointNumber].X; 00845 std::cout.width(6); 00846 std::cout << " y=" << points[pointNumber].Y; 00847 std::cout.width(6); 00848 std::cout << " z=" << points[pointNumber].Z; 00849 std::cout.width(6); 00850 std::cout << " phi=" << RadToDeg*points[pointNumber].phi; 00851 std::cout.width(6); 00852 std::cout << " theta=" <<RadToDeg*points[pointNumber].theta; 00853 std::cout.width(6); 00854 std::cout << " psi=" << RadToDeg*points[pointNumber].psi; 00855 std::cout << std::endl; 00856 if(useLinearMode) { 00857 katana->moveRobotLinearTo(points[pointNumber].X, points[pointNumber].Y, points[pointNumber].Z, points[pointNumber].phi, points[pointNumber].theta, points[pointNumber].psi); 00858 } else { 00859 katana->moveRobotTo(points[pointNumber].X, points[pointNumber].Y, points[pointNumber].Z, points[pointNumber].phi, points[pointNumber].theta, points[pointNumber].psi); 00860 } 00861 break; 00862 00863 case 3: 00864 case 4: 00865 case 27: //VK_ESCAPE 00866 loop = false; 00867 continue; 00868 00869 case ' ': //VK_SPACE 00870 //Move to the next point 00871 std::cout.width(10); 00872 std::cout.precision(3); 00873 std::cout << "Moving to point " << counter << ": "; 00874 std::cout.width(6); 00875 std::cout << " x=" << points[counter].X; 00876 std::cout.width(6); 00877 std::cout << " y=" << points[counter].Y; 00878 std::cout.width(6); 00879 std::cout << " z=" << points[counter].Z; 00880 std::cout.width(6); 00881 std::cout << " phi=" << RadToDeg*points[counter].phi; 00882 std::cout.width(6); 00883 std::cout << " theta=" <<RadToDeg*points[counter].theta; 00884 std::cout.width(6); 00885 std::cout << " psi=" << RadToDeg*points[counter].psi; 00886 std::cout << std::endl; 00887 00888 if(useLinearMode) { 00889 katana->moveRobotLinearTo(points[counter].X, points[counter].Y, points[counter].Z, points[counter].phi, points[counter].theta, points[counter].psi); 00890 } else { 00891 katana->moveRobotTo(points[counter].X, points[counter].Y, points[counter].Z, points[counter].phi, points[counter].theta, points[counter].psi); 00892 } 00893 counter++; 00894 counter = counter % ((short) points.size()); 00895 break; 00896 00897 case 'g': 00898 std::cout << "Opening gripper...\n"; 00899 katana->openGripper(); 00900 break; 00901 case 'h': 00902 std::cout << "Close gripper...\n"; 00903 katana->closeGripper(); 00904 break; 00905 case 'n': 00906 std::cout << "Set speed collision limit for motors to: \n"; 00907 for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) { 00908 std::cout << motorNumber+1 << ": "; 00909 std::cin >> limit; 00910 katana->setSpeedCollisionLimit(motorNumber, limit); 00911 } 00912 break; 00913 case 'N': 00914 std::cout << "Set speed collision limit for all motors to: \n"; 00915 std::cin >> limit; 00916 for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) { 00917 katana->setSpeedCollisionLimit(motorNumber, limit); 00918 } 00919 break; 00920 case 's': 00921 std::cout << "Set position collision limit for motors to: \n"; 00922 for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) { 00923 std::cout << motorNumber+1 << ": "; 00924 std::cin >> limit; 00925 katana->setPositionCollisionLimit(motorNumber, limit); 00926 } 00927 break; 00928 case 'S': 00929 std::cout << "Set position collision limit for all motors to: \n"; 00930 std::cin >> limit; 00931 for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) { 00932 katana->setPositionCollisionLimit(motorNumber, limit); 00933 } 00934 break; 00935 case 't': 00936 std::cout << "Collision detection enabled\n"; 00937 katana->enableCrashLimits(); 00938 break; 00939 case 'T': 00940 std::cout << "WARNING: Collision detection disabled\n"; 00941 katana->disableCrashLimits(); 00942 break; 00943 case 'u': 00944 std::cout << "Unblocking motors\n"; 00945 katana->unBlock(); 00946 break; 00947 00948 case 'f': { 00949 using namespace std; 00950 00951 cout << "Loading which file?\n"; 00952 string filename; 00953 cin >> filename; 00954 00955 ifstream listfile(filename.c_str()); 00956 if(!listfile) { 00957 cout << "File not found or access denied." << endl; 00958 break; 00959 } 00960 00961 string line; 00962 vector<string> tokens; 00963 const string delimiter = ","; 00964 00965 int lines = 0; 00966 00967 while(!listfile.eof()) { 00968 listfile >> line; 00969 string::size_type lastPos = line.find_first_not_of(delimiter, 0); 00970 string::size_type pos = line.find_first_of(delimiter, lastPos); 00971 00972 while (string::npos != pos || string::npos != lastPos) { 00973 // Found a token, add it to the vector. 00974 tokens.push_back(line.substr(lastPos, pos - lastPos)); 00975 // Skip delimiters. Note the "not_of" 00976 lastPos = line.find_first_not_of(delimiter, pos); 00977 // Find next "non-delimiter" 00978 pos = line.find_first_of(delimiter, lastPos); 00979 } 00980 00981 TPoint point; 00982 point.X = atof((tokens.at(0)).data()); 00983 point.Y = atof((tokens.at(1)).data()); 00984 point.Z = atof((tokens.at(2)).data()); 00985 point.phi = atof((tokens.at(3)).data())/RadToDeg; 00986 point.theta = atof((tokens.at(4)).data())/RadToDeg; 00987 point.psi = atof((tokens.at(5)).data())/RadToDeg; 00988 points.push_back( point ); 00989 ++lines; 00990 tokens.clear(); 00991 } 00992 cout << lines << " points loaded.\n"; 00993 00994 break; 00995 } 00996 00997 case '=': { 00998 using namespace std; 00999 cout << "Which file? WARNING: Will be overwritten!\n"; 01000 string filename; 01001 cin >> filename; 01002 01003 ofstream listfile(filename.c_str(), ios_base::out); 01004 01005 int count = 0; 01006 01007 for(std::vector<TPoint>::iterator iter = points.begin(); iter != points.end(); ++iter) { 01008 listfile.precision(8); 01009 if (count != 0) 01010 listfile << endl; 01011 listfile << iter->X << "," << iter->Y << "," << iter->Z << ","; 01012 listfile << iter->phi << "," << iter->theta << "," << iter->psi; 01013 ++count; 01014 } 01015 cout << count << " points saved.\n"; 01016 listfile.close(); 01017 break; 01018 } 01019 case 'p': { 01020 using namespace std; 01021 //int available; 01022 //cout << "How many movements?\n"; 01023 //int moves; 01024 //cin >> moves; 01025 std::cout << "Start playback!\n"; 01026 for(int i = 1; i > 0/*i <= moves*/ ; ++i) { 01027 if(useLinearMode) { 01028 katana->moveRobotLinearTo( points[i%points.size()].X, points[i%points.size()].Y, points[i%points.size()].Z, points[i%points.size()].phi, points[i%points.size()].theta, points[i%points.size()].psi, true, 10000 ); 01029 } 01030 else{ 01031 katana->moveRobotTo( points[i%points.size()].X, points[i%points.size()].Y, points[i%points.size()].Z, points[i%points.size()].phi, points[i%points.size()].theta, points[i%points.size()].psi, true, 10000 ); 01032 } 01033 if (i%100 == 0) { 01034 std::cout << i << ", " << std::flush; 01035 } 01036 } 01037 01038 break; 01039 01040 } 01041 01042 case 'd': { 01043 long motor; 01044 double degrees; 01045 std::cout << "\nMoving motor to degrees\n"; 01046 std::cout << " motor: "; 01047 std::cin >> motor; 01048 std::cout << " degrees: "; 01049 std::cin >> degrees; 01050 if ((motor > 0) && (motor < 7)) { 01051 katana->movDegrees(motor - 1, degrees); 01052 } else { 01053 std::cout << "motor has to be a number from 1 to 6\n"; 01054 } 01055 01056 break; 01057 } 01058 01059 case 'z': { 01060 double xoff, yoff, zoff, psioff; 01061 std::cout << "X offset (m): "; 01062 std::cin >> xoff; 01063 std::cout << "Y offset (m): "; 01064 std::cin >> yoff; 01065 std::cout << "Z offset (m): "; 01066 std::cin >> zoff; 01067 std::cout << "psi offset around x axis (rad): "; 01068 std::cin >> psioff; 01069 katana->setTcpOffset(xoff, yoff, zoff, psioff); 01070 01071 break; 01072 } 01073 01074 default: //Error message 01075 std::cout << "\n'" << input << "' is not a valid command.\n" << std::endl; 01076 break; 01077 } 01078 01079 } catch (Exception &e) { 01080 std::cout << "\nERROR: " << e.message() << std::endl; 01081 } 01082 01083 } 01084 /*//Q&D test to launch program: 01085 if(runProgram){ 01086 StartProgram(0); 01087 } 01088 */ 01089 return 0; 01090 } 01092 void StartProgram(int index){ 01093 //Q&D test to launch program: 01094 //std::system("/home/katprog katana6M180.cfg 1"); 01095 progRunning = true; 01096 pthread_create(&tid, NULL, RunProgram, (void*)&retVal);//(&tid, NULL, start_func, arg); 01097 pthread_detach(tid); 01098 } 01100 void* RunProgram(void*){ 01101 //katana->calibrate(); 01102 std::cout << "\nProgram running...type any key to stop after the next cycle\n"; 01103 while(progRunning){ 01104 if(progRunning) katana->moveRobotToEnc(Tpos::x, true); 01105 if(progRunning) katana->moveRobotToEnc(Tpos::y, true); 01106 if(progRunning) katana->moveRobotToEnc(Tpos::z, true); 01107 if(progRunning) katana->moveRobotToEnc(Tpos::u, true); 01108 if(progRunning) katana->moveRobotToEnc(Tpos::v, true); 01109 if(progRunning) katana->moveRobotToEnc(Tpos::w, true); 01110 } 01111 pthread_exit((void*) &retVal); 01112 return ((void*) &retVal); 01113 } 01115