00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00018
00019
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
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
00056 std::auto_ptr<CLMBase> katana;
00057
00058
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
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
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());
00177 std::cout << "-------------------------------------------\n";
00178 std::cout << "success: communication with Katana initialized\n";
00179 std::cout << "-------------------------------------------\n";
00180
00181
00182
00183
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
00219
00220
00221
00222
00223
00224
00225
00226 CSctBase* sensctrl = &katana->GetBase()->GetSCT()->arr[0];
00227 int limit;
00228
00229
00230 katana->setMaximumLinearVelocity(60);
00231
00232 while (loop) {
00233 double arr_pos[6];
00234
00235 int input = _getch();
00236 if(progRunning){
00237
00238 progRunning = false;
00239 continue;
00240 }
00241
00242 try {
00243
00244 switch (input) {
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
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
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
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
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
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
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
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
00599
00600
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':
00621 std::cout << "\nCalibrating Katana, please wait for termination... \n" << std::flush;
00622 katana->calibrate();
00623 break;
00624
00625 case 'b':
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':
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':
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':
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':
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':
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++) {
00687 change[k] = false;
00688 lastarr[k] = (byte) data->arr[k];
00689 }
00690
00691 sensctrl->recvDAT();
00692 for (int i=0; i<data->cnt; i++) {
00693 std::cout.width(5);
00694 std::cout << data->arr[i] << " ";
00695 } std::cout << "\n";
00696
00697
00698
00699
00700
00701
00702
00703
00704 break;
00705 }
00706 case 'v':
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':
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':
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':
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 ',':
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':
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':
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':
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':
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:
00866 loop = false;
00867 continue;
00868
00869 case ' ':
00870
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
00974 tokens.push_back(line.substr(lastPos, pos - lastPos));
00975
00976 lastPos = line.find_first_not_of(delimiter, pos);
00977
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
01022
01023
01024
01025 std::cout << "Start playback!\n";
01026 for(int i = 1; i > 0 ; ++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:
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
01085
01086
01087
01088
01089 return 0;
01090 }
01092 void StartProgram(int index){
01093
01094
01095 progRunning = true;
01096 pthread_create(&tid, NULL, RunProgram, (void*)&retVal);
01097 pthread_detach(tid);
01098 }
01100 void* RunProgram(void*){
01101
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