00001
00002
00003 #include "stdafx.h"
00004
00005 #include "KNI.net.h"
00006
00007 #include<vcclr.h>
00008 #include <string>
00009 #include <exception>
00010
00011 namespace KNInet {
00012
00013 bool To_string( System::String^ source, std::string &target ) {
00014 int len = (( source->Length+1) * 2);
00015 char *ch = new char[ len ];
00016 bool result ;
00017 {
00018 pin_ptr<const wchar_t> wch = PtrToStringChars( source );
00019 result = wcstombs( ch, wch, len ) != -1;
00020 }
00021 target = ch;
00022 delete ch;
00023 return result ;
00024 }
00026 Katana::~Katana() {
00027 if(katana) delete katana;
00028 if(proto) delete proto;
00029 if(comm) delete comm;
00030 if(socket) delete socket;
00031 }
00033 Katana::Katana(System::String ^ipAddress, System::String ^configurationFile) :
00034 katana(0), socket(0), proto(0) {
00035 std::string ip, portno, configFile;
00036 To_string(ipAddress, ip);
00037
00038 To_string(configurationFile, configFile);
00039 try {
00040 socket = new CCdlSocket(const_cast<char*>(ip.c_str()), atoi("5566"));
00041 proto = new CCplSerialCRC();
00042 proto->init(socket);
00043
00044 katana = new CLMBase();
00045 katana->create(configFile.c_str(), proto);
00046 } catch(std::exception &e) {
00047 throw gcnew System::Exception(gcnew System::String(e.what()));
00048 }
00049 }
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00071 void Katana::calibrate(void) {
00072 try {
00073 katana->calibrate();
00074 } catch(std::exception &e) {
00075 throw gcnew System::Exception(gcnew System::String(e.what()));
00076 }
00077 }
00079 array<int>^ Katana::getRobotEncoders(bool refreshEncoders) {
00080 std::vector<int> encodersVec(katana->getNumberOfMotors(), 0);
00081 array<int> ^encoders = gcnew array<int>(encodersVec.size());
00082 try {
00083 katana->getRobotEncoders(encodersVec.begin(), encodersVec.end(), refreshEncoders);
00084 } catch(std::exception &e) {
00085 throw gcnew System::Exception(gcnew System::String(e.what()));
00086 }
00087 for(unsigned int i = 0; i < encodersVec.size(); ++i) {
00088 encoders[i] = encodersVec[i];
00089 }
00090 return encoders;
00091 }
00093 void Katana::moveRobotToEnc(array<int> ^encoders, bool waitUntilReached, int waitTimeout) {
00094 std::vector<int> encoderVector(katana->getNumberOfMotors(), 0);
00095 for(int i = 0; i < katana->getNumberOfMotors(); ++i) {
00096 encoderVector[i] = encoders[i];
00097 }
00098 try {
00099 katana->moveRobotToEnc(encoderVector, waitUntilReached, waitTimeout);
00100 } catch(std::exception &e) {
00101 throw gcnew System::Exception(gcnew System::String(e.what()));
00102 }
00103 }
00105 void Katana::moveMotorToEnc(int motor, int encoder, bool waitUntilReached, int waitTimeout){
00106 try {
00107 katana->moveMotorToEnc(motor, encoder, waitUntilReached, waitTimeout);
00108 } catch(std::exception &e) {
00109 throw gcnew System::Exception(gcnew System::String(e.what()));
00110 }
00111 }
00113 array<double>^ Katana::getCoordinates(bool refreshEncoders) {
00114 std::vector<double> coordinateVector(6,0);
00115 try {
00116 katana->getCoordinates(
00117 coordinateVector[0],
00118 coordinateVector[1],
00119 coordinateVector[2],
00120 coordinateVector[3],
00121 coordinateVector[4],
00122 coordinateVector[5],
00123 refreshEncoders);
00124 } catch(Exception &e) {
00125 throw gcnew System::Exception(gcnew System::String(e.what()));
00126 }
00127
00128 array<double> ^coordinates = gcnew array<double>(6);
00129 for(unsigned int i = 0; i < 6; ++i) {
00130 coordinates[i] = coordinateVector[i];
00131 }
00132 return coordinates;
00133 }
00135 void Katana::moveRobotTo(array<double> ^coordinates, bool waitUntilReached, int waitTimeout) {
00136 std::vector<double> coordinateVector(6, 0);
00137 for(unsigned int i = 0; i < 6; ++i) {
00138 coordinateVector[i] = coordinates[i];
00139 }
00140 try {
00141 katana->moveRobotTo(coordinateVector, waitUntilReached, waitTimeout);
00142 } catch(std::exception &e) {
00143 throw gcnew System::Exception(gcnew System::String(e.what()));
00144 }
00145 }
00147 void Katana::moveRobotLinearTo(array<double> ^coordinates, bool waitUntilReached, int waitTimeout) {
00148 std::vector<double> coordinateVector(6, 0);
00149 for(unsigned int i = 0; i < 6; ++i) {
00150 coordinateVector[i] = coordinates[i];
00151 }
00152 try {
00153 katana->moveRobotLinearTo(coordinateVector, waitUntilReached, waitTimeout);
00154 } catch(std::exception &e) {
00155 throw gcnew System::Exception(gcnew System::String(e.what()));
00156 }
00157 }
00159 void Katana::setMaximumLinearVelocity(double maximumVelocity) {
00160 katana->setMaximumLinearVelocity(maximumVelocity);
00161 }
00163 double Katana::getMaximumLinearVelocity() {
00164 return katana->getMaximumLinearVelocity();
00165 }
00167 void Katana::setActivatePositionController(bool activate) {
00168 katana->setActivatePositionController(activate);
00169 }
00171 bool Katana::getActivatePositionController() {
00172 return katana->getActivatePositionController();
00173 }
00175 void Katana::enableCollisionLimits() {
00176 try {
00177 katana->enableCrashLimits();
00178 } catch(std::exception &e) {
00179 throw gcnew System::Exception(gcnew System::String(e.what()));
00180 }
00181 }
00183 void Katana::disableCollisionLimits() {
00184 try {
00185 katana->disableCrashLimits();
00186 } catch(std::exception &e) {
00187 throw gcnew System::Exception(gcnew System::String(e.what()));
00188 }
00189 }
00191 void Katana::unBlock() {
00192 try {
00193 katana->unBlock();
00194 } catch(std::exception &e) {
00195 throw gcnew System::Exception(gcnew System::String(e.what()));
00196 }
00197 }
00198 void Katana::setCollisionLimit(int number, int limit) {
00199 try {
00200 katana->setCrashLimit(number, limit);
00201 } catch(std::exception &e) {
00202 throw gcnew System::Exception(gcnew System::String(e.what()));
00203 }
00204 }
00206 int Katana::getNumberOfMotors() {
00207 return katana->getNumberOfMotors();
00208 }
00210 int Katana::getMotorEncoders(int number, bool refreshEncoders) {
00211 try {
00212 return katana->getMotorEncoders(number, refreshEncoders);
00213 } catch(std::exception &e) {
00214 throw gcnew System::Exception(gcnew System::String(e.what()));
00215 }
00216 }
00218 int Katana::getMotorVelocityLimit(int number) {
00219 return katana->getMotorVelocityLimit(number);
00220 }
00222 int Katana::getMotorAccelerationLimit(int number) {
00223 return katana->getMotorAccelerationLimit(number);
00224 }
00226 void Katana::setMotorVelocityLimit(int number, int velocity) {
00227 katana->setMotorVelocityLimit(number, velocity);
00228 }
00230 void Katana::setMotorAccelerationLimit(int number, int acceleration) {
00231 katana->setMotorAccelerationLimit(number, acceleration);
00232 }
00234 void Katana::openGripper(bool waitUntilReached, int waitTimeout) {
00235 try {
00236 katana->openGripper(waitUntilReached, waitTimeout);
00237 } catch(std::exception &e) {
00238 throw gcnew System::Exception(gcnew System::String(e.what()));
00239 }
00240 }
00242 void Katana::closeGripper(bool waitUntilReached, int waitTimeout) {
00243 try {
00244 katana->closeGripper(waitUntilReached, waitTimeout);
00245 } catch(std::exception &e) {
00246 throw gcnew System::Exception(gcnew System::String(e.what()));
00247 }
00248 }
00250 void Katana::freezeRobot() {
00251 try {
00252 katana->freezeRobot();
00253 } catch(std::exception &e) {
00254 throw gcnew System::Exception(gcnew System::String(e.what()));
00255 }
00256 }
00258 void Katana::freezeMotor(int number) {
00259 try {
00260 katana->freezeMotor(number);
00261 } catch(std::exception &e) {
00262 throw gcnew System::Exception(gcnew System::String(e.what()));
00263 }
00264 }
00266 void Katana::switchRobotOn() {
00267 try {
00268 katana->switchRobotOn();
00269 } catch(std::exception &e) {
00270 throw gcnew System::Exception(gcnew System::String(e.what()));
00271 }
00272 }
00274 void Katana::switchRobotOff() {
00275 try {
00276 katana->switchRobotOff();
00277 } catch(std::exception &e) {
00278 throw gcnew System::Exception(gcnew System::String(e.what()));
00279 }
00280 }
00282 void Katana::switchMotorOn(int number) {
00283 try {
00284 katana->switchMotorOn(number);
00285 } catch(std::exception &e) {
00286 throw gcnew System::Exception(gcnew System::String(e.what()));
00287 }
00288 }
00290 void Katana::switchMotorOff(int number) {
00291 try {
00292 katana->switchMotorOff(number);
00293 } catch(std::exception &e) {
00294 throw gcnew System::Exception(gcnew System::String(e.what()));
00295 }
00296 }
00298 }