rt100.C

Go to the documentation of this file.
00001 /*!@file Devices/rt100.C Interface to a UMI rt-100 robot arm */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00005 // University of Southern California (USC) and the iLab at USC.         //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // //////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: Lior Elazary <elazary@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Devices/rt100.C $
00035 // $Id: rt100.C 7833 2007-02-01 23:01:39Z rjpeters $
00036 //
00037 
00038 #include "Devices/rt100.H"
00039 
00040 #include "Component/OptionManager.H"
00041 #include "Devices/Serial.H"
00042 
00043 // ######################################################################
00044 RT100::RT100(OptionManager& mgr, const std::string& descrName,
00045          const std::string& tagName, const char *defdev) :
00046   ModelComponent(mgr, descrName, tagName),
00047   itsPort(new Serial(mgr)),
00048   itsWristTiltVal(0),
00049   itsWristRollVal(0)
00050 {
00051   // set a default config for our serial port:
00052   itsPort->configure(defdev, 9600, "8N1", false, true, 0); //use hardware flow
00053 
00054   // attach our port as a subcomponent:
00055   addSubComponent(itsPort);
00056 
00057 }
00058 
00059 // ######################################################################
00060 RT100::~RT100()
00061 { }
00062 
00063 
00064 // ######################################################################
00065 int RT100::getNumJoints()
00066 {
00067   return NUM_JOINTS;
00068 }
00069 
00070 // ######################################################################
00071 int RT100::setJointParam(JOINTS joint, PARAM param, short int val)
00072 {
00073 
00074   switch(joint)
00075   {
00076     case BASE1:
00077       break;
00078     case BASE2:
00079       break;
00080     case WRIST1:
00081       deferredWrite(IP0, IP0WRIST1, param, val);
00082       break;
00083     case WRIST2:
00084       deferredWrite(IP0, IP0WRIST2, param, val);
00085       break;
00086     case SENSOR:
00087       break;
00088     case ELBOW:
00089       deferredWrite(IP1, IP1ELBOW, param, val);
00090       break;
00091     case SHOLDER:
00092       deferredWrite(IP1, IP1SHOLDER, param, val);
00093       break;
00094     case YAW:
00095       deferredWrite(IP1, IP1YAW, param, val);
00096       break;
00097     case ZED:
00098       deferredWrite(IP1, IP1ZED, param, val);
00099       break;
00100     case GRIPPER:
00101       deferredWrite(IP1, IP1GRIPPER, param, val);
00102       break;
00103 
00104     case ROLL_WRIST:
00105       break;
00106 
00107     case TILT_WRIST:
00108       break;
00109 
00110     default:
00111       LFATAL("Invalid joint");
00112       break;
00113 
00114   }
00115 
00116   return RT100_OK;
00117 }
00118 
00119 
00120 // ######################################################################
00121 int RT100::setJointPosition(JOINTS joint, short int position, bool immediate)
00122 {
00123   unsigned char resCode;
00124   int wrist1Pos, wrist2Pos;
00125 
00126   switch(joint)
00127   {
00128     case BASE1:
00129       break;
00130     case BASE2:
00131       break;
00132     case WRIST1:
00133       deferredWrite(IP0, IP0WRIST1, NEW_POSITION, position);
00134       break;
00135     case WRIST2:
00136       deferredWrite(IP0, IP0WRIST2, NEW_POSITION, position);
00137       break;
00138     case SENSOR:
00139       break;
00140     case ELBOW:
00141       deferredWrite(IP1, IP1ELBOW, NEW_POSITION, position);
00142       break;
00143     case SHOLDER:
00144       deferredWrite(IP1, IP1SHOLDER, NEW_POSITION, position);
00145       break;
00146     case YAW:
00147       deferredWrite(IP1, IP1YAW, NEW_POSITION, position);
00148       break;
00149     case ZED:
00150       deferredWrite(IP1, IP1ZED, NEW_POSITION, position);
00151       break;
00152     case GRIPPER:
00153       deferredWrite(IP1, IP1GRIPPER, NEW_POSITION, position);
00154       break;
00155 
00156     case ROLL_WRIST:
00157       itsWristRollVal = position;
00158       wrist1Pos = itsWristTiltVal + position;
00159       wrist2Pos = itsWristTiltVal + -1*position;
00160       deferredWrite(IP0, IP0WRIST1, NEW_POSITION, wrist1Pos);
00161       deferredWrite(IP0, IP0WRIST2, NEW_POSITION, wrist2Pos);
00162       break;
00163 
00164     case TILT_WRIST:
00165       itsWristTiltVal = position;
00166       wrist1Pos = itsWristRollVal + position;
00167       wrist2Pos = -1*itsWristRollVal + position;
00168       deferredWrite(IP0, IP0WRIST1, NEW_POSITION, wrist1Pos);
00169       deferredWrite(IP0, IP0WRIST2, NEW_POSITION, wrist2Pos);
00170       break;
00171 
00172     default:
00173       LFATAL("Invalid joint");
00174       break;
00175 
00176   }
00177 
00178   if (immediate)
00179   {
00180     rawCommand(IP0, 0xAC, &resCode);
00181     rawCommand(IP1, 0xBF, &resCode);
00182   }
00183 
00184 
00185   return RT100_OK;
00186 }
00187 
00188 int RT100::getJointPosition(JOINTS joint, short int *position)
00189 {
00190   ASSERT(position != NULL);
00191 
00192   switch (joint)
00193   {
00194     case BASE1:
00195       break;
00196     case BASE2:
00197       break;
00198     case WRIST1:
00199       immediateRead(IP0, IP0WRIST1, CURRENT_POSITION, position);
00200       break;
00201     case WRIST2:
00202       immediateRead(IP0, IP0WRIST2, CURRENT_POSITION, position);
00203       break;
00204     case ROLL_WRIST:
00205       break;
00206     case TILT_WRIST:
00207       break;
00208     case SENSOR:
00209       break;
00210     case ELBOW:
00211       immediateRead(IP1, IP1ELBOW, CURRENT_POSITION, position);
00212       break;
00213     case SHOLDER:
00214       immediateRead(IP1, IP1SHOLDER, CURRENT_POSITION, position);
00215       break;
00216     case YAW:
00217       immediateRead(IP1, IP1YAW, CURRENT_POSITION, position);
00218       break;
00219     case ZED:
00220       immediateRead(IP1, IP1ZED, CURRENT_POSITION, position);
00221       break;
00222     case GRIPPER:
00223       immediateRead(IP1, IP1GRIPPER, CURRENT_POSITION, position);
00224       break;
00225 
00226     default:
00227       LFATAL("Invalid joint");
00228       break;
00229   }
00230 
00231   return RT100_OK;
00232 }
00233 
00234 // ######################################################################
00235 int RT100::moveArm(bool waitUntilComplete)
00236 {
00237 
00238   unsigned char resCode, byte1, byte2;
00239   rawCommand(IP0, 0xAC, &resCode);
00240   rawCommand(IP1, 0xBF, &resCode);
00241 
00242   if (waitUntilComplete)
00243   {
00244     bool moveDone = false;
00245     for(int i=0; i<1000 && !moveDone; i++) //timeout
00246     {
00247       rawCommand(IP1, 0x17, &resCode, &byte1, &byte2); //general status
00248       if (!(byte1 & (1 << TASK_IN_PROGRESS))) //check if no movment in the last time
00249         moveDone = true;
00250     }
00251   }
00252 
00253 
00254   return RT100_OK;
00255 
00256 }
00257 
00258 bool RT100::moveComplete()
00259 {
00260   unsigned char resCode, byte1, byte2;
00261 
00262   rawCommand(IP1, 0x17, &resCode, &byte1, &byte2); //general status
00263   if (!(byte1 & (1 << TASK_IN_PROGRESS))) //check if no movment in the last time
00264     return true;
00265   else
00266     return false;
00267 }
00268 
00269 
00270 // ######################################################################
00271 int RT100::interpolationMove(std::vector<short int> &moveVals)
00272 {
00273   unsigned char resCode;
00274   ASSERT(moveVals.size() == NUM_JOINTS);
00275 
00276   unsigned char ip0Byte1 = 0x00, ip0Byte2 = 0x00, ip0Byte3 = 0x00;
00277   unsigned char ip1Byte1 = 0x00, ip1Byte2 = 0x00, ip1Byte3 = 0x00;
00278 
00279 
00280   for(unsigned int joint = 0; joint<NUM_JOINTS; joint++)
00281   {
00282     short int jointVal = moveVals[joint];
00283     if (jointVal > 7 || jointVal < -8)
00284       LINFO("WARANING!!! joint %i is out of range (-8 < %i < 7)", joint, moveVals[joint]);
00285 
00286     jointVal = jointVal & 0x0F; //trancate to 4 bits
00287 
00288 
00289   enum IP0MOTORS {IP0BASE1, IP0BASE2, IP0WRIST1, IP0WRIST2, IP0SENSOR};
00290     switch (joint)
00291     {
00292       case BASE1:
00293         ip0Byte3 = 0x00;
00294         break;
00295       case BASE2:
00296         ip0Byte3 = 0x00;
00297         break;
00298       case WRIST1:
00299         ip0Byte3 += jointVal;
00300         break;
00301       case WRIST2:
00302         ip0Byte3 = ip0Byte3 + (jointVal << 4);
00303         break;
00304       case ROLL_WRIST:
00305         break;
00306       case TILT_WRIST:
00307         break;
00308       case SENSOR:
00309         ip0Byte1 = 0xC0 ;
00310         break;
00311       case ELBOW:
00312         ip1Byte2 += jointVal;
00313         break;
00314       case SHOLDER:
00315         ip1Byte2 = ip1Byte2 + (jointVal << 4);
00316         break;
00317       case YAW:
00318         ip1Byte3 = ip1Byte3 + (jointVal << 4);
00319         break;
00320       case ZED:
00321         ip1Byte3 += jointVal;
00322         break;
00323       case GRIPPER:
00324         ip1Byte1 = 0xC0 + jointVal;
00325         break;
00326 
00327       default:
00328         LFATAL("Invalid joint");
00329         break;
00330     }
00331   }
00332 
00333   //only send if we have a movement
00334   if (ip0Byte1 || ip0Byte2 || ip0Byte3)
00335   {
00336     LDEBUG("Sending interpolation command to IP0: %x %x %x", ip0Byte1, ip0Byte2, ip0Byte3);
00337     rawCommand(IP0, ip0Byte1, ip0Byte2, ip0Byte3, &resCode);
00338   }
00339 
00340   if (ip1Byte1 || ip1Byte2 || ip1Byte3)
00341   {
00342     LDEBUG("Sending interpolation command to IP1: %x %x %x", ip1Byte1, ip1Byte2, ip1Byte3);
00343     rawCommand(IP1, ip1Byte1, ip1Byte2, ip1Byte3, &resCode);
00344   }
00345 
00346 
00347   return RT100_OK;
00348 
00349 }
00350 
00351 // ######################################################################
00352 int RT100::init()
00353 {
00354   unsigned char resCode;
00355   unsigned cmd;
00356   LINFO("Initalizing arm to home position...");
00357 
00358   //init
00359   cmd = 0x00;
00360   itsPort->write(&cmd,1);
00361   usleep(10000); //TODO remove
00362 
00363   //reset IPs
00364   //rawCommand(IP0, 0x20, &resCode);
00365   //rawCommand(IP1, 0x20, &resCode);
00366 
00367 
00368   //free off (enable PWM output)
00369   rawCommand(IP0, EMERGENCY_STOP + FREE_OFF, &resCode);
00370   rawCommand(IP1, EMERGENCY_STOP + FREE_OFF, &resCode);
00371 
00372 
00373   initZed();
00374   initGripper();
00375   initWrist();
00376   initSholder();
00377   initElbowYaw();
00378 
00379   //set the current position of all joints
00380   immediateWrite(IP1, IP1ELBOW, CURRENT_POSITION, 62907);
00381   immediateWrite(IP1, IP1SHOLDER, CURRENT_POSITION, 2629);
00382   immediateWrite(IP1, IP1ZED, CURRENT_POSITION, 0);
00383   immediateWrite(IP1, IP1YAW, CURRENT_POSITION, 63588);
00384   immediateWrite(IP1, IP1GRIPPER, CURRENT_POSITION, 0);
00385 
00386   immediateWrite(IP0, IP0WRIST1, CURRENT_POSITION, 62435);
00387   immediateWrite(IP0, IP0WRIST2, CURRENT_POSITION, 459);
00388 
00389   gotoHomePosition();
00390 
00391   //define home position
00392  // rawCommand(IP0, 0x21, &resCode);
00393  // rawCommand(IP1, 0x21, &resCode);
00394 
00395   return RT100_OK;
00396 }
00397 
00398 void RT100::gotoHomePosition()
00399 {
00400 
00401   unsigned char resCode;
00402   deferredWrite(IP1, IP1ELBOW, NEW_POSITION, 0);
00403   deferredWrite(IP1, IP1SHOLDER, NEW_POSITION, 0);
00404   deferredWrite(IP1, IP1ZED, NEW_POSITION, -400); //fffb
00405   deferredWrite(IP1, IP1YAW, NEW_POSITION, 0);
00406   deferredWrite(IP1, IP1GRIPPER, NEW_POSITION, 0);
00407 
00408   deferredWrite(IP0, IP0WRIST1, NEW_POSITION, 0);
00409   deferredWrite(IP0, IP0WRIST2, NEW_POSITION, 0);
00410 
00411   rawCommand(IP0, 0xAC, &resCode); //drive all motors home
00412   rawCommand(IP1, 0xBF, &resCode); //drive all motors home
00413 
00414   sleep(2);
00415 
00416 }
00417 
00418 void RT100::initZed()
00419 {
00420   unsigned char resCode, byte1, byte2;
00421   deferredWrite(IP1, IP1ZED, SPEED, 30);
00422   deferredWrite(IP1, IP1ZED, MAX_FORCE, 30);
00423 
00424   rawCommand(IP0, EMERGENCY_STOP + DEAD_STOP, &resCode);
00425   rawCommand(IP1, EMERGENCY_STOP + DEAD_STOP, &resCode);
00426 
00427   rawCommand(IP0, (unsigned char)0x80, (unsigned char)0x00, (unsigned char)0x00, &resCode); //all off
00428   rawCommand(IP1, 0x80, 0x10, 0x00, &resCode); //drive zed up
00429 
00430   //check if we hit the end stop
00431   bool hitEndStop = false;
00432   for(int i=0; i<500 && !hitEndStop; i++)
00433   {
00434     rawCommand(IP1, 0x10 + IP1ZED, &resCode, &byte1, &byte2);
00435     if (byte1 & (1 << NO_MOVEMENT)) //check if no movment in the last time
00436      hitEndStop = true;
00437   }
00438   rawCommand(IP1, (unsigned char)0x80, (unsigned char)0x00, (unsigned char)0x00, &resCode); //all off
00439 
00440   if (!hitEndStop)
00441     LFATAL("ERROR: Can not init zed");
00442 
00443   //define home position
00444   rawCommand(IP0, 0x21, &resCode);
00445   rawCommand(IP1, 0x21, &resCode);
00446 
00447 }
00448 
00449 void RT100::initGripper()
00450 {
00451   unsigned char resCode, byte1, byte2;
00452 
00453   deferredWrite(IP1, IP1GRIPPER, SPEED, 30);
00454   deferredWrite(IP1, IP1GRIPPER, MAX_FORCE, 30);
00455 
00456   rawCommand(IP0, EMERGENCY_STOP + DEAD_STOP, &resCode);
00457   rawCommand(IP1, EMERGENCY_STOP + DEAD_STOP, &resCode);
00458 
00459   rawCommand(IP0, (unsigned char)0x80, (unsigned char)0x00, (unsigned char)0x00, &resCode); //all off
00460   rawCommand(IP1, 0x80, 0x00, 0x02, &resCode); //gripper close
00461 
00462   //check if we hit the end stop
00463   bool hitEndStop = false;
00464   for(int i=0; i<500 && !hitEndStop; i++)
00465   {
00466     rawCommand(IP1, 0x10 + IP1GRIPPER, &resCode, &byte1, &byte2);
00467     if (byte1 & (1 << NO_MOVEMENT)) //check if no movment in the last time
00468      hitEndStop = true;
00469   }
00470 
00471 
00472   rawCommand(IP1, (unsigned char)0x80, (unsigned char)0x00, (unsigned char)0x00, &resCode); //all off
00473 
00474   if (!hitEndStop)
00475     LFATAL("ERROR: Can not init zed");
00476 
00477   //define home position
00478   rawCommand(IP0, 0x21, &resCode);
00479   rawCommand(IP1, 0x21, &resCode);
00480 
00481   //reload pids
00482   rawCommand(IP0, 0x22, &resCode);
00483   rawCommand(IP1, 0x22, &resCode);
00484 
00485 
00486   //open the gripper a bit and set that as the 0 position
00487 
00488   deferredWrite(IP1, IP1GRIPPER, NEW_POSITION, 30);
00489 
00490   rawCommand(IP1, EMERGENCY_STOP + DEAD_STOP, &resCode);
00491   rawCommand(IP1, 0xA0|(1<<IP1GRIPPER), &resCode); //numeric drive gripper
00492 
00493   //check if we hit the end stop
00494   bool taskComplete = false;
00495   for(int i=0; i<500 && !taskComplete; i++)
00496   {
00497     rawCommand(IP1, 0x10 + IP1GRIPPER, &resCode, &byte1, &byte2);
00498     if (byte1 & (1 << TASK_COMPLETE)) //check if no movment in the last time
00499       taskComplete = true;
00500   }
00501 
00502   //define home position
00503   rawCommand(IP0, 0x21, &resCode);
00504   rawCommand(IP1, 0x21, &resCode);
00505 
00506   //reload pids
00507   rawCommand(IP0, 0x22, &resCode);
00508   rawCommand(IP1, 0x22, &resCode);
00509 
00510 }
00511 
00512 void RT100::initWrist()
00513 {
00514   unsigned char resCode, byte1, byte2;
00515 
00516   //TODO: move elbow a bit. Is this nessesary
00517 
00518   deferredWrite(IP0, IP0WRIST1, SPEED, 80);
00519   deferredWrite(IP0, IP0WRIST2, SPEED, 80);
00520 
00521   rawCommand(IP0, EMERGENCY_STOP + DEAD_STOP, &resCode);
00522   rawCommand(IP1, EMERGENCY_STOP + DEAD_STOP, &resCode);
00523 
00524 
00525   //Rotate the wrist
00526   rawCommand(IP1, (unsigned char)0x80, (unsigned char)0x00, (unsigned char)0x00, &resCode); //all off
00527   rawCommand(IP0, 0x80, 0x60, 0x00, &resCode); //rotate wrist all the way
00528 
00529   //check if we hit the end stop
00530   bool hitEndStop = false;
00531   for(int i=0; i<500 && !hitEndStop; i++)
00532   {
00533     rawCommand(IP0, 0x10 + IP0WRIST1, &resCode, &byte1, &byte2);
00534     if (byte1 & (1 << NO_MOVEMENT)) //check if no movment in the last time
00535      hitEndStop = true;
00536   }
00537 
00538   rawCommand(IP0, (unsigned char)0x80, (unsigned char)0x00, (unsigned char)0x00, &resCode); //all off
00539 
00540   if (!hitEndStop)
00541     LFATAL("ERROR: Can not init zed");
00542 
00543   //Move the wrist down
00544   rawCommand(IP0, 0x80, 0xa0, 0x00, &resCode); //rotate wrist all the way
00545 
00546   //check if we hit the end stop
00547    hitEndStop = false;
00548   for(int i=0; i<500 && !hitEndStop; i++)
00549   {
00550     rawCommand(IP0, 0x10 + IP0WRIST1, &resCode, &byte1, &byte2);
00551     if (byte1 & (1 << NO_MOVEMENT)) //check if no movment in the last time
00552      hitEndStop = true;
00553   }
00554 
00555   rawCommand(IP0, (unsigned char)0x80, (unsigned char)0x00, (unsigned char)0x00, &resCode); //all off
00556 
00557   if (!hitEndStop)
00558     LFATAL("ERROR: Can not init zed");
00559 
00560   rawCommand(IP0, EMERGENCY_STOP + DEAD_STOP, &resCode);
00561   rawCommand(IP1, EMERGENCY_STOP + DEAD_STOP, &resCode);
00562 
00563 
00564   //define home position
00565   rawCommand(IP0, 0x21, &resCode);
00566   rawCommand(IP1, 0x21, &resCode);
00567 
00568   //reload pids
00569   rawCommand(IP0, 0x22, &resCode);
00570   rawCommand(IP1, 0x22, &resCode);
00571 
00572 }
00573 
00574 void RT100::initSholder()
00575 {
00576   unsigned char resCode, byte1, byte2;
00577   deferredWrite(IP1, IP1SHOLDER, SPEED, 30);
00578   deferredWrite(IP1, IP1SHOLDER, MAX_FORCE, 20);
00579 
00580   rawCommand(IP0, EMERGENCY_STOP + DEAD_STOP, &resCode);
00581   rawCommand(IP1, EMERGENCY_STOP + DEAD_STOP, &resCode);
00582 
00583   rawCommand(IP0, (unsigned char)0x80, (unsigned char)0x00, (unsigned char)0x00, &resCode); //all off
00584   rawCommand(IP1, 0x80, 0x04, 0x00, &resCode); //move sholder to end point
00585 
00586   //check if we hit the end stop
00587   bool hitEndStop = false;
00588   for(int i=0; i<500 && !hitEndStop; i++)
00589   {
00590     rawCommand(IP1, 0x10 + IP1SHOLDER, &resCode, &byte1, &byte2);
00591     if (byte1 & (1 << NO_MOVEMENT)) //check if no movment in the last time
00592      hitEndStop = true;
00593   }
00594   rawCommand(IP1, (unsigned char)0x80, (unsigned char)0x00, (unsigned char)0x00, &resCode); //all off
00595 
00596   if (!hitEndStop)
00597     LFATAL("ERROR: Can not init sholder");
00598 
00599   //define home position
00600   rawCommand(IP0, 0x21, &resCode);
00601   rawCommand(IP1, 0x21, &resCode);
00602 
00603   //reload pids
00604   rawCommand(IP0, 0x22, &resCode);
00605   rawCommand(IP1, 0x22, &resCode);
00606 
00607 }
00608 
00609 void RT100::initElbowYaw()
00610 {
00611   unsigned char resCode, byte1, byte2;
00612   deferredWrite(IP1, IP1ELBOW, SPEED, 30);
00613   deferredWrite(IP1, IP1ELBOW, MAX_FORCE, 20);
00614 
00615   deferredWrite(IP1, IP1YAW, SPEED, 30);
00616   deferredWrite(IP1, IP1YAW, MAX_FORCE, 20);
00617 
00618   deferredWrite(IP1, IP1YAW, ERROR_LIMIT, 1500);
00619 
00620   rawCommand(IP0, EMERGENCY_STOP + DEAD_STOP, &resCode);
00621   rawCommand(IP1, EMERGENCY_STOP + DEAD_STOP, &resCode);
00622 
00623   rawCommand(IP0, (unsigned char)0x80, (unsigned char)0x00, (unsigned char)0x00, &resCode); //all off
00624   rawCommand(IP1, 0x80, 0x82, 0x00, &resCode); //move elbow and sholder to end point
00625 
00626   //check if we hit the end stop
00627   bool hitEndStop = false;
00628   for(int i=0; i<500 && !hitEndStop; i++)
00629   {
00630     rawCommand(IP1, 0x10 + IP1YAW, &resCode, &byte1, &byte2);
00631     if (byte1 & (1 << NO_MOVEMENT)) //check if no movment in the last time
00632      hitEndStop = true;
00633   }
00634   rawCommand(IP1, (unsigned char)0x80, (unsigned char)0x00, (unsigned char)0x00, &resCode); //all off
00635 
00636   if (!hitEndStop)
00637     LFATAL("ERROR: Can not init sholder");
00638 
00639   //define home position
00640   rawCommand(IP0, 0x21, &resCode);
00641   rawCommand(IP1, 0x21, &resCode);
00642 
00643   //reload pids
00644   rawCommand(IP0, 0x22, &resCode);
00645   rawCommand(IP1, 0x22, &resCode);
00646 
00647 }
00648 
00649 // ######################################################################
00650 int RT100::shutdown()
00651 {
00652 
00653   return RT100_OK;
00654 }
00655 
00656 int RT100::deferredRead(IP ipID, unsigned int ctrl, PARAM param, short int *val)
00657 {
00658 
00659   unsigned char cmd[3];
00660   unsigned char cmdResults[3];
00661 
00662 
00663   switchIP(ipID);
00664 
00665   //first transaction, specify the ctrl to read from and the param
00666   cmd[0] = 0x06 << 4;
00667   cmd[0] += ctrl;
00668   cmd[1] = param;
00669   cmd[2] = 0xcc; //ignored
00670 
00671   LDEBUG("Sending T1: %x %x %x", cmd[0], cmd[1], cmd[2]);
00672   itsPort->write(&cmd,3);
00673   usleep(10000); //TODO remove
00674   unsigned char result = itsPort->read();
00675   if (result != 0xC0)
00676     LFATAL("Error in deferred read\n");
00677 
00678   //second transaction, specify the ctrl to read from and the param
00679   cmd[0] = 0x06 << 4;
00680   cmd[0] += 0x08 + ctrl;
00681 
00682   LDEBUG("Sending T2: %x", cmd[0]);
00683   itsPort->write(&cmd,1);
00684   usleep(10000); //TODO remove
00685   itsPort->read(cmdResults, 3);
00686   LDEBUG("Got: %x %x %x", cmdResults[0], cmdResults[1], cmdResults[2]);
00687 
00688   //todo check schecksum
00689 
00690   *val = cmdResults[2] << 8;
00691   *val += cmdResults[1];
00692 
00693   return RT100_OK;
00694 }
00695 
00696 int RT100::deferredWrite(IP ipID, unsigned int ctrl, PARAM param, short int val)
00697 {
00698 
00699   unsigned char cmd[3];
00700   unsigned char cmdResults[3];
00701   switchIP(ipID);
00702 
00703   //first transaction, set the parameter to change
00704   cmd[0] = 0x07 << 4; //set the controller for first transaction
00705   cmd[0] += ctrl;
00706   cmd[1] = param; //the paramter to change
00707   cmd[2] = 0xcc; //ignored in first transaction
00708 
00709   LDEBUG("Sending T1: %x %x %x", cmd[0], cmd[1], cmd[2]);
00710   itsPort->write(&cmd,3);
00711   usleep(10000); //TODO remove
00712   unsigned char result = itsPort->read();
00713   if (result != 0xE0)
00714     LFATAL("Error in deferred Write (sent %x,%x,%x got %x)\n",
00715         cmd[0], cmd[1], cmd[2], result);
00716 
00717   //second transaction, set the parameter value
00718   cmd[0] = 0x07 << 4; //set the controller for second transaction
00719   cmd[0] += 0x08 + ctrl;
00720   cmd[1] = val;  //LSB
00721   cmd[2] = val >> 8; //MSB
00722 
00723   LDEBUG("Sending T2: %x %x %x", cmd[0], cmd[1], cmd[2]);
00724   itsPort->write(&cmd,3);
00725   usleep(10000); //TODO remove
00726   itsPort->read(cmdResults, 3);
00727   LDEBUG("Got: %x %x %x", cmdResults[0], cmdResults[1], cmdResults[2]);
00728 
00729   //TODO check checksum
00730 
00731 
00732   return RT100_OK;
00733 }
00734 
00735 int RT100::immediateRead(IP ipID, unsigned int ctrl, PARAM param, short int *val)
00736 {
00737   unsigned char cmd[3];
00738   unsigned char cmdResults[3];
00739 
00740   switchIP(ipID);
00741 
00742   cmd[0] = 0x04 << 4;
00743   switch (param)
00744   {
00745     case CP_ERROR:
00746       cmd[0] += ctrl;
00747       break;
00748     case CURRENT_POSITION:
00749       cmd[0] += 0x08 + ctrl;
00750       break;
00751     default:
00752       LFATAL("Can not send param in immediate mode");
00753       break;
00754   }
00755 
00756   LDEBUG("Sending: %x", cmd[0]);
00757   itsPort->write(&cmd,1);
00758   usleep(10000); //TODO remove
00759   itsPort->read(cmdResults, 3);
00760   LDEBUG("Got: %x %x %x", cmdResults[0], cmdResults[1], cmdResults[2]);
00761 
00762   //todo check schecksum
00763 
00764   *val = cmdResults[2] << 8;
00765   *val += cmdResults[1];
00766 
00767   return RT100_OK;
00768 }
00769 
00770 int RT100::immediateWrite(IP ipID, unsigned int ctrl, PARAM param, short int val)
00771 {
00772 
00773   unsigned char cmd[3];
00774   switchIP(ipID);
00775 
00776   if (param != CURRENT_POSITION) LFATAL("Can only write to CURRENT_POSITION");
00777 
00778   cmd[0] = 0x05 << 4; //set the controller for first transaction
00779   cmd[0] += 0x08 + ctrl;
00780   cmd[1] = val;  //LSB
00781   cmd[2] = val >> 8; //MSB
00782 
00783   LDEBUG("Sending: %x %x %x", cmd[0], cmd[1], cmd[2]);
00784   itsPort->write(&cmd,3);
00785   usleep(10000); //TODO remove
00786   itsPort->read();
00787 
00788   //TODO check checksum
00789 
00790   return RT100_OK;
00791 }
00792 
00793 int RT100::switchIP(IP ipID)
00794 {
00795   unsigned char cmd;
00796   unsigned char cmdResults[3];
00797 
00798 
00799   //get the current IP number
00800   cmd = 0x01; //get IP identification
00801   if (itsPort->write(&cmd,1) != 1)
00802     LFATAL("Can not send command to arm");
00803   usleep(10000); //TODO remove
00804 
00805   if (itsPort->read(cmdResults, 1) != 1)
00806     LFATAL("Can not get IP id");
00807   usleep(10000); //TODO remove
00808 
00809 
00810   if ((cmdResults[0] == 0x20 && ipID == IP1) ||
00811       (cmdResults[0] == 0x21 && ipID == IP0) ) //if we are on a diffrent ip, switch
00812   {
00813     cmd = 0x29; //switch IPS
00814     if (itsPort->write(&cmd,1) != 1)
00815       LFATAL("Can not send command to arm");
00816     usleep(10000); //TODO remove
00817     if (itsPort->read(cmdResults, 1) != 1)
00818       LFATAL("Can not get results");
00819     if (cmdResults[0] != 0x00 && cmdResults[0] != 0x21)
00820       LFATAL("Can not switch IPS got: %x", cmdResults[0]);
00821   }
00822 
00823 
00824   usleep(10000);
00825 
00826   return RT100_OK;
00827 
00828 }
00829 
00830 // ######################################################################
00831 int RT100::rawCommand(IP ipID, unsigned char cmdType,
00832     unsigned char *resCode, short int *results)
00833 {
00834 
00835   unsigned char cmdResults[3];
00836   switchIP(ipID); //switch to a diffrent IP if nessesary
00837 
00838   LDEBUG("Sending: %x", cmdType);
00839   if (itsPort->write(&cmdType,1) != 1)
00840     LFATAL("Can not send command to arm");
00841   usleep(10000); //TODO remove
00842 
00843   cmdResults[0] = 0xFF;
00844   int n = itsPort->read(cmdResults, 3);
00845   *resCode = cmdResults[0];
00846   LDEBUG("Got: %x", cmdResults[0]);
00847   if (n > 1)
00848   {
00849     //TODO combin results
00850   }
00851   usleep(10000); //TODO remove
00852 
00853   return RT100_OK;
00854 }
00855 
00856 // ######################################################################
00857 int RT100::rawCommand(IP ipID, unsigned char cmdType,
00858     unsigned char *resCode, unsigned char *byte1, unsigned char *byte2)
00859 {
00860 
00861   unsigned char cmdResults[3];
00862   switchIP(ipID); //switch to a diffrent IP if nessesary
00863 
00864   LDEBUG("Sending: %x", cmdType);
00865   if (itsPort->write(&cmdType,1) != 1)
00866     LFATAL("Can not send command to arm");
00867   usleep(10000); //TODO remove
00868 
00869   cmdResults[0] = 0xFF;
00870   int n = itsPort->read(cmdResults, 3);
00871   LDEBUG("Got: %x %x %x", cmdResults[0], cmdResults[1], cmdResults[2]);
00872 
00873   *resCode = cmdResults[0];
00874   if (n > 1)
00875   {
00876     *byte1 = cmdResults[1];
00877     *byte2 = cmdResults[2];
00878   } else {
00879     *byte1 = 0xFF;
00880     *byte2 = 0xFF;
00881   }
00882 
00883   usleep(10000); //TODO remove
00884 
00885   return RT100_OK;
00886 }
00887 
00888 // ######################################################################
00889 int RT100::rawCommand(IP ipID, unsigned char cmdType,
00890     unsigned char byte1, unsigned char byte2,
00891     unsigned char *resCode)
00892 {
00893   unsigned char cmdResults;
00894   unsigned char cmd[3];
00895 
00896   switchIP(ipID); //switch to a diffrent IP if nessesary
00897 
00898   cmd[0] = cmdType;
00899   cmd[1] = byte1;
00900   cmd[2] = byte2;
00901   LDEBUG("Sending: %x %x %x", cmd[0], cmd[1], cmd[2]);
00902   if (itsPort->write(&cmd,3) != 3)
00903     LFATAL("Can not send command to arm");
00904   usleep(10000); //TODO remove
00905 
00906   cmdResults = 0xFF;
00907   itsPort->read(&cmdResults, 1);
00908   LDEBUG("Got: %x", cmdResults);
00909   *resCode = cmdResults;
00910   usleep(10000); //TODO remove
00911 
00912   return RT100_OK;
00913 }
00914 
00915 
00916 
00917 // ######################################################################
00918 /* So things look consistent in everyone's emacs... */
00919 /* Local Variables: */
00920 /* indent-tabs-mode: nil */
00921 /* End: */
Generated on Sun May 8 08:40:38 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3