control.cpp
Go to the documentation of this file.
1 /**********************************************************************************
2  * Katana Native Interface - A C++ interface to the robot arm Katana.
3  * Copyright (C) 2005-2008 Neuronics AG
4  * Check out the AUTHORS file for detailed contact information.
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU General Public License for more details.
13  * You should have received a copy of the GNU General Public License
14  * along with this program; if not, write to the Free Software
15  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
16  **********************************************************************************/
18 // control.cpp
19 // demo program for KNI libraries
21 #include "kniBase.h"
22 #include <iostream>
23 #include <cstdio>
24 #include <memory>
25 #include <vector>
26 #include <fstream>
27 #include <pthread.h>
29 #ifdef WIN32
30 # include <conio.h>
31 #else //LINUX
32 # include "keyboard.h"
33 #endif
34 #define LEFT false
35 #define RIGHT true
36 //Thread structs:
38 pthread_mutex_t mutex;
39 
40 struct TPoint {
41  double X, Y, Z;
42  double phi, theta, psi;
43 };
44 
45 struct TCurrentMot {
46  int idx;
47  bool running;
48  bool dir;
49 };
50 
51 struct Tpos{
52  static std::vector<int> x,y,z,u,v,w;
53  static const int xArr[], yArr[], zArr[], uArr[], vArr[], wArr[];
54 };
55 //Katana obj.
56 std::unique_ptr<CLMBase> katana;
57 //std::vector<int> Foo::vec(array, array + sizeof(array)/sizeof(*array));
58 //positionen, hard-coded. Use values from file instead
59 const int Tpos::xArr[] = {30206, -23393, -3066, 14454, 30000, 30000};
60 const int Tpos::yArr[] = {24327, -7837, -16796, 5803, 30290, 31000};
61 const int Tpos::zArr[] = {24327, -7837, -16796, 5802, 30290, 10924};
62 const int Tpos::uArr[] = {5333, -13791, -9985, 11449, 30996, 12063};
63 const int Tpos::vArr[] = {-3799, -5703, -11676, 8210, 30995, 12063};
64 const int Tpos::wArr[] = {-3799, -5703, -11676, 8210, 30995, 30992};
65 std::vector<int> Tpos::x(xArr, xArr + sizeof(xArr)/sizeof(*xArr));
66 std::vector<int> Tpos::y(yArr, yArr + sizeof(yArr)/sizeof(*yArr));
67 std::vector<int> Tpos::z(zArr, zArr + sizeof(zArr)/sizeof(*zArr));
68 std::vector<int> Tpos::u(uArr, uArr + sizeof(uArr)/sizeof(*uArr));
69 std::vector<int> Tpos::v(vArr, vArr + sizeof(vArr)/sizeof(*vArr));
70 std::vector<int> Tpos::w(wArr, wArr + sizeof(wArr)/sizeof(*wArr));
71 std::vector<TPoint> points(0);
72 void StartPointlistMovement();
73 void StartProgram(int index);
74 pthread_t tid;
75 void* RunProgram(void*);
76 pid_t threadPid;
77 int retVal = 0;
78 bool progRunning = false;
79 const double PI = 3.14159265358979323846;
81 void DisplayHelp() {
82  std::cout << "------------------------------------------- \n";
83  std::cout << "?: Display this help\n";
84  std::cout << "c: Calibrate the Katana\n";
85  std::cout << "b: Read current controller types\n";
86  std::cout << "e: Read the current encoder values\n";
87  std::cout << "E: Read the current force values\n";
88  std::cout << "o: Switch motors off/on (Default: On)\n";
89  std::cout << "r: Switch angle format: Radian/Degree (Default: Rad)\n";
90  std::cout << "x: Read the current position\n";
91  std::cout << "v: Set the velocity limits for all motors seperately\n";
92  std::cout << "V: Set the velocity limits for all motors (or for the TCP if in linear movement mode)\n";
93  std::cout << "a: Set the acceleration limits for all motors seperately\n";
94  std::cout << "A: Set the acceleration limits for all motors\n";
95  std::cout << ",: Set the force limits for all motors\n";
96  std::cout << "w: Read the velocity limits of all motors \n";
97  std::cout << "W: Read the acceleration limits of all motors \n";
98  std::cout << "q: Read the Sensors\n";
99  std::cout << "y: Set a new position using IK\n";
100  std::cout << "l: Switch on/off linear movements\n";
101  std::cout << "<: Add a point to the point list\n";
102  std::cout << ">: Move to a specific point\n";
103  std::cout << " : (space) Move to the next point in the point list\n";
104  std::cout << "=: write pointlist to file\n";
105  std::cout << "(: calculate DK from any encoders\n";
106  std::cout << "f: read pointlist from file\n";
107  std::cout << "g: Open Gripper\n";
108  std::cout << "h: Close Gripper\n";
109  std::cout << "n: Set the speed collision limit for all motors seperately\n";
110  std::cout << "N: Set the speed collision limit for all motors\n";
111  std::cout << "s: Set the position collision limit for all motors seperately\n";
112  std::cout << "S: Set the position collision limit for all motors\n";
113  std::cout << "t: Switch collision limit on\n";
114  std::cout << "T: Switch collision limit off\n";
115  std::cout << "u: Unblock motors after crash\n";
116  std::cout << "d: Move motor to degrees\n";
117  std::cout << "z: Set TCP offset\n\n\n";
118  std::cout << "Keyboard of Katana, use the following keys:\n\n";
119  std::cout << "1: Move motor1 left\n";
120  std::cout << "2: Move motor1 right\n";
121  std::cout << "3: Move motor2 left\n";
122  std::cout << "4: Move motor2 right\n";
123  std::cout << "5: Move motor3 left\n";
124  std::cout << "6: Move motor3 right\n";
125  std::cout << "7: Move motor4 left\n";
126  std::cout << "8: Move motor4 right\n";
127  std::cout << "9: Move motor5 left\n";
128  std::cout << "0: Move motor5 right\n";
129  std::cout << "/: Move motor6 left\n";
130  std::cout << "*: Move motor6 right\n";
131  std::cout << ".: Toggle Step mode\n";
132  std::cout << "+: Increase step size\n";
133  std::cout << "-: Decrease step size\n\n";
134  std::cout << "$: Start/Stop Program\n";
135  std::cout << "p: Start/Stop movement through points list\n\n";
136 }
138 int main(int argc, char *argv[]) {
139 
140  if (argc != 3) {
141  std::cout << "---------------------------------\n";
142  std::cout << "usage: control CONFIGFILE IP_ADDR\n";
143  std::cout << "---------------------------------\n";
144  return 0;
145  }
146 
147  std::cout << "--------------------\n";
148  std::cout << "CONTROL DEMO STARTED\n";
149  std::cout << "--------------------\n";
150 
151  //----------------------------------------------------------------//
152  //open device: a serial port is opened in this case
153  //----------------------------------------------------------------//
154 
155 
156  std::unique_ptr<CCdlSocket> device;
157  std::unique_ptr<CCplSerialCRC> protocol;
158 
159  try {
160 
161  int port = 5566;
162  device.reset(new CCdlSocket(argv[2], port));
163 
164  std::cout << "-------------------------------------------\n";
165  std::cout << "success: port " << port << " open\n";
166  std::cout << "-------------------------------------------\n";
167 
168  //--------------------------------------------------------//
169  //init protocol:
170  //--------------------------------------------------------//
171 
172  protocol.reset(new CCplSerialCRC());
173  std::cout << "-------------------------------------------\n";
174  std::cout << "success: protocol class instantiated\n";
175  std::cout << "-------------------------------------------\n";
176  protocol->init(device.get()); //fails if no response from Katana
177  std::cout << "-------------------------------------------\n";
178  std::cout << "success: communication with Katana initialized\n";
179  std::cout << "-------------------------------------------\n";
180 
181 
182  //--------------------------------------------------------//
183  //init robot:
184  //--------------------------------------------------------//
185  katana.reset(new CLMBase());
186  katana->create(argv[1], protocol.get());
187 
188 
189  } catch(Exception &e) {
190  std::cout << "ERROR: " << e.message() << std::endl;
191  return -1;
192  }
193  std::cout << "-------------------------------------------\n";
194  std::cout << "success: katana initialized\n";
195  std::cout << "-------------------------------------------\n";
196 
197  DisplayHelp();
198 
199  short counter = 0;
200  bool loop = true;
201  int pos = 0;
202  bool DispInRad = true;
203  bool IsOff = false;
204  bool useLinearMode = false;
205  double RadToDeg = 1.0;
206  std::vector<int> encoders(katana->getNumberOfMotors(), 0);
207  const TKatMOT* motors;
208  TCurrentMot mot[6];
209  for (int i = 0; i< 6; i++){
210  mot[i].running = false;
211  mot[i].idx = i;
212  mot[i].dir = true;
213  mot[i].dir = RIGHT;
214  }
215  bool stepMode = false;
216  int stepSize = 100;
217  bool runProgram = false;
218  //pthread_mutex_init(&mutex, 0);
219  //pthread_mutex_lock(&mutex);
220  //pthread_mutex_unlock(&mutex);
221  //set sensor controller values
222  /*TSctDesc sctdesc[1] = {{15, 8, 16}}; //sctID,resol,count
223  CSctBase* sctarr = new CSctBase[1]; //create sensor-ctrl class
224  TKatSCT katsct = {1, sctarr, sctdesc}; //fill TKatSCT structure
225  */
226  CSctBase* sensctrl = &katana->GetBase()->GetSCT()->arr[0];
227  int limit;
228 
229  //set linear velocity to 60
230  katana->setMaximumLinearVelocity(60);
231 
232  while (loop) {
233  double arr_pos[6];
234 
235  int input = _getch();
236  if(progRunning){
237  //Thread killen:
238  progRunning = false;
239  continue;
240  }
241 
242  try {
243 
244  switch (input) {
245 
246 /* case 'i': //VK_I: Test routine, connected P2P movement
247  {
248  double posX = points[counter].X;
249  double posY = points[counter].Y;
250  double posZ = points[counter].Z;
251  double posPhi = points[counter].phi;
252  double posTheta = points[counter].theta;
253  double posPsi = points[counter].psi;
254  katana->moveRobotTo(posX, posY, posZ, posPhi, posTheta, posPsi, false);
255  counter++;
256  counter = counter % ((short) points.size());
257  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);
258  counter++;
259  counter = counter % ((short) points.size());
260  break;
261  }
262 */
263  case '(':
264  {
265  std::vector<double> pose_result(6, 0);
266  std::vector<int> etc;
267  etc.push_back(20000);
268  etc.push_back(-20000);
269  etc.push_back(-20000);
270  etc.push_back(20000);
271  etc.push_back(20000);
272  etc.push_back(20000);
273  katana->getCoordinatesFromEncoders(pose_result, etc);
274  std::cout.precision(6);
275  std::cout << "\n------------------------------------\n";
276  std::cout << "X: " << pose_result.at(0) << "\n";
277  std::cout << "Y: " << pose_result.at(1) << "\n";
278  std::cout << "Z: " << pose_result.at(2) << "\n";
279  std::cout << "phi: " << RadToDeg*pose_result.at(3) << "\n";
280  std::cout << "theta: " << RadToDeg*pose_result.at(4) << "\n";
281  std::cout << "psi: " << RadToDeg*pose_result.at(5) << "\n";
282  std::cout << "------------------------------------\n";
283  }
284  break;
285 
286  case '.':
287  if(stepMode == false){
288  stepMode = true;
289  std::cout << "Step Mode ON \n" << std::endl;
290  }
291  else{
292  stepMode = false;
293  std::cout << "Step Mode OFF \n" << std::endl;
294  }
295  break;
296  case '+':
297  if(stepSize <= 3000){
298  stepSize += 300;
299  std::cout << "Step Size = " << stepSize << std::endl;
300 
301  }
302  break;
303  case '-':
304  if(stepSize >= 330){
305  stepSize -= 300;
306  std::cout << "Step Size = " << stepSize << std::endl;
307  }
308  break;
309  case '1':
310  if((stepMode == true) ||(mot[0].running == false || (mot[0].running == true && mot[0].dir == LEFT))){
311  mot[0].idx = 0;
312  motors = katana->GetBase()->GetMOT();
313  pos = motors->arr[mot[0].idx].GetEncoderMinPos();
314  if(motors->arr[mot[0].idx].checkEncoderInRange(mot[0].idx)){
315  if(stepMode){
316  pos = katana->getMotorEncoders(0) + stepSize;
317  }
318  std::cout << "\nMoving to max encoder position " << pos << std::endl;
319  katana->moveMotorToEnc(mot[0].idx, pos);
320  mot[0].running = true;
321  mot[0].dir = RIGHT;
322  }
323  else{
324  std::cout << "\nEncoder target value out of range! \n" << std::endl;
325  }
326  }
327  else{
328  katana->freezeMotor(0);
329  mot[0].running = false;
330  }
331  break;
332  case '2':
333  if((stepMode == true) ||(mot[0].running == false || (mot[0].running == true && mot[0].dir == RIGHT))){
334  mot[0].idx = 0;
335  //motors = katana->GetBase()->GetMOT();
336  motors = katana->GetBase()->GetMOT();
337  pos = motors->arr[mot[0].idx].GetEncoderMaxPos();
338  if(motors->arr[mot[0].idx].checkEncoderInRange(mot[0].idx)){
339  if(stepMode){
340  pos = katana->getMotorEncoders(0) - stepSize;
341  }
342  std::cout << "\nMoving to max encoder position " << pos << std::endl;
343  katana->moveMotorToEnc(mot[0].idx, pos);
344  mot[0].running = true;
345  mot[0].dir = LEFT;
346  }
347  else{
348  std::cout << "\nEncoder target value out of range! \n" << std::endl;
349  }
350  }
351  else{
352  katana->freezeMotor(0);
353  mot[0].running = false;
354  }
355  break;
356  case '3':
357  if((stepMode == true) ||(mot[1].running == false || (mot[1].running == true && mot[1].dir == LEFT))){
358  mot[1].idx = 1;
359  motors = katana->GetBase()->GetMOT();
360  pos = motors->arr[mot[1].idx].GetEncoderMinPos();
361  if(motors->arr[mot[1].idx].checkEncoderInRange(mot[1].idx)){
362  if(stepMode){
363  pos = katana->getMotorEncoders(1) + stepSize;
364  }
365  std::cout << "\nMoving to max encoder position " << pos << std::endl;
366  katana->moveMotorToEnc(mot[1].idx, pos);
367  mot[1].running = true;
368  mot[1].dir = RIGHT;
369  }
370  else{
371  std::cout << "\nEncoder target value out of range! \n" << std::endl;
372  }
373  }
374  else{
375  katana->freezeMotor(1);
376  mot[1].running = false;
377  }
378  break;
379  case '4':
380  if((stepMode == true) ||(mot[1].running == false || (mot[1].running == true && mot[1].dir == RIGHT))){
381  mot[1].idx = 1;
382  //motors = katana->GetBase()->GetMOT();
383  motors = katana->GetBase()->GetMOT();
384  pos = motors->arr[mot[1].idx].GetEncoderMaxPos();
385  if(motors->arr[mot[1].idx].checkEncoderInRange(mot[1].idx)){
386  if(stepMode){
387  pos = katana->getMotorEncoders(1) - stepSize;
388  }
389  std::cout << "\nMoving to max encoder position " << pos << std::endl;
390  katana->moveMotorToEnc(mot[1].idx, pos);
391  mot[1].running = true;
392  mot[1].dir = LEFT;
393  }
394  else{
395  std::cout << "\nEncoder target value out of range! \n" << std::endl;
396  }
397  }
398  else{
399  katana->freezeMotor(1);
400  mot[1].running = false;
401  }
402  break;
403  case '5':
404  if((stepMode == true) ||(mot[2].running == false || (mot[2].running == true && mot[2].dir == LEFT))){
405  mot[2].idx = 2;
406  motors = katana->GetBase()->GetMOT();
407  pos = motors->arr[mot[2].idx].GetEncoderMinPos();
408  if(motors->arr[mot[2].idx].checkEncoderInRange(mot[2].idx)){
409  if(stepMode){
410  pos = katana->getMotorEncoders(2) + stepSize;
411  }
412  std::cout << "\nMoving to max encoder position " << pos << std::endl;
413  katana->moveMotorToEnc(mot[2].idx, pos);
414  mot[2].running = true;
415  mot[2].dir = RIGHT;
416  }
417  else{
418  std::cout << "\nErunProgram =ncoder target value out of range! \n" << std::endl;
419  }
420  }
421  else{
422  katana->freezeMotor(2);
423  mot[2].running = false;
424  }
425  break;
426  case '6':
427  if((stepMode == true) ||(mot[2].running == false || (mot[2].running == true && mot[2].dir == RIGHT))){
428  mot[2].idx = 2;
429  //motors = katana->GetBase()->GetMOT();
430  motors = katana->GetBase()->GetMOT();
431  pos = motors->arr[mot[2].idx].GetEncoderMaxPos();
432  if(motors->arr[mot[2].idx].checkEncoderInRange(mot[2].idx)){
433  if(stepMode){
434  pos = katana->getMotorEncoders(2) - stepSize;
435  }
436  std::cout << "\nMoving to max encoder position " << pos << std::endl;
437  katana->moveMotorToEnc(mot[2].idx, pos);
438  mot[2].running = true;
439  mot[2].dir = LEFT;
440  }
441  else{
442  std::cout << "\nEncoder target value out of range! \n" << std::endl;
443  }
444  }
445  else{
446  katana->freezeMotor(2);
447  mot[2].running = false;
448  }
449  break;
450  case '7':
451  if((stepMode == true) ||(mot[3].running == false || (mot[3].running == true && mot[3].dir == LEFT))){
452  mot[3].idx = 3;
453  motors = katana->GetBase()->GetMOT();
454  pos = motors->arr[mot[3].idx].GetEncoderMinPos();
455  if(motors->arr[mot[3].idx].checkEncoderInRange(mot[3].idx)){
456  if(stepMode){
457  pos = katana->getMotorEncoders(3) + stepSize;
458  }
459  std::cout << "\nMoving to max encoder position " << pos << std::endl;
460  katana->moveMotorToEnc(mot[3].idx, pos);
461  mot[3].running = true;
462  mot[3].dir = RIGHT;
463  }
464  else{
465  std::cout << "\nEncoder target value out of range! \n" << std::endl;
466  }
467  }
468  else{
469  katana->freezeMotor(3);runProgram =
470  mot[3].running = false;
471  }
472  break;
473  case '8':
474  if((stepMode == true) ||(mot[3].running == false || (mot[3].running == true && mot[3].dir == RIGHT))){
475  mot[3].idx = 3;
476  //motors = katana->GetBase()->GetMOT();
477  motors = katana->GetBase()->GetMOT();
478  pos = motors->arr[mot[3].idx].GetEncoderMaxPos();
479  if(motors->arr[mot[3].idx].checkEncoderInRange(mot[3].idx)){
480  if(stepMode){
481  pos = katana->getMotorEncoders(3) - stepSize;
482  }
483  std::cout << "\nMoving to max encoder position " << pos << std::endl;
484  katana->moveMotorToEnc(mot[3].idx, pos);
485  mot[3].running = true;
486  mot[3].dir = LEFT;
487  }
488  else{
489  std::cout << "\nEncoder target value out of range! \n" << std::endl;
490  }
491  }
492  else{
493  katana->freezeMotor(3);
494  mot[3].running = false;
495  }
496  break;
497  case '9':
498  if((stepMode == true) ||(mot[4].running == false || (mot[4].running == true && mot[4].dir == LEFT))){
499  mot[4].idx = 4;
500  motors = katana->GetBase()->GetMOT();
501  pos = motors->arr[mot[4].idx].GetEncoderMinPos();
502  if(motors->arr[mot[4].idx].checkEncoderInRange(mot[4].idx)){
503  if(stepMode){
504  pos = katana->getMotorEncoders(4) + stepSize;
505  }
506  std::cout << "\nMoving to max encoder position " << pos << std::endl;
507  katana->moveMotorToEnc(mot[4].idx, pos);
508  mot[4].running = true;
509  mot[4].dir = RIGHT;
510  }
511  else{
512  std::cout << "\nEncoder target value out of range! \n" << std::endl;
513  }
514  }
515  else{
516  katana->freezeMotor(4);
517  mot[4].running = false;
518  }
519  break;
520  case '0':
521  if((stepMode == true) ||(mot[4].running == false || (mot[4].running == true && mot[4].dir == RIGHT))){
522  mot[4].idx = 4;
523  //motors = katana->GetBase()->GetMOT();
524  motors = katana->GetBase()->GetMOT();
525  pos = motors->arr[mot[4].idx].GetEncoderMaxPos();
526  if(motors->arr[mot[4].idx].checkEncoderInRange(mot[4].idx)){
527  if(stepMode){
528  pos = katana->getMotorEncoders(4) - stepSize;
529  }
530  std::cout << "\nMoving to max encoder position " << pos << std::endl;
531  katana->moveMotorToEnc(mot[4].idx, pos);
532  mot[4].running = true;
533  mot[4].dir = LEFT;
534  }
535  else{
536  std::cout << "\nEncoder target value out of range! \n" << std::endl;
537  }
538  }
539  else{
540  katana->freezeMotor(4);
541  mot[4].running = false;
542  }
543  break;
544  case '/':
545  if((stepMode == true) ||(mot[5].running == false || (mot[5].running == true && mot[5].dir == LEFT))){
546  mot[5].idx = 5;
547  motors = katana->GetBase()->GetMOT();
548  pos = motors->arr[mot[0].idx].GetEncoderMinPos();
549  if(motors->arr[mot[0].idx].checkEncoderInRange(mot[5].idx)){
550  if(stepMode){
551  pos = katana->getMotorEncoders(5) + stepSize;
552  std::cout << "\nMoving to max encoder position " << pos + 1000 << std::endl;
553  katana->moveMotorToEnc(mot[5].idx, pos);
554  }
555  else{
556  katana->openGripper();
557  }
558  mot[5].running = true;
559  mot[5].dir = RIGHT;
560  }
561  else{
562  std::cout << "\nEncoder target value out of range! \n" << std::endl;
563  }
564  }
565  else{
566  katana->freezeMotor(5);
567  mot[5].running = false;
568  }
569  break;
570  case '*':
571  if((stepMode == true) ||(mot[5].running == false || (mot[5].running == true && mot[5].dir == RIGHT))){
572  mot[5].idx = 5;
573  //motors = katana->GetBase()->GetMOT();
574  motors = katana->GetBase()->GetMOT();
575  pos = motors->arr[mot[5].idx].GetEncoderMaxPos();
576  if(motors->arr[mot[5].idx].checkEncoderInRange(mot[5].idx)){
577  if(stepMode){
578  pos = katana->getMotorEncoders(5) - stepSize;
579  std::cout << "\nMoving to max encoder position " << pos << std::endl;
580  katana->moveMotorToEnc(mot[5].idx, pos);
581  }
582  else{
583  katana->closeGripper();
584  }
585  mot[5].running = true;
586  mot[5].dir = LEFT;
587  }
588  else{
589  std::cout << "\nEncoder target value out of range! \n" << std::endl;
590  }
591  }
592  else{
593  katana->freezeMotor(5);
594  mot[5].running = false;
595  }
596  break;
597  case '$':
598 // loop = false;
599 // runProgram = true;
600 // StartProgram(0);
601  std::cout << "\n Adjust hardcoded pointlist and program for your katana before running the program!!!\n" << std::endl;
602  break;
603  case '?':
604  DisplayHelp();
605  break;
606 
607  case 'r':
608  if(DispInRad) {
609  DispInRad = false;
610  RadToDeg = 180.0/PI;
611  std::cout << "\nAngles in DEGREE !! \n" << std::endl;
612  } else {
613  DispInRad = true;
614  RadToDeg = 1.0;
615  std::cout << "\nAngles in RADIAN !! \n" << std::endl;
616  }
617  break;
618 
619 
620  case 'c': //VK_C (Calibration)
621  std::cout << "\nCalibrating Katana, please wait for termination... \n" << std::flush;
622  katana->calibrate();
623  break;
624 
625  case 'b': //VK_B (get current controller type)
626  std::cout << "\nController types: " << std::endl;
627  for (int i = 0; i < katana->getNumberOfMotors(); ++i) {
628  std::cout << " " << (katana->getCurrentControllerType(i+1) == 0 ? "position" : "current");
629  }
630  std::cout << std::endl;
631  break;
632 
633  case 'e': //VK_E (read encoder values)
634  std::cout << "\nEncoder values: " << std::endl;
635  katana->getRobotEncoders(encoders.begin(), encoders.end());
636  for (std::vector<int>::iterator i= encoders.begin(); i != encoders.end(); ++i) {
637  std::cout << *i << " ";
638  }
639  std::cout << std::endl;
640  break;
641 
642  case 'E': //VK_E (read force values)
643  std::cout << "\nForce values:" << std::endl;
644  for (int i = 0; i < katana->getNumberOfMotors(); ++i) {
645  std::cout << " " << (int)katana->getForce(i+1);
646  }
647  std::cout << std::endl;
648  break;
649 
650  case 'o': //VK_O (motors off/on)
651  if(IsOff) {
652  katana->switchRobotOn();
653  IsOff = false;
654  std::cout << "\nMotors on\n";
655  } else {
656  katana->switchRobotOff();
657  IsOff = true;
658  std::cout << "\nMotors off\n";
659  }
660  break;
661 
662  case 'x': //VK_O (DK to screen)
663  {
664  TPoint p;
665  katana->getCoordinates(p.X, p.Y, p.Z, p.phi, p.theta, p.psi);
666 
667  std::cout.precision(6);
668  std::cout << "\n------------------------------------\n";
669  std::cout << "X: " << p.X << "\n";
670  std::cout << "Y: " << p.Y << "\n";
671  std::cout << "Z: " << p.Z << "\n";
672  std::cout << "phi: " << RadToDeg*p.phi << "\n";
673  std::cout << "theta: " << RadToDeg*p.theta << "\n";
674  std::cout << "psi: " << RadToDeg*p.psi << "\n";
675  std::cout << "------------------------------------\n";
676 
677  break;
678  }
679  case 'q': //read sensors:
680  {
681  std::cout << "\nCurrent Sensor values:" << std::endl;
682  const TSctDAT* data = sensctrl->GetDAT();
683  bool* change = new bool[data->cnt];
684  byte* lastarr = new byte[data->cnt];
685  sensctrl->recvDAT();
686  for (int k=0; k<data->cnt; k++) { //init stuff
687  change[k] = false;
688  lastarr[k] = (byte) data->arr[k];
689  }
690  //clock_t t = clock(); //init timer
691  sensctrl->recvDAT(); //update sensor data
692  for (int i=0; i<data->cnt; i++) {
693  std::cout.width(5);
694  std::cout << data->arr[i] << " "; //printout data
695  } std::cout << "\n";
696  /*
697  while (clock() - t < 25) {} //wait 25 millisec
698 
699  for (int j=0; j<data->cnt; j++) {
700  change[j] |= (data->arr[j] != lastarr[j]);
701  lastarr[j] = data->arr[j];
702  }
703  */
704  break;
705  }
706  case 'v': //VK_V (Set max vel)
707  {
708  short velocity;
709  std::cout << "\n\nSet maximum velocity for motors to: \n";
710  for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) {
711  std::cout << motorNumber+1 << ": ";
712  std::cin >> velocity;
713  katana->setMotorVelocityLimit(motorNumber, velocity);
714  }
715  }
716  break;
717 
718  case 'V': //VK_V (Set max vel)
719  {
720  if(useLinearMode) {
721  double velocity;
722  std::cout << "\n\nSet the TCP velocity to: ";
723  std::cin >> velocity;
724  katana->setMaximumLinearVelocity(velocity);
725  katana->setRobotVelocityLimit(static_cast<short>(velocity));
726  } else {
727  short velocity;
728  std::cout << "\n\nSet maximum velocity for all motors to: ";
729  std::cin >> velocity;
730  katana->setRobotVelocityLimit(velocity);
731  katana->setMaximumLinearVelocity(static_cast<double>(velocity));
732  }
733  std::cout << std::endl;
734  }
735  break;
736 
737  case 'a': //VK_A (Set max acc)
738  {
739  short acceleration;
740  std::cout << "\n\nSet maximum velocity for motors to: \n";
741  for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) {
742  std::cout << motorNumber+1 << ": ";
743  std::cin >> acceleration;
744  katana->setMotorAccelerationLimit(motorNumber, acceleration);
745  }
746  }
747  break;
748  case 'A': //VK_A (Set max acc)
749  {
750  short acceleration;
751  std::cout << "\n\nSet maximum acceleration for all motors to: ";
752  std::cin >> acceleration;
753  std::cout << std::endl;
754  katana->setRobotAccelerationLimit(acceleration);
755  }
756  break;
757  case ',': //set force limits for all motors
758  {
759  short limit;
760  std::cout << "\nSet force limit for all motors to (%): ";
761  std::cin >> limit;
762  std::cout << std::endl;
763  katana->setForceLimit(0, limit);
764  }
765  break;
766 
767  case 'w': //VK_W (Read current max vel)
768  {
769  std::cout << "\nCurrent velocity limits:" << std::endl;
770  for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber)
771  std::cout << motorNumber+1 << ": " << katana->getMotorVelocityLimit(motorNumber) << std::endl;
772  std::cout << "linear: " << katana->getMaximumLinearVelocity() << std::endl;
773  break;
774  }
775  case 'W': //VK_W (Read current max acc)
776  {
777  std::cout << "\nCurrent acceleration limits:" << std::endl;
778  for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber)
779  std::cout << motorNumber+1 << ": " << katana->getMotorAccelerationLimit(motorNumber) << std::endl;
780  break;
781  }
782 
783  case 'l': //VK_L (switch linear mode)
784  if(useLinearMode) {
785  std::cout << "Switching to inverse kinematics movement mode\n";
786  } else {
787  std::cout << "Switching to linear movement mode\n";
788  }
789  useLinearMode = !useLinearMode;
790  break;
791  case 'y': //VK_Y (IKGoto)
792  std::cout << "\n\nInsert cartesian parameters: \n";
793  std::cout << "X: ";
794  std::cin >> arr_pos[0];
795  std::cout << "Y: ";
796  std::cin >> arr_pos[1];
797  std::cout << "Z: ";
798  std::cin >> arr_pos[2];
799  std::cout << "phi: ";
800  std::cin >> arr_pos[3];
801  std::cout << "theta: ";
802  std::cin >> arr_pos[4];
803  std::cout << "psi: ";
804  std::cin >> arr_pos[5];
805 
806  arr_pos[3] = 1/RadToDeg*arr_pos[3];
807  arr_pos[4] = 1/RadToDeg*arr_pos[4];
808  arr_pos[5] = 1/RadToDeg*arr_pos[5];
809 
810  if(useLinearMode) {
811  katana->moveRobotLinearTo(arr_pos[0], arr_pos[1], arr_pos[2], arr_pos[3], arr_pos[4], arr_pos[5]);
812  } else {
813  katana->moveRobotTo(arr_pos[0], arr_pos[1], arr_pos[2], arr_pos[3], arr_pos[4], arr_pos[5]);
814  }
815  break;
816 
817  case '<':
818  std::cout.precision(6);
819  TPoint point;
820  katana->getCoordinates(point.X, point.Y, point.Z, point.phi, point.theta, point.psi);
821  std::cout << "Point: ";
822  std::cout << " X=" << point.X;
823  std::cout << ", Y=" << point.Y;
824  std::cout << ", Z=" << point.Z;
825  std::cout << ", phi=" << RadToDeg*point.phi;
826  std::cout << ", theta="<< RadToDeg*point.theta;
827  std::cout << ", psi=" << RadToDeg*point.psi;
828  std::cout << " ... added to point list as number ";
829  std::cout << points.size() << std::endl;
830  points.push_back(point);
831  break;
832 
833  case '>':
834  std::cout.width(10);
835  std::cout.precision(3);
836  std::cout << "\nMoving to point? ";
837  unsigned int pointNumber;
838  std::cin >> pointNumber;
839  if(pointNumber >= points.size()) {
840  std::cout << "Invalid point number. You have only " << points.size() << " points in your list" << std::endl;
841  break;
842  }
843  std::cout.width(6);
844  std::cout << " x=" << points[pointNumber].X;
845  std::cout.width(6);
846  std::cout << " y=" << points[pointNumber].Y;
847  std::cout.width(6);
848  std::cout << " z=" << points[pointNumber].Z;
849  std::cout.width(6);
850  std::cout << " phi=" << RadToDeg*points[pointNumber].phi;
851  std::cout.width(6);
852  std::cout << " theta=" <<RadToDeg*points[pointNumber].theta;
853  std::cout.width(6);
854  std::cout << " psi=" << RadToDeg*points[pointNumber].psi;
855  std::cout << std::endl;
856  if(useLinearMode) {
857  katana->moveRobotLinearTo(points[pointNumber].X, points[pointNumber].Y, points[pointNumber].Z, points[pointNumber].phi, points[pointNumber].theta, points[pointNumber].psi);
858  } else {
859  katana->moveRobotTo(points[pointNumber].X, points[pointNumber].Y, points[pointNumber].Z, points[pointNumber].phi, points[pointNumber].theta, points[pointNumber].psi);
860  }
861  break;
862 
863  case 3:
864  case 4:
865  case 27: //VK_ESCAPE
866  loop = false;
867  continue;
868 
869  case ' ': //VK_SPACE
870  //Move to the next point
871  std::cout.width(10);
872  std::cout.precision(3);
873  std::cout << "Moving to point " << counter << ": ";
874  std::cout.width(6);
875  std::cout << " x=" << points[counter].X;
876  std::cout.width(6);
877  std::cout << " y=" << points[counter].Y;
878  std::cout.width(6);
879  std::cout << " z=" << points[counter].Z;
880  std::cout.width(6);
881  std::cout << " phi=" << RadToDeg*points[counter].phi;
882  std::cout.width(6);
883  std::cout << " theta=" <<RadToDeg*points[counter].theta;
884  std::cout.width(6);
885  std::cout << " psi=" << RadToDeg*points[counter].psi;
886  std::cout << std::endl;
887 
888  if(useLinearMode) {
889  katana->moveRobotLinearTo(points[counter].X, points[counter].Y, points[counter].Z, points[counter].phi, points[counter].theta, points[counter].psi);
890  } else {
891  katana->moveRobotTo(points[counter].X, points[counter].Y, points[counter].Z, points[counter].phi, points[counter].theta, points[counter].psi);
892  }
893  counter++;
894  counter = counter % ((short) points.size());
895  break;
896 
897  case 'g':
898  std::cout << "Opening gripper...\n";
899  katana->openGripper();
900  break;
901  case 'h':
902  std::cout << "Close gripper...\n";
903  katana->closeGripper();
904  break;
905  case 'n':
906  std::cout << "Set speed collision limit for motors to: \n";
907  for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) {
908  std::cout << motorNumber+1 << ": ";
909  std::cin >> limit;
910  katana->setSpeedCollisionLimit(motorNumber, limit);
911  }
912  break;
913  case 'N':
914  std::cout << "Set speed collision limit for all motors to: \n";
915  std::cin >> limit;
916  for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) {
917  katana->setSpeedCollisionLimit(motorNumber, limit);
918  }
919  break;
920  case 's':
921  std::cout << "Set position collision limit for motors to: \n";
922  for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) {
923  std::cout << motorNumber+1 << ": ";
924  std::cin >> limit;
925  katana->setPositionCollisionLimit(motorNumber, limit);
926  }
927  break;
928  case 'S':
929  std::cout << "Set position collision limit for all motors to: \n";
930  std::cin >> limit;
931  for(short motorNumber = 0; motorNumber < katana->getNumberOfMotors(); ++motorNumber) {
932  katana->setPositionCollisionLimit(motorNumber, limit);
933  }
934  break;
935  case 't':
936  std::cout << "Collision detection enabled\n";
937  katana->enableCrashLimits();
938  break;
939  case 'T':
940  std::cout << "WARNING: Collision detection disabled\n";
941  katana->disableCrashLimits();
942  break;
943  case 'u':
944  std::cout << "Unblocking motors\n";
945  katana->unBlock();
946  break;
947 
948  case 'f': {
949  using namespace std;
950 
951  cout << "Loading which file?\n";
952  string filename;
953  cin >> filename;
954 
955  ifstream listfile(filename.c_str());
956  if(!listfile) {
957  cout << "File not found or access denied." << endl;
958  break;
959  }
960 
961  string line;
962  vector<string> tokens;
963  const string delimiter = ",";
964 
965  int lines = 0;
966 
967  while(!listfile.eof()) {
968  listfile >> line;
969  string::size_type lastPos = line.find_first_not_of(delimiter, 0);
970  string::size_type pos = line.find_first_of(delimiter, lastPos);
971 
972  while (string::npos != pos || string::npos != lastPos) {
973  // Found a token, add it to the vector.
974  tokens.push_back(line.substr(lastPos, pos - lastPos));
975  // Skip delimiters. Note the "not_of"
976  lastPos = line.find_first_not_of(delimiter, pos);
977  // Find next "non-delimiter"
978  pos = line.find_first_of(delimiter, lastPos);
979  }
980 
981  TPoint point;
982  point.X = atof((tokens.at(0)).data());
983  point.Y = atof((tokens.at(1)).data());
984  point.Z = atof((tokens.at(2)).data());
985  point.phi = atof((tokens.at(3)).data())/RadToDeg;
986  point.theta = atof((tokens.at(4)).data())/RadToDeg;
987  point.psi = atof((tokens.at(5)).data())/RadToDeg;
988  points.push_back( point );
989  ++lines;
990  tokens.clear();
991  }
992  cout << lines << " points loaded.\n";
993 
994  break;
995  }
996 
997  case '=': {
998  using namespace std;
999  cout << "Which file? WARNING: Will be overwritten!\n";
1000  string filename;
1001  cin >> filename;
1002 
1003  ofstream listfile(filename.c_str(), ios_base::out);
1004 
1005  int count = 0;
1006 
1007  for(std::vector<TPoint>::iterator iter = points.begin(); iter != points.end(); ++iter) {
1008  listfile.precision(8);
1009  if (count != 0)
1010  listfile << endl;
1011  listfile << iter->X << "," << iter->Y << "," << iter->Z << ",";
1012  listfile << iter->phi << "," << iter->theta << "," << iter->psi;
1013  ++count;
1014  }
1015  cout << count << " points saved.\n";
1016  listfile.close();
1017  break;
1018  }
1019  case 'p': {
1020  using namespace std;
1021  //int available;
1022  //cout << "How many movements?\n";
1023  //int moves;
1024  //cin >> moves;
1025  std::cout << "Start playback!\n";
1026  for(int i = 1; i > 0/*i <= moves*/ ; ++i) {
1027  if(useLinearMode) {
1028  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 );
1029  }
1030  else{
1031  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 );
1032  }
1033  if (i%100 == 0) {
1034  std::cout << i << ", " << std::flush;
1035  }
1036  }
1037 
1038  break;
1039 
1040  }
1041 
1042  case 'd': {
1043  long motor;
1044  double degrees;
1045  std::cout << "\nMoving motor to degrees\n";
1046  std::cout << " motor: ";
1047  std::cin >> motor;
1048  std::cout << " degrees: ";
1049  std::cin >> degrees;
1050  if ((motor > 0) && (motor < 7)) {
1051  katana->movDegrees(motor - 1, degrees);
1052  } else {
1053  std::cout << "motor has to be a number from 1 to 6\n";
1054  }
1055 
1056  break;
1057  }
1058 
1059  case 'z': {
1060  double xoff, yoff, zoff, psioff;
1061  std::cout << "X offset (m): ";
1062  std::cin >> xoff;
1063  std::cout << "Y offset (m): ";
1064  std::cin >> yoff;
1065  std::cout << "Z offset (m): ";
1066  std::cin >> zoff;
1067  std::cout << "psi offset around x axis (rad): ";
1068  std::cin >> psioff;
1069  katana->setTcpOffset(xoff, yoff, zoff, psioff);
1070 
1071  break;
1072  }
1073 
1074  default: //Error message
1075  std::cout << "\n'" << input << "' is not a valid command.\n" << std::endl;
1076  break;
1077  }
1078 
1079  } catch (Exception &e) {
1080  std::cout << "\nERROR: " << e.message() << std::endl;
1081  }
1082 
1083  }
1084  /*//Q&D test to launch program:
1085  if(runProgram){
1086  StartProgram(0);
1087  }
1088  */
1089  return 0;
1090 }
1092 void StartProgram(int index){
1093  //Q&D test to launch program:
1094  //std::system("/home/katprog katana6M180.cfg 1");
1095  progRunning = true;
1096  pthread_create(&tid, NULL, RunProgram, (void*)&retVal);//(&tid, NULL, start_func, arg);
1097  pthread_detach(tid);
1098 }
1100 void* RunProgram(void*){
1101  //katana->calibrate();
1102  std::cout << "\nProgram running...type any key to stop after the next cycle\n";
1103  while(progRunning){
1104  if(progRunning) katana->moveRobotToEnc(Tpos::x, true);
1105  if(progRunning) katana->moveRobotToEnc(Tpos::y, true);
1106  if(progRunning) katana->moveRobotToEnc(Tpos::z, true);
1107  if(progRunning) katana->moveRobotToEnc(Tpos::u, true);
1108  if(progRunning) katana->moveRobotToEnc(Tpos::v, true);
1109  if(progRunning) katana->moveRobotToEnc(Tpos::w, true);
1110  }
1111  pthread_exit((void*) &retVal);
1112  return ((void*) &retVal);
1113 }
1115 
Tpos::wArr
static const int wArr[]
Definition: control.cpp:53
acceleration
int acceleration
Definition: wrapper_control_demo.cpp:40
Tpos::uArr
static const int uArr[]
Definition: control.cpp:53
pid_t
int pid_t
Definition: sched.h:124
device
static std::unique_ptr< CCdlSocket > device
Definition: kni_wrapper.cpp:23
TPoint::Z
double Z
Definition: control.cpp:41
TSctDAT
[DAT] sensor data
Definition: kmlSctBase.h:57
byte
unsigned char byte
type specification (8 bit)
Definition: cdlBase.h:29
RIGHT
#define RIGHT
Definition: control.cpp:35
protocol
static std::unique_ptr< CCplSerialCRC > protocol
Definition: kni_wrapper.cpp:24
Exception::message
std::string message() const
Definition: include/common/exception.h:93
Tpos::xArr
static const int xArr[]
Definition: control.cpp:53
Tpos::zArr
static const int zArr[]
Definition: control.cpp:53
katana
std::unique_ptr< CLMBase > katana
Definition: control.cpp:56
TPoint::theta
double theta
Definition: control.cpp:42
DisplayHelp
void DisplayHelp()
Definition: control.cpp:81
CSctBase
Sensor Controller class.
Definition: kmlSctBase.h:72
CCplSerialCRC
Implement the Serial-Zero protocol.
Definition: cplSerial.h:137
TPoint::phi
double phi
Definition: control.cpp:42
pthread_detach
PTW32_DLLPORT int PTW32_CDECL pthread_detach(pthread_t tid)
Tpos
Definition: control.cpp:51
ptw32_handle_t
Definition: pthread.h:566
TPoint::Y
double Y
Definition: control.cpp:41
TSctDAT::cnt
short cnt
count of sensors
Definition: kmlSctBase.h:58
mutex
pthread_mutex_t mutex
Definition: control.cpp:38
TSctDAT::arr
short * arr
sensor data
Definition: kmlSctBase.h:59
TKatMOT
[MOT] every motor&#39;s attributes
Definition: kmlMotBase.h:40
StartPointlistMovement
void StartPointlistMovement()
_getch
int _getch()
Definition: demo/common/keyboard.cpp:58
CCdlSocket
Encapsulates the socket communication device.
Definition: cdlSocket.h:65
StartProgram
void StartProgram(int index)
Definition: control.cpp:1092
Tpos::w
static std::vector< int > w
Definition: control.cpp:52
CSctBase::GetDAT
const TSctDAT * GetDAT()
Definition: kmlSctBase.h:82
Tpos::z
static std::vector< int > z
Definition: control.cpp:52
TCurrentMot
structure for the currently active axis
Definition: control.cpp:45
LEFT
#define LEFT
Definition: control.cpp:34
Tpos::x
static std::vector< int > x
Definition: control.cpp:52
TPoint::psi
double psi
Definition: control.cpp:42
CLMBase
Linear movement Class.
Definition: lmBase.h:73
main
int main(int argc, char *argv[])
Definition: control.cpp:138
Tpos::yArr
static const int yArr[]
Definition: control.cpp:53
Tpos::u
static std::vector< int > u
Definition: control.cpp:52
tid
pthread_t tid
Definition: control.cpp:74
retVal
int retVal
Definition: control.cpp:77
velocity
int velocity
Definition: wrapper_control_demo.cpp:41
progRunning
bool progRunning
Definition: control.cpp:78
points
std::vector< TPoint > points(0)
Tpos::v
static std::vector< int > v
Definition: control.cpp:52
pthread_create
PTW32_DLLPORT int PTW32_CDECL pthread_create(pthread_t *tid, const pthread_attr_t *attr, void *(*start)(void *), void *arg)
TCurrentMot::running
bool running
Definition: control.cpp:47
pthread_mutex_t
struct pthread_mutex_t_ * pthread_mutex_t
Definition: pthread.h:575
TCurrentMot::dir
bool dir
Definition: control.cpp:48
RunProgram
void * RunProgram(void *)
Definition: control.cpp:1100
TPoint::X
double X
Definition: control.cpp:41
threadPid
pid_t threadPid
Definition: control.cpp:76
Tpos::y
static std::vector< int > y
Definition: control.cpp:52
PI
const double PI
Definition: control.cpp:79
pthread_exit
PTW32_DLLPORT void PTW32_CDECL pthread_exit(void *value_ptr)
encoders
std::vector< int > encoders
Definition: kni_wrapper.cpp:29
CSctBase::recvDAT
void recvDAT()
receive data
Definition: kmlSctBase.cpp:27
Tpos::vArr
static const int vArr[]
Definition: control.cpp:53

kni
Author(s): Martin Günther
autogenerated on Fri Jun 7 2019 22:06:44