wrapper_control_demo.cpp
Go to the documentation of this file.
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 // wrapper_control.cpp
00019 // demo program for the kni C wrapper dll       
00020 // PKE/JHA/TPE 2008
00022 #include "kniBase.h"
00023 #include "kni_wrapper/kni_wrapper.h"
00024 #include "kniBase.h"
00025 #include <iostream>
00026 #include <cstdio>
00027 #include <memory>
00028 #include <vector>
00030 //defines
00031 #ifdef WIN32
00032 #       include <conio.h>
00033 #else //LINUX
00034 #       include "keyboard.h"
00035 #endif
00036 
00037 //globals:
00038 const int DEFAULT_ACCELERATION = 2;
00039 const int DEFAULT_VELOCITY = 50;
00040 int acceleration = DEFAULT_ACCELERATION;
00041 int velocity = DEFAULT_VELOCITY;
00042 std::vector<TMovement*> movement_vector;
00043 bool isOff = false;
00045 //helper functions
00046 TMovement* allocateMovement() {
00047         TMovement *movement = (struct TMovement*)malloc(sizeof(TMovement));
00048         TPos *pos = (struct TPos*)malloc(sizeof(TPos));
00049         movement->pos = *pos;
00050         movement_vector.push_back(movement);
00051         return movement;
00052 }
00053 void freeAllMovements() {
00054         for (int i = 0; i < (int)movement_vector.size(); ++i) {
00055                 free(movement_vector.at(i));
00056         }
00057 }
00058 void displayTrajectoryHelp() {
00059         std::cout << "--- Trajectory Menu ------------------------------" << std::endl;
00060         std::cout << "?: Display this help     o: Switch on/off         " << std::endl;
00061         std::cout << "v: Change velocity       a: Change acceleration   " << std::endl;
00062         std::cout << "p: Add PTP movement      l: Add LINEAR movement   " << std::endl;
00063         std::cout << "u: Unblock               Esc: Exit to main menu   " << std::endl;
00064         std::cout << "--------------------------------------------------" << std::endl;
00065         std::cout << std::endl;
00066 }
00067 void displayMainHelp() {
00068         std::cout << "--- Main Menu ------------------------------------" << std::endl;
00069         std::cout << "?: Display this help     o: Switch on/off         " << std::endl;
00070         std::cout << "t: Add trajectory        r: Run trajectory        " << std::endl;
00071         std::cout << "u: Unblock               f: Flush movebuffers     " << std::endl;
00072         std::cout << "Esc: Exit program                                 " << std::endl;
00073         std::cout << "--------------------------------------------------" << std::endl;
00074         std::cout << std::endl;
00075 }
00076 void addTrajectory() {
00077         std::cout << "Name of Trajectory to create / add? ";
00078         std::string name_str;
00079         std::cin >> name_str;
00080         char * name = new char(strlen(name_str.c_str()));
00081         strcpy(name,name_str.c_str());
00082         std::cout << std::endl;
00083         std::cout << "current velocity: " << velocity << ", current acceleration: " << acceleration << std::endl;
00084         std::cout << std::endl;
00085         displayTrajectoryHelp();
00086         bool loop = true;
00087         int input, new_value;
00088         TMovement *current_move;
00089         while (loop) {
00090                 input = _getch();
00091                 switch (input) {
00092                 case 3:
00093                 case 4:
00094                 case 27: //VK_ESCAPE 
00095                         loop = false;
00096                         continue;
00097                 case '?':
00098                         displayTrajectoryHelp();
00099                         break;
00100                 case 'o': //VK_O (motors off/on)
00101                         if(isOff) {
00102                                 allMotorsOn();
00103                                 isOff = false;
00104                                 std::cout << "Motors on" << std::endl << std::endl;
00105                         } else {
00106                                 allMotorsOff();
00107                                 isOff = true;
00108                                 std::cout << "Motors off" << std::endl << std::endl;
00109                         }
00110                         break;
00111                 case 'v': //VK_V (change velocity)
00112                         std::cout << "New velocity: ";
00113                         std::cin >> new_value;
00114                         if ((new_value >= 10) && (new_value <= 200)) {
00115                                 velocity = new_value;
00116                                 std::cout << "   ... OK" << std::endl << std::endl;
00117                         } else {
00118                                 std::cout << "   ... FAILED, has to be min. 10 and max. 200" << std::endl << std::endl;
00119                         }
00120                         break;
00121                 case 'a': //VK_A (change acceleration)
00122                         std::cout << "New acceleration: ";
00123                         std::cin >> new_value;
00124                         if ((new_value >= 1) && (new_value <= 2)) {
00125                                 acceleration = new_value;
00126                                 std::cout << "   ... OK" << std::endl << std::endl;
00127                         } else {
00128                                 std::cout << "   ... FAILED, has to be min. 1 and max. 2" << std::endl << std::endl;
00129                         }
00130                         break;
00131                 case 'p': //VK_P (add PTP movement to here)
00132                         current_move = allocateMovement();      
00133                         getPosition(&(current_move->pos));
00134                         current_move->transition = PTP;
00135                         current_move->velocity = velocity;
00136                         current_move->acceleration = acceleration;
00137                         pushMovementToStack(current_move, name);
00138                         std::cout << "Added PTP movement with v=" << velocity << " and a=" << acceleration << " to point:" << std::endl;
00139                         std::cout.precision(3);
00140                         std::cout.setf(std::ios::fixed,std::ios::floatfield);
00141                         std::cout << " X=" << current_move->pos.X;
00142                         std::cout << ", Y=" << current_move->pos.Y;
00143                         std::cout << ", Z=" << current_move->pos.Z;
00144                         std::cout << ", phi=" << current_move->pos.phi;
00145                         std::cout << ", theta=" << current_move->pos.theta;
00146                         std::cout << ", psi=" << current_move->pos.psi;
00147                         std::cout << std::endl << std::endl;
00148                         break;
00149                 case 'l': //VK_L (add LINEAR movement to here)
00150                         current_move = allocateMovement();      
00151                         getPosition(&(current_move->pos));
00152                         current_move->transition = LINEAR;
00153                         current_move->velocity = velocity;
00154                         current_move->acceleration = acceleration;
00155                         pushMovementToStack(current_move, name);
00156                         std::cout << "Added LINEAR movement with v=" << velocity << " and a=" << acceleration << " to point:" << std::endl;
00157                         std::cout.precision(3);
00158                         std::cout.setf(std::ios::fixed,std::ios::floatfield);
00159                         std::cout << " X=" << current_move->pos.X;
00160                         std::cout << ", Y=" << current_move->pos.Y;
00161                         std::cout << ", Z=" << current_move->pos.Z;
00162                         std::cout << ", phi=" << current_move->pos.phi;
00163                         std::cout << ", theta=" << current_move->pos.theta;
00164                         std::cout << ", psi=" << current_move->pos.psi;
00165                         std::cout << std::endl << std::endl;
00166                         break;          
00167                 case 'u': //VK_U (unblock)
00168                         unblock();
00169                         std::cout << "Unblocked" << std::endl << std::endl;
00170                         break;
00171                 default: //Error message
00172                         std::cout << "'" << input << "' is not a valid command." << std::endl << std::endl;
00173                         break;
00174                 }
00175         }       
00176 }
00178 int main(int argc, char *argv[]) {
00179         if (argc != 3) {
00180                 std::cout << "usage: wrapper_control CONFIGFILE IP_ADDR" << std::endl;
00181                 return 0;
00182         }
00183         try {
00184                 initKatana(argv[1], argv[2]);
00185                 calibrate(0);
00186 
00187         } catch(Exception &e) {
00188                 std::cout << "ERROR: " << e.message() << std::endl;
00189                 return -1;
00190         }
00191         std::cout << "-------------------------------------------" << std::endl;
00192         std::cout << "success: katana initiated" << std::endl;
00193         std::cout << "-------------------------------------------" << std::endl;
00195         try{
00196                 displayMainHelp();
00197                 bool loop = true;
00198                 int input, repetitions;
00199                 std::string name_str;
00200                 char * name;
00201                 while (loop) {
00202                         input = _getch();
00203                         switch (input) {
00204                         case 3:
00205                         case 4:
00206                         case 27: //VK_ESCAPE 
00207                                 loop = false;
00208                                 continue;
00209                         case '?':
00210                                 displayMainHelp();
00211                                 break;
00212                         case 'o': //VK_O (motors off/on)
00213                                 if(isOff) {
00214                                         allMotorsOn();
00215                                         isOff = false;
00216                                         std::cout << "Motors on" << std::endl << std::endl;
00217                                 } else {
00218                                         allMotorsOff();
00219                                         isOff = true;
00220                                         std::cout << "Motors off" << std::endl << std::endl;
00221                                 }
00222                                 break;
00223                         case 't': //VK_O (DK to screen)
00224                                 addTrajectory();
00225                                 displayMainHelp();
00226                                 break;
00227                         case 'r':
00228                                 std::cout << "Name of Trajectory to run? ";
00229                                 std::cin >> name_str;
00230                                 name = new char(strlen(name_str.c_str()));
00231                                 strcpy(name,name_str.c_str());
00232                                 std::cout << std::endl;
00233                                 std::cout << "Number of repetitions? ";
00234                                 std::cin >> repetitions;
00235                                 std::cout << std::endl;
00236                                 runThroughMovementStack(name, repetitions);
00237                                 displayMainHelp();
00238                                 break;
00239                         case 'u': //VK_U (unblock)
00240                                 unblock();
00241                                 std::cout << "Unblocked" << std::endl << std::endl;
00242                                 break;
00243                         case 'f': //VK_F (flush)
00244                                 flushMoveBuffers();
00245                                 std::cout << "MoveBuffers flushed" << std::endl << std::endl;
00246                                 break;
00247                         default: //Error message
00248                                 std::cout << "\n'" << input << "' is not a valid command.\n" << std::endl << std::endl;
00249                                 break;
00250                         }
00251 
00252                 }
00253                 
00254                 // simple movement (was first try)
00255                 /*TMovement *current_move = allocateMovement(); 
00256                 getPosition(&(current_move->pos));
00257                 current_move->transition = PTP;
00258                 current_move->velocity = velocity;
00259                 current_move->acceleration = acceleration;
00260                 current_move->pos.Z += 50;
00261                 executeMovement(current_move);*/
00262         }
00263         catch(Exception &e) {
00264                 std::cout << "ERROR: " << e.message() << std::endl;
00265                 freeAllMovements();
00266                 return -1;
00267         }
00268         freeAllMovements();
00269         return 0;
00270 }
00272 
00273 
00274 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Tue May 28 2013 14:52:55