$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 // 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