Scorbot.C

Go to the documentation of this file.
00001 /*!@file Devices/Scorbot.C Interfaces to the 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/Scorbot.C $
00035 // $Id: Scorbot.C 13960 2010-09-17 17:15:03Z lior $
00036 //
00037 
00038 #include "Devices/Scorbot.H"
00039 #include "Component/OptionManager.H"
00040 #include "Util/MathFunctions.H"
00041 #include "Util/Assert.H"
00042 #include "Util/Timer.H"
00043 #include "rutz/compat_snprintf.h"
00044 
00045 //void* Scorbot_Control(void *scorbot);
00046 
00047 void* Scorbot_Control(void *obj)
00048 {
00049   Scorbot *scorbot = (Scorbot*)obj;
00050   scorbot->controlLoop();
00051   return NULL;
00052 }
00053 
00054 
00055 
00056 // ######################################################################
00057 Scorbot::Scorbot(OptionManager& mgr, const std::string& descrName,
00058     const std::string& tagName,
00059     const char *defdev) :
00060   RobotArm(mgr, descrName, tagName),
00061   itsSerial(new Serial(mgr)),
00062   pidBase(0.05, 0.001, 0.02, -10, 10, 1, BASE_POS_THRESH, BASE_NEG_THRESH),
00063   pidSholder(0.05, 0.001, 0.01,-50, 50, 1, SHOLDER_POS_THRESH, SHOLDER_NEG_THRESH),
00064   pidElbow(0.05, 0.001, 0.03, -50, 50, 1, ELBOW_POS_THRESH, ELBOW_NEG_THRESH),
00065   pidWrist1(0.055, 0.01, 0.01,-50, 50, 1, WRIST_PITCH_POS_THRESH, WRIST_PITCH_NEG_THRESH),
00066   pidWrist2(0.055, 0.01, 0.01,-50, 50, 1, WRIST_ROLL_POS_THRESH, WRIST_ROLL_NEG_THRESH),
00067   pidGripper(0.025, 0.01, 0.01,-50, 50, 1, GRIPPER_POS_THRESH, GRIPPER_NEG_THRESH),
00068   pidEx1(0.010, 0, 0.009,-50, 50, 1, EX1_POS_THRESH, EX1_NEG_THRESH),
00069   pidEx2(0.0, 0, 0.0,-40, 40),
00070   itsUpperarmLength(222.0),
00071   itsForearmLength(222.0),
00072   itsUpperarmCOM(110),
00073   itsUpperarmMass(80),
00074   itsForearmCOM(200),
00075   itsForearmMass(200),
00076   itsRadPerSholderEncoder(M_PI/(10128.0*2.0)), //10234 ticks per 90 deg
00077   itsRadPerElbowEncoder(M_PI/(10128.0*2.0)), //10234 ticks per 90 deg
00078   itsMMPerSlideEncoder(500.0/139520.0),
00079   itsBaseXOffset(0),
00080   itsBaseZOffset(0),
00081   itsHomeBaseOffset(0),
00082   itsHomeSholderOffset(0),
00083   itsHomeElbowOffset(0),
00084   itsHomeWristRollBaseOffset(-1320),
00085   itsHomeWristPitchBaseOffset(-578),
00086   itsBasePos("BasePos", this, 0, ALLOW_ONLINE_CHANGES),
00087   itsSholderPos("SholderPos", this, 0, ALLOW_ONLINE_CHANGES),
00088   itsElbowPos("ElbowPos", this, 0, ALLOW_ONLINE_CHANGES),
00089   itsWrist1Pos("Wrist1Pos", this, 0, ALLOW_ONLINE_CHANGES),
00090   itsWrist2Pos("Wrist2Pos", this, 0, ALLOW_ONLINE_CHANGES),
00091   itsGripperPos("GripperPos", this, 0, ALLOW_ONLINE_CHANGES),
00092   itsEX1Pos("EX1Pos", this, 0, ALLOW_ONLINE_CHANGES),
00093   itsEX2Pos("EX2Pos", this, 0, ALLOW_ONLINE_CHANGES),
00094   itsArmXPos("ArmXPos", this, 0, ALLOW_ONLINE_CHANGES),
00095   itsArmYPos("ArmYPos", this, 0, ALLOW_ONLINE_CHANGES),
00096   itsArmZPos("ArmZPos", this, 0, ALLOW_ONLINE_CHANGES),
00097   itsMoveTo3D("MoveTo3D", this, 0, ALLOW_ONLINE_CHANGES),
00098   itsDuration("Duration", this, 2000, ALLOW_ONLINE_CHANGES),
00099   itsInternalPIDen("InternalPID", this, false, ALLOW_ONLINE_CHANGES),
00100   itsRunControl(false),
00101   itsStopControl(true),
00102   itsReadEncoders(false),
00103   itsShutdownMotorsOff(true) //Shutdown the motors during shutdown by default
00104 {
00105   itsSerial->configure (defdev, 115200, "8N1", false, false, 10000);
00106 
00107   addSubComponent(itsSerial);
00108 
00109   itsRunControl = false;
00110   pthread_mutex_init(&itsLock, NULL);
00111   pthread_mutex_init(&itsSerialLock, NULL);
00112   pthread_mutex_init(&itsPosLock, NULL);
00113 
00114   itsDesiredPos.base = (int)itsHomeBaseOffset;
00115   itsDesiredPos.sholder = -7036;
00116   itsDesiredPos.elbow = 10958;
00117   itsDesiredPos.wristRoll = -1930;
00118   itsDesiredPos.wristPitch = 4748;
00119   itsDesiredPos.ex1 = 54000;
00120 
00121   itsArmVelParams.ex1.a = itsDesiredPos.ex1;
00122   itsArmVelParams.ex1.b = 0;
00123   itsArmVelParams.ex1.c = 0;
00124   itsArmVelParams.ex1.d = 0;
00125 
00126 
00127 }
00128 
00129 // ######################################################################
00130 void Scorbot::start2()
00131 {
00132 //  itsSerial->setBlocking(false);
00133 
00134   //Start the controll thread
00135   //pthread_create(&itsControlThread, NULL, &Scorbot_Control, (void *)this);
00136 }
00137 
00138 // ######################################################################
00139 void Scorbot::stop1()
00140 {
00141   // stop our thread:
00142   while(itsRunControl == true)
00143     usleep(10000);
00144   itsRunControl = false;
00145 
00146   if (itsShutdownMotorsOff)
00147           motorsOff();
00148 }
00149 
00150 
00151 Scorbot::~Scorbot()
00152 {
00153   pthread_mutex_destroy(&itsLock);
00154 }
00155 
00156 
00157 double Scorbot::gravityComp(int sholderPos, int elbowPos)
00158 {
00159   //sholderPos =getEncoder(SHOLDER);
00160   //elbowPos = getEncoder(ELBOW);
00161 
00162   double sholderAng = (double)sholderPos*M_PI/20000.0;
00163   double elbowAng = -1*(double)elbowPos*M_PI/20000.0;
00164 
00165   double c1x = cos(sholderAng)*itsUpperarmCOM;
00166   double c1y = sin(sholderAng)*itsUpperarmCOM;
00167 
00168   double c2x = cos(sholderAng)*itsUpperarmLength + cos(elbowAng)*itsForearmCOM;
00169   double c2y = sin(sholderAng)*itsUpperarmLength + sin(elbowAng)*itsForearmCOM;
00170 
00171   double MvCv_x = (itsUpperarmMass*c1x + itsForearmMass*c2x)/2.0;
00172   double MvCv_y = (itsUpperarmMass*c1y + itsForearmMass*c2y)/2.0;
00173 
00174   double torque = sqrt(squareOf(MvCv_x) + squareOf(MvCv_y))*9.81*1000*sin(atan(MvCv_y/MvCv_x)+M_PI/2);
00175 
00176   return torque;
00177 }
00178 
00179 
00180 int Scorbot::mm2enc(double mm)
00181 {
00182   return (int)(mm*69.39);
00183 }
00184 
00185 double Scorbot::enc2mm(int encoderTicks)
00186 {
00187   return double(encoderTicks) / 69.39;
00188 }
00189 
00190 double Scorbot::enc2ang(int encoderTicks)
00191 {
00192   return 90.0/10234.0*double(encoderTicks);
00193 }
00194 
00195 int Scorbot::ang2enc(double degrees)
00196 {
00197   return int(10234.0 / 90.0 * degrees);
00198 }
00199 
00200 void Scorbot::controlLoop()
00201 {
00202   itsRunControl = true;
00203 
00204   pidBase.setSpeed(50);
00205   pidSholder.setSpeed(40);
00206   pidElbow.setSpeed(40);
00207   pidGripper.setSpeed(50);
00208   pidWrist1.setSpeed(35);
00209   pidWrist2.setSpeed(35);
00210   pidEx1.setSpeed(30);
00211   pidEx2.setSpeed(30);
00212 
00213   int moveDoneCounter = 0;
00214   int thresh = 250;
00215 
00216 
00217 
00218   while(itsRunControl)
00219   {
00220     if (!itsStopControl)
00221     {
00222       pthread_mutex_lock(&itsPosLock);
00223       double time = itsTimer.getSecs();
00224       printf("%f %i\n", time, itsTemp);
00225       itsTemp = false;
00226       if (time > itsDesiredPos.duration) //Ensure that we do not go over the time limit
00227         time = itsDesiredPos.duration;
00228 
00229 
00230       int basePos = getEncoder(BASE);
00231 
00232       int basePwm = (int)pidBase.update(itsDesiredPos.base, basePos);
00233       int baseErr = abs(itsDesiredPos.base - basePos);
00234 
00235       int sholderPos =getEncoder(SHOLDER);
00236       int sholderPwm =  (int)pidSholder.update(itsDesiredPos.sholder, sholderPos);
00237       int sholderErr = abs(itsDesiredPos.sholder - sholderPos);
00238 
00239       int elbowPos = getEncoder(ELBOW);
00240       int elbowPwm = (int)pidElbow.update(itsDesiredPos.elbow, elbowPos);
00241       int elbowErr = abs(itsDesiredPos.elbow - elbowPos);
00242 
00243 
00244       int desiredWrist1Pos = (itsDesiredPos.wristRoll + itsDesiredPos.wristPitch)/2;
00245       int desiredWrist2Pos = (itsDesiredPos.wristRoll - itsDesiredPos.wristPitch)/2;
00246 
00247       int wrist1Pos = getEncoder(WRIST1);
00248       int wrist1Pwm = (int)pidWrist1.update(desiredWrist1Pos, wrist1Pos);
00249       int wrist1Err = abs(desiredWrist1Pos - wrist1Pos);
00250 
00251       int wrist2Pos = getEncoder(WRIST2);
00252       int wrist2Pwm = (int)pidWrist2.update(desiredWrist2Pos, wrist2Pos);
00253       int wrist2Err = abs(desiredWrist2Pos - wrist2Pos);
00254 
00255       int gripperPos = getEncoder(GRIPPER);
00256       int gripperPwm = (int)pidGripper.update(itsDesiredPos.gripper, gripperPos);
00257       int gripperErr = abs(itsDesiredPos.gripper - gripperPos);
00258 
00259       int ex1Pos = getEncoder(EX1);
00260       float ex1DesiredPos = itsArmVelParams.ex1.a + itsArmVelParams.ex1.c*(time*time) + itsArmVelParams.ex1.d*(time*time*time);
00261       float ex1DesiredVel =  2*itsArmVelParams.ex1.c*(time) + 3*itsArmVelParams.ex1.d*(time*time);
00262       int ex1Pwm = (int)pidEx1.update(ex1DesiredPos, ex1DesiredVel, ex1Pos);
00263       int ex1Err = abs(itsDesiredPos.ex1 - ex1Pos);
00264 
00265       int ex2Pos = getEncoder(EX2);
00266       int ex2Pwm = (int)pidEx2.update(itsDesiredPos.ex2, ex2Pos);
00267       //int ex2Err = abs(itsDesiredPos.ex2 - ex2Pos);
00268 
00269       //double gc = gravityComp(sholderPos, elbowPos)/10000000.0;
00270       //sholderPwm -= (int)gc;
00271 
00272       //LINFO("B:%i:%i S:%i:%i E:%i:%i wr:%i:%i wp:%i:%i gr(%i):%i:%i Ex:%i:%i",
00273       //    basePwm, baseErr,
00274       //    sholderPwm, sholderErr, // gc,
00275       //    elbowPwm, elbowErr,
00276       //    wrist1Pwm, wrist1Err,
00277       //    wrist2Pwm, wrist2Err,
00278       //    itsCurrentPos.gripper, gripperPwm, gripperErr,
00279       //    ex1Pwm, ex1Err);
00280 
00281      // printf("%i %i %i %i %i %i %i \n",
00282      //     basePos, sholderPos, elbowPos, wrist1Pos, wrist2Pos, gripperPos, ex1Pos);
00283       printf("%i %f %f %i\n", ex1Pos, ex1DesiredPos, ex1DesiredVel, ex1Pwm);
00284       fflush(stdout);
00285       pthread_mutex_unlock(&itsPosLock);
00286 
00287       //Update the itsCurrentPos structure
00288 
00289       pthread_mutex_lock(&itsLock);
00290       itsCurrentPos.base = basePos;
00291       itsCurrentPos.sholder = sholderPos;
00292       itsCurrentPos.elbow = elbowPos;
00293       itsCurrentPos.gripper = gripperPos;
00294       itsCurrentPos.wrist1 = wrist1Pos;
00295       itsCurrentPos.wrist2 = wrist2Pos;
00296       itsCurrentPos.wristRoll = wrist1Pos + wrist2Pos;
00297       itsCurrentPos.wristPitch = wrist1Pos - wrist2Pos;
00298       itsCurrentPos.ex1 = ex1Pos;
00299       itsCurrentPos.ex2 = ex2Pos;
00300       pthread_mutex_unlock(&itsLock);
00301 
00302       if (moveDoneCounter > 3)
00303         itsMoveDone = true;
00304       else
00305         itsMoveDone = false;
00306 
00307       if (baseErr < thresh &&
00308           sholderErr < thresh &&
00309           elbowErr < thresh &&
00310           gripperErr < thresh &&
00311           wrist1Err < thresh &&
00312           wrist2Err < thresh &&
00313           ex1Err < 1500)
00314         moveDoneCounter++;
00315       else
00316         moveDoneCounter = 0;
00317 
00318 
00319       //LINFO("moveDone %i %i", itsMoveDone, moveDoneCounter);
00320 
00321       setMotor(BASE, (int)basePwm);
00322       setMotor(SHOLDER, (int)sholderPwm);
00323       setMotor(ELBOW, (int)elbowPwm);
00324       setMotor(GRIPPER, (int)gripperPwm);
00325       setMotor(WRIST1, wrist1Pwm);
00326       setMotor(WRIST2, wrist2Pwm);
00327       setMotor(EX1, (int)ex1Pwm);
00328       setMotor(EX2, (int)ex2Pwm);
00329 
00330 
00331     } else {
00332       if (itsReadEncoders)
00333       {
00334         pthread_mutex_lock(&itsLock);
00335         itsCurrentPos = readArmState();
00336 
00337         //itsCurrentPos.base = getEncoder(BASE);
00338         //itsCurrentPos.sholder =getEncoder(SHOLDER);
00339         //itsCurrentPos.elbow = getEncoder(ELBOW);
00340         //itsCurrentPos.gripper = getEncoder(GRIPPER);
00341         //itsCurrentPos.wrist1 = getEncoder(WRIST1);
00342         //itsCurrentPos.wrist2 = getEncoder(WRIST2);
00343         //itsCurrentPos.wristRoll = itsCurrentPos.wrist1 + itsCurrentPos.wrist2;
00344         //itsCurrentPos.wristPitch = itsCurrentPos.wrist1 - itsCurrentPos.wrist2;
00345         //itsCurrentPos.ex1 = getEncoder(EX1);
00346         //itsCurrentPos.ex2 = getEncoder(EX2);
00347         pthread_mutex_unlock(&itsLock);
00348       }
00349     }
00350 
00351     usleep(10000);
00352   }
00353 
00354   pthread_exit(0);
00355 
00356 }
00357 
00358 
00359 void Scorbot::setArmPos(ArmPos &armPos)
00360 {
00361   LINFO("Setting arm to: b:%i s:%i e:%i g:%i e1:%i e2:%i duration: %i",
00362       armPos.base, armPos.sholder, armPos.elbow, armPos.gripper,
00363       armPos.ex1, armPos.ex2, armPos.duration);
00364 
00365   unsigned char cmd[100];
00366 
00367   cmd[0] = 20;
00368   
00369   cmd[1] = (armPos.base >> 24) & 0xFF;
00370   cmd[2] = (armPos.base >> 16) & 0xFF;
00371   cmd[3] = (armPos.base >>  8) & 0xFF;
00372   cmd[4] = (armPos.base >>  0) & 0xFF;
00373 
00374   cmd[5] = (armPos.sholder >> 24) & 0xFF;
00375   cmd[6] = (armPos.sholder >> 16) & 0xFF;
00376   cmd[7] = (armPos.sholder >>  8) & 0xFF;
00377   cmd[8] = (armPos.sholder >>  0) & 0xFF;
00378 
00379   cmd[9] = (armPos.elbow >> 24) & 0xFF;
00380   cmd[10] = (armPos.elbow >> 16) & 0xFF;
00381   cmd[11] = (armPos.elbow >>  8) & 0xFF;
00382   cmd[12] = (armPos.elbow >>  0) & 0xFF;
00383 
00384   cmd[13] = (armPos.wrist1 >> 24) & 0xFF;
00385   cmd[14] = (armPos.wrist1 >> 16) & 0xFF;
00386   cmd[15] = (armPos.wrist1 >>  8) & 0xFF;
00387   cmd[16] = (armPos.wrist1 >>  0) & 0xFF;
00388 
00389   cmd[17] = (armPos.wrist2 >> 24) & 0xFF;
00390   cmd[18] = (armPos.wrist2 >> 16) & 0xFF;
00391   cmd[19] = (armPos.wrist2 >>  8) & 0xFF;
00392   cmd[20] = (armPos.wrist2 >>  0) & 0xFF;
00393 
00394 
00395   cmd[21] = (armPos.gripper >> 24) & 0xFF;
00396   cmd[22] = (armPos.gripper >> 16) & 0xFF;
00397   cmd[23] = (armPos.gripper >>  8) & 0xFF;
00398   cmd[24] = (armPos.gripper >>  0) & 0xFF;
00399 
00400 
00401   cmd[25] = (armPos.ex1 >> 24) & 0xFF;
00402   cmd[26] = (armPos.ex1 >> 16) & 0xFF;
00403   cmd[27] = (armPos.ex1 >>  8) & 0xFF;
00404   cmd[28] = (armPos.ex1 >>  0) & 0xFF;
00405 
00406   cmd[29] = (armPos.ex2 >> 24) & 0xFF;
00407   cmd[30] = (armPos.ex2 >> 16) & 0xFF;
00408   cmd[31] = (armPos.ex2 >>  8) & 0xFF;
00409   cmd[32] = (armPos.ex2 >>  0) & 0xFF;
00410 
00411 
00412   cmd[33] = (armPos.duration >> 24) & 0xFF;
00413   cmd[34] = (armPos.duration >> 16) & 0xFF;
00414   cmd[35] = (armPos.duration >>  8) & 0xFF;
00415   cmd[36] = (armPos.duration >>  0) & 0xFF;
00416 
00417   cmd[37] = 0xFF;
00418   cmd[38] = 0x00;
00419   cmd[39] = 0xFF;
00420   cmd[40] = 0x00;
00421 
00422   printf("Sending: ");
00423   for(int i=0; i<41; i++)
00424     printf("%u ", (unsigned char)cmd[i]);
00425   printf("\n");
00426 
00427   itsSerial->write(cmd, 41);
00428 
00429   usleep(50000);
00430   unsigned char buff[1024];
00431 
00432   int ret= itsSerial->read(buff, 1024);
00433   printf("Got %i: ", ret);
00434   for(int i=0; i<ret; i++)
00435     printf("%3d ", buff[i]);
00436   printf("\n");
00437 
00438 
00439 }
00440 
00441 
00442 Scorbot::ArmPos Scorbot::getArmPos()
00443 {
00444   return Scorbot::readArmState();
00445 }
00446 
00447 Scorbot::ArmPos Scorbot::getDesiredArmPos()
00448 {
00449   return itsDesiredPos;
00450 }
00451 
00452 
00453 void Scorbot::getEF_pos(float &x, float &y, float &z)
00454 {
00455 
00456   float th1 = -1*getEncoderAng(RobotArm::SHOLDER) + (M_PI/2);
00457   float th2 = getEncoderAng(RobotArm::ELBOW) - th1;
00458 
00459   x = 0.220*cos(th1) + 0.220*cos(th1+th2);
00460   y = 0;
00461   z = 0.220*sin(th1) + 0.220*sin(th1+th2);
00462 }
00463 
00464 Point3D<float> Scorbot::getEFPos(const ArmPos& armPos)
00465 {
00466   Point3D<float> efPos;
00467 
00468 
00469   float sholderAng = -1*(armPos.sholder-itsHomeSholderOffset)*itsRadPerSholderEncoder;
00470   float elbowAng = -1*(armPos.elbow-itsHomeElbowOffset)*itsRadPerElbowEncoder;
00471 
00472   //LINFO("Ang:: %i==>%f %f", armPos.sholder, sholderAng*180/M_PI, elbowAng*180/M_PI);
00473   efPos.x = armPos.ex1*itsMMPerSlideEncoder;
00474   efPos.y = itsBaseXOffset +
00475             itsUpperarmLength*sin(sholderAng+M_PI/2) +
00476             itsForearmLength*sin(-elbowAng+M_PI/2);
00477   efPos.z = itsBaseZOffset +
00478             itsUpperarmLength*cos(sholderAng+M_PI/2) +
00479             itsForearmLength*cos(-elbowAng+M_PI/2);
00480 
00481 
00482   return efPos;
00483 
00484 }
00485 
00486 Scorbot::ArmPos Scorbot::getIKArmPos(const Point3D<float>& efPos)
00487 {
00488 
00489   double y = efPos.y + itsUpperarmLength+itsForearmLength;
00490   double z = efPos.z;
00491 
00492 
00493   double c2 = ( squareOf(y) +
00494                 squareOf(z) -
00495                 squareOf(itsUpperarmLength) -
00496                 squareOf(itsForearmLength) ) /
00497               ( 2 * itsUpperarmLength * itsForearmLength );
00498   double s2 = sqrt(1 - squareOf(c2)); //+/- for the diffrent solutions
00499 
00500   //check valid disance to goal sqrt(x^2+y^2) < L1+L2
00501   //Not satafy when point is out of reach
00502   //Also, check valid (-1 < c2 < 1)
00503 
00504   double hatElbowAng = 1*atan2(s2, c2);
00505 
00506   double beta = M_PI/2 - atan2(z,y);
00507 
00508   double k1 = itsUpperarmLength+(itsForearmLength*c2);
00509   double k2 = itsForearmLength*s2;
00510 
00511   double sholderAng = 0;
00512   if (hatElbowAng > 0)
00513   {
00514     sholderAng = M_PI/2 - beta + atan2(k2,k1);
00515   } else {
00516     sholderAng = M_PI/2 - beta - atan2(k2,k1);
00517   }
00518 
00519   double elbowAng = -1*sholderAng + hatElbowAng;
00520 
00521   LINFO("Ang Shoulder: %f Elbow: %f %f", sholderAng*180/M_PI, elbowAng*180/M_PI, itsRadPerElbowEncoder);
00522 
00523   ArmPos armPos;
00524   armPos.base    =(int) itsHomeBaseOffset;
00525   armPos.sholder = ((int)(sholderAng/itsRadPerSholderEncoder)); 
00526   armPos.elbow   = ((int)(elbowAng/itsRadPerElbowEncoder));
00527   armPos.wristRoll = (int)itsCurrentPos.wristRoll;
00528   armPos.wristPitch = (int)itsCurrentPos.wristPitch;
00529   armPos.gripper = (int)itsCurrentPos.gripper;
00530   armPos.ex1 = (int)(((float)efPos.x)/itsMMPerSlideEncoder);
00531   armPos.ex2 = itsCurrentPos.ex2;
00532 
00533   return armPos;
00534 
00535 }
00536 
00537 bool Scorbot::set3DPos(bool relative, int duration )
00538 {
00539 
00540   Point3D<float> efPos(itsArmXPos.getVal(), itsArmYPos.getVal(), itsArmZPos.getVal());
00541         Scorbot::ArmPos armPos = getIKArmPos(efPos);
00542         armPos.duration = duration;
00543 
00544   LINFO("3D Pos %f %f %f", efPos.x, efPos.y, efPos.z);
00545 
00546         LINFO("Encoders: B:%d S:%d E:%d WR:%d WP:%d W1:%i W2:%i G:%d E1:%d E2:%d",
00547                         armPos.base,
00548                         armPos.sholder,
00549                         armPos.elbow, 
00550                         armPos.wristRoll,
00551                         armPos.wristPitch,
00552                         armPos.wrist1,
00553                         armPos.wrist2,
00554                         armPos.gripper,
00555                         armPos.ex1,
00556                         armPos.ex2);
00557 
00558   setArmPos(armPos);
00559   return true;
00560 }
00561 
00562 Scorbot::ArmPos Scorbot::readArmState()
00563 {
00564 
00565   unsigned char command = 22;
00566   itsSerial->write(&command, 1);
00567   std::vector<unsigned char> serialData = itsSerial->readFrame(command);
00568 
00569   if(serialData.size() != 16)
00570   {
00571     LERROR("Bad Serial Size!");
00572     return itsCurrentPos;
00573   }
00574 
00575   ArmPos currPos;
00576 
00577   currPos.base       = (short int)      ((int(0x00FF & serialData[0])  << 8) | (0x00FF & serialData[1]));
00578   currPos.sholder    = (short int)      ((int(0x00FF & serialData[2])  << 8) | (0x00FF & serialData[3]));
00579   currPos.elbow      = (short int)      ((int(0x00FF & serialData[4])  << 8) | (0x00FF & serialData[5]));
00580   currPos.wrist1     = (short int)      ((int(0x00FF & serialData[6])  << 8) | (0x00FF & serialData[7]));
00581   currPos.wrist2     = (short int)      ((int(0x00FF & serialData[8])  << 8) | (0x00FF & serialData[9]));
00582   currPos.gripper    = (short int)      ((int(0x00FF & serialData[10]) << 8) | (0x00FF & serialData[11]));
00583   currPos.ex1        = (unsigned int)   ((int(0x00FF & serialData[12]) << 8) | (0x00FF & serialData[13]))*4; //WE define the encoders by 4 for positions
00584   currPos.ex2        = (unsigned int)   ((int(0x00FF & serialData[14]) << 8) | (0x00FF & serialData[15]))*4;
00585 
00586   currPos.wristRoll  = currPos.wrist1 + currPos.wrist2;
00587   currPos.wristPitch = currPos.wrist1 - currPos.wrist2;
00588 
00589   currPos.baseMS = 0;
00590   currPos.sholderMS = 0;
00591   currPos.elbowMS = 0;
00592   currPos.wrist1MS = 0;
00593   currPos.wrist2MS = 0;
00594   currPos.gripperMS = 0;
00595   currPos.duration=0;
00596 
00597 
00598   return currPos;
00599 }
00600 
00601 bool Scorbot::enableInternalPID()
00602 {
00603   LINFO("Enable PID");
00604   unsigned char cmd = 16;
00605   itsSerial->write(&cmd, 1);
00606   return true;
00607 }
00608 
00609 bool Scorbot::disableInternalPID()
00610 {
00611   LINFO("Disable PID");
00612   unsigned char cmd = 17;
00613   itsSerial->write(&cmd, 1);
00614   return true;
00615 }
00616 
00617 bool Scorbot::setJointPos(MOTOR joint, int position, bool relative, int duration)
00618 {
00619   unsigned char cmd[14];
00620 
00621   LINFO("Joint %i position %i duration %i",
00622       joint, position, duration);
00623   cmd[0] = 19;
00624   cmd[1] = joint;
00625 
00626   cmd[2] = (position >> 24) & 0xFF;
00627   cmd[3] = (position >> 16) & 0xFF;
00628   cmd[4] = (position >>  8) & 0xFF;
00629   cmd[5] = (position >>  0) & 0xFF;
00630 
00631   cmd[6] = (duration >> 24) & 0xFF;
00632   cmd[7] = (duration >> 16) & 0xFF;
00633   cmd[8] = (duration >>  8) & 0xFF;
00634   cmd[9] = (duration >>  0) & 0xFF;
00635 
00636   cmd[10] = 0xFF;
00637   cmd[11] = 0x00;
00638   cmd[12] = 0xFF;
00639   cmd[13] = 0x00;
00640 
00641   printf("Sending: ");
00642   for(int i=0; i<14; i++)
00643     printf("%u ", (unsigned char)cmd[i]);
00644   printf("\n");
00645 
00646   itsSerial->write(&cmd, 14);
00647 
00648   sleep(1);
00649   char buff[1024];
00650 
00651   int ret= itsSerial->read(buff, 1024);
00652   printf("Got %i: ", ret);
00653   for(int i=0; i<ret; i++)
00654     printf("%u ", buff[i]);
00655   printf("\n");
00656 
00657 
00658   return true;
00659 }
00660 
00661 void Scorbot::paramChanged(ModelParamBase* const param,
00662                                    const bool valueChanged,
00663                                    ParamClient::ChangeStatus* status)
00664 {
00665   ModelComponent::paramChanged(param, valueChanged, status);
00666 
00667   if (param == &itsBasePos ) { setJointPos(BASE, itsBasePos.getVal(), false, itsDuration.getVal()); }
00668   else if (param == &itsSholderPos ) { setJointPos(SHOLDER, itsSholderPos.getVal(), false, itsDuration.getVal()); }
00669   else if (param == &itsElbowPos ) { setJointPos(ELBOW, itsElbowPos.getVal(), false, itsDuration.getVal()); }
00670   else if (param == &itsWrist1Pos ) { setJointPos(WRIST1, itsWrist1Pos.getVal(), false, itsDuration.getVal()); }
00671   else if (param == &itsWrist2Pos ) { setJointPos(WRIST2, itsWrist2Pos.getVal(), false, itsDuration.getVal()); }
00672   else if (param == &itsGripperPos ) { setJointPos(GRIPPER, itsGripperPos.getVal(), false, itsDuration.getVal()); }
00673   else if (param == &itsEX1Pos ) { setJointPos(EX1, itsEX1Pos.getVal(), false, itsDuration.getVal()); }
00674   else if (param == &itsEX2Pos ) { setJointPos(EX2, itsEX2Pos.getVal(), false, itsDuration.getVal()); }
00675   else if (param == &itsMoveTo3D ) { set3DPos(false, itsDuration.getVal()); }
00676   else if (param == &itsInternalPIDen ) {
00677           if (itsInternalPIDen.getVal())
00678                 enableInternalPID();
00679           else
00680                 disableInternalPID();
00681   }
00682 
00683 }
00684 
00685 
00686 bool Scorbot::setInternalPIDGain(MOTOR m, int P, int I, int D)
00687 {
00688   unsigned char cmd[14];
00689 
00690   cmd[0]  = 16;
00691   cmd[1]  = m;
00692 
00693   cmd[2]  = 0x00FF & (P >> 24);
00694   cmd[3]  = 0x00FF & (P >> 16);
00695   cmd[4]  = 0x00FF & (P >>  8);
00696   cmd[5]  = 0x00FF & (P >>  0);
00697 
00698   cmd[6]  = 0x00FF & (I >> 24);
00699   cmd[7]  = 0x00FF & (I >> 16);
00700   cmd[8]  = 0x00FF & (I >>  8);
00701   cmd[9]  = 0x00FF & (I >>  0);
00702 
00703   cmd[10] = 0x00FF & (D >> 24);
00704   cmd[11] = 0x00FF & (D >> 16);
00705   cmd[12] = 0x00FF & (D >>  8);
00706   cmd[13] = 0x00FF & (D >>  0);
00707 
00708   itsSerial->write(&cmd, 14);
00709   return true;
00710 }
00711 
00712 bool Scorbot::setInternalPos(ArmPos pos)
00713 {
00714   unsigned char cmd[15];
00715   LINFO("Setting Pos: %d", pos.ex1);
00716 
00717   cmd[0] = 19;
00718   cmd[1] = 6;
00719   cmd[2] = 0x00FF & (pos.ex1 >> 24);
00720   cmd[3] = 0x00FF & (pos.ex1 >> 16);
00721   cmd[4] = 0x00FF & (pos.ex1 >>  8);
00722   cmd[5] = 0x00FF & (pos.ex1 >>  0);
00723 
00724   unsigned char buf[256];
00725   itsSerial->write(&cmd, 14);
00726   usleep(1000000);
00727 
00728   int bytes_read = itsSerial->read(buf, 4);
00729 
00730   if(bytes_read == 4)
00731   {
00732     int d = ((0x00FF & buf[3]) << 24) |
00733       ((0x00FF & buf[2]) << 16) |
00734       ((0x00FF & buf[1]) << 8)  |
00735       ((0x00FF & buf[0]) << 0);
00736 
00737     int us = 80000000/1000000;
00738     int p = (-200*d)/(64*us) - 100;
00739 
00740 
00741     LINFO("MOTOR: %d", p);
00742   }
00743   else
00744     LINFO("ERROR! Read %d Bytes", bytes_read);
00745 
00746 
00747 
00748   return true;
00749 }
00750 
00751 bool Scorbot::setMotor(MOTOR m, int pwm)
00752 {
00753   unsigned char cmd[6];
00754   //unsigned char buf[255];
00755 
00756   LINFO("Set motor %i pwm %i", m, pwm);
00757   if (pwm > MAX_PWM) pwm = MAX_PWM;
00758   if (pwm < -MAX_PWM) pwm = -MAX_PWM;
00759 
00760   cmd[0] = 10; //move motor
00761 
00762   if (m == WRIST_ROLL || m == WRIST_PITCH)
00763   {
00764     cmd[1] = WRIST1; //move motor
00765     cmd[3] = 10; //move motor
00766     cmd[4] = WRIST2;
00767 
00768 
00769     if (pwm >= 0)
00770     {
00771       cmd[2] = (short int)pwm&0x7F;
00772     }
00773     else
00774     {
00775       cmd[2] = (abs((short int)pwm)&0x7F) | 0x80;
00776     }
00777 
00778     if (m == WRIST_ROLL)
00779     {
00780       if (pwm >= 0)
00781       {
00782         cmd[5] = (short int)pwm&0x7F;
00783       }
00784       else
00785       {
00786         cmd[5] = (abs((short int)pwm)&0x7F) | 0x80;
00787       }
00788     } else {
00789       if (pwm <= 0)
00790       {
00791         cmd[5] = abs((short int)pwm)&0x7F;
00792       }
00793       else
00794       {
00795         cmd[5] = (abs((short int)pwm)&0x7F) | 0x80;
00796       }
00797     }
00798 
00799     itsSerial->write(cmd, 6);
00800     usleep(10000);
00801 
00802     //int bytes_read = 0;
00803     //while(bytes_read != 2)
00804     //{
00805     //  bytes_read += itsSerial->read(buf+bytes_read, 1);
00806     //  usleep(10000);
00807     //}
00808 
00809     //if (buf[0]  != 255 && buf[1] != 255)
00810     //  return false;
00811     //else
00812       return true;
00813 
00814   } else {
00815     cmd[1] = m; //motor
00816     if (pwm >= 0)
00817       cmd[2] = (short int)pwm&0x7F;
00818     else
00819       cmd[2] = (abs((short int)pwm)&0x7F) | 0x80;
00820 
00821     itsSerial->write(cmd, 3);
00822     usleep(10000);
00823 
00824     char buf[100];
00825     int bytes_read = itsSerial->read(buf, 1);
00826     while(bytes_read != 1)
00827     {
00828       bytes_read = itsSerial->read(buf, 1);
00829       usleep(10000);
00830     }
00831     LINFO("Got %i", bytes_read);
00832     for(int i=0; i<bytes_read; i++)
00833         LINFO("Bytes %i %u", i, buf[i]);
00834 
00835     //if (buf[0]  != 255)
00836     //  return false;
00837     //else
00838       return true;
00839   }
00840 
00841   return false;
00842 
00843 
00844 }
00845 
00846 bool Scorbot::motorsOff()
00847 {
00848   unsigned char cmd[1];
00849   //unsigned char buf[1];
00850 
00851   LINFO("Turning motors off");
00852   cmd[0] = 15;
00853   itsSerial->write(cmd, 1);
00854 
00855   //itsSerial->read(buf, 1);
00856   //if (buf[0]  != 255)
00857   //  return false;
00858   //else
00859    return true;
00860 }
00861 
00862 bool Scorbot::motorsOn()
00863 {
00864   unsigned char cmd[1];
00865  // unsigned char buf[1];
00866 
00867   LINFO("Turning motors on");
00868   cmd[0] = 14;
00869   itsSerial->write(cmd, 1);
00870   usleep(50000);
00871 
00872   itsSerial->read(cmd, 1);
00873   if (cmd[0]  != 255)
00874     return false;
00875   else
00876     return true;
00877 }
00878 
00879 
00880 
00881 bool Scorbot::stopAllMotors()
00882 {
00883   unsigned char cmd[1];
00884   //unsigned char buf[1];
00885 
00886   cmd[0] = 11; //stop all motors
00887   itsSerial->write(cmd, 1);
00888 
00889   //itsSerial->read(buf, 1);
00890   //if (buf[0]  != 255)
00891   //  return false;
00892   //else
00893     return true;
00894 }
00895 
00896 
00897 
00898 int Scorbot::getMicroSwitch()
00899 {
00900   unsigned char cmd;
00901   unsigned char buf = 11;
00902  // unsigned char check_byte;
00903  // unsigned char buf;
00904   int bytes_read;
00905 
00906   std::vector<unsigned char> serialIn;
00907 
00908   cmd = 21; //get all ms
00909 
00910   itsSerial->write(&cmd, 1);
00911 
00912   while(1)
00913   {
00914     bytes_read = itsSerial->read(&buf, 1);
00915 
00916     if(bytes_read > 0)
00917     {
00918       serialIn.push_back(buf);
00919 
00920       if(serialIn.size() > 3)
00921         serialIn.erase(serialIn.begin());
00922 
00923       if(serialIn.size() == 3)
00924       {
00925         if(serialIn.at(1) == 255 && serialIn.at(2) == 99)
00926           return serialIn.at(0);
00927       }
00928     }
00929   }
00930   return buf;
00931 }
00932 
00933 char* Scorbot::getMicroSwitchByte()
00934 {
00935   return int2byte(getMicroSwitch());
00936 }
00937 bool Scorbot::getMicroSwitchMotor(RobotArm::MOTOR m)
00938 {
00939   int axis = -1;
00940   if(m == BASE)
00941     axis = 1;
00942   if(m == SHOLDER)
00943     axis = 2;
00944   if(m == ELBOW)
00945     axis = 3;
00946   if(m == WRIST_ROLL)
00947     axis = 5;
00948   if(m == WRIST_PITCH)
00949     axis = 4;
00950 
00951   return !((getMicroSwitch() >> (8-axis)) & 0x01);
00952 //  return int2array(getMicroSwitch(),m);
00953 }
00954 
00955 int Scorbot::getEncoder(MOTOR m)
00956 {
00957   std::vector<char> serialIn;
00958   unsigned char cmd[2];
00959   unsigned char buf[255];
00960 
00961   //Clear the serial port
00962   while(itsSerial->read(&buf, 255) > 0);
00963 
00964   if (m == WRIST_ROLL || m == WRIST_PITCH)
00965   {
00966     int wrist1 = getEncoder(WRIST1);
00967     int wrist2 = getEncoder(WRIST2);
00968 
00969     if (m==WRIST_PITCH)
00970       return wrist1-wrist2;
00971     else
00972       return wrist1+wrist2;
00973 
00974   } else {
00975 
00976     unsigned char axis = 0;
00977     switch(m)
00978     {
00979       case BASE:
00980         axis = 0;
00981         break;
00982       case SHOLDER:
00983         axis = 1;
00984         break;
00985       case ELBOW:
00986         axis = 2;
00987         break;
00988       case WRIST1:
00989         axis = 4;
00990         break;
00991       case WRIST2:
00992         axis = 3;
00993         break;
00994       case GRIPPER:
00995         axis = 5;
00996         break;
00997       case EX1:
00998         axis = 6;
00999         break;
01000       case EX2:
01001         axis = 7;
01002         break;
01003       default:
01004         LFATAL("UNKOWN axis: %d", axis);
01005         break;
01006     }
01007 
01008     cmd[0] = 12; //get encoder
01009 
01010     cmd[1] = axis; //motor
01011 
01012     short val = 0;
01013     itsSerial->write(cmd, 2);
01014     //LINFO("REad: %i %i", cmd[0], cmd[1]);
01015 
01016 
01017     usleep(10000);
01018     int bytes_read = itsSerial->read(buf, 3);
01019 
01020     while (bytes_read < 3)
01021     {
01022       bytes_read += itsSerial->read(buf+bytes_read, 3-bytes_read);
01023       usleep(10000);
01024     }
01025     //printf("R %i: ", bytes_read);
01026     //for(int i=0; i<bytes_read; i++)
01027     //  printf("%i ", buf[i]);
01028     //printf("\n");
01029 
01030     //while(1)
01031     //{
01032     //  bytes_read = itsSerial->read(buf, 1);
01033     //  if(bytes_read > 0 && buf[0] == 255 && serialIn.size() >= 2)
01034     //  {
01035     //    break;
01036     //  }
01037     //  else
01038     //  {
01039     //    serialIn.push_back(buf[0]);
01040 
01041     //    if(serialIn.size() > 2)
01042     //      serialIn.erase(serialIn.begin());
01043     //  }
01044     //}
01045 
01046     //val = (int(0x00FF & serialIn.at(0)) << 8) | (0x00FF & serialIn.at(1));
01047     val = (int(0x00FF & buf[0]) << 8) | (0x00FF & buf[1]);
01048 
01049     if (m == EX1)
01050       return (unsigned short)val;
01051     else
01052       return (short int)val;
01053 
01054     //if (i == 1) //Only read one byte, wait for another
01055     //{
01056     //  usleep(10000);
01057     //  i += itsSerial->read(buf+1, 2);
01058     //}
01059     //if (i == 2) //Only read one byte, wait for another
01060     //{
01061     //  usleep(10000);
01062     //  i += itsSerial->read(buf+1, 1);
01063     //}
01064 
01065 
01066 
01067     //if (i < 3 || buf[2] != 255)
01068     //{
01069     //  LINFO("Error reading encoder value read(%i %x %x %x)", i,
01070     //      buf[0], buf[1], buf[2]);
01071     //} else {
01072     //  val = (buf[0] << 8) + buf[1];
01073     //  return val;
01074     //}
01075   }
01076   //pthread_mutex_lock(&itsSerialLock);
01077   return 0;
01078 }
01079 
01080 float Scorbot::getEncoderAng(MOTOR m)
01081 {
01082 
01083   return 0; //(float)e*M_PI/(2975*2);
01084 
01085 }
01086 
01087 void Scorbot::setSafety(bool val)
01088 {
01089   unsigned char cmd[2];
01090   cmd[0] = SAFE_MODE;
01091   if (val)
01092     cmd[1] = 1;
01093   else
01094     cmd[1] = 0;
01095 
01096   itsSerial->write(cmd, 2);
01097 }
01098 
01099 bool Scorbot::resetEncoders()
01100 {
01101   unsigned char cmd[1];
01102   //unsigned char buf[1];
01103   cmd[0] = 13; //Reset encoders
01104   itsSerial->write(cmd, 1);
01105   usleep(50000);
01106 
01107   itsSerial->read(cmd, 1);
01108   if (cmd[0]  != 255)
01109     return false;
01110   else
01111     return true;
01112 
01113 }
01114 
01115 int Scorbot::getPWM(MOTOR m)
01116 {
01117   unsigned char cmd[2];
01118   unsigned char buf[2];
01119   cmd[0] = GET_PWM;
01120   cmd[1] = m; //motor
01121   itsSerial->write(cmd, 2);
01122 
01123   int i = itsSerial->read(buf, 1);
01124   if (i < 1) return -1;
01125   return buf[0];
01126 }
01127 
01128 void Scorbot::setMotorPos(MOTOR m, int pos)
01129 {
01130 
01131 
01132 }
01133 void Scorbot::resetMotorPos(void)
01134 {
01135 }
01136 
01137 void Scorbot::tunePID(MOTOR m, float p, float i, float d, bool relative)
01138 {
01139 
01140   PID<float> *pid = 0;
01141 
01142   switch(m)
01143   {
01144     case BASE:
01145       pid = &pidBase;
01146       break;
01147     case SHOLDER:
01148       pid = &pidSholder;
01149       break;
01150     case ELBOW:
01151       pid = &pidElbow;
01152       break;
01153     case GRIPPER:
01154       pid = &pidGripper;
01155       break;
01156     case EX1:
01157       pid = &pidEx1;
01158       break;
01159     case EX2:
01160       pid = &pidEx2;
01161       break;
01162     default:
01163       LERROR("INVALID TUNEPID MOTOR (%d)", m);
01164       return;
01165   }
01166 
01167   if (relative)
01168   {
01169     pid->setPIDPgain(pid->getPIDPgain() + p);
01170     pid->setPIDIgain(pid->getPIDIgain() + i);
01171     pid->setPIDDgain(pid->getPIDDgain() + d);
01172   } else {
01173     pid->setPIDPgain(p);
01174     pid->setPIDIgain(i);
01175     pid->setPIDDgain(d);
01176   }
01177   LINFO("Setting PID p=%f i=%f d=%f",
01178       pid->getPIDPgain(),
01179       pid->getPIDIgain(),
01180       pid->getPIDDgain());
01181 }
01182 
01183 
01184 //void Scorbot::homeMotors()
01185 //{
01186 //
01187 //  //First, move all joints to their limits
01188 //  int   Base_LimitSeekSpeed       = -50;
01189 ////  int   Base_MSJumpSpeed          = 100;
01190 ////  float Base_MSJumpDelay          = 3.5;
01191 ////  int   Base_MSSeekSpeed          = 15;
01192 //
01193 //  int   Shoulder_LimitSeekSpeed   = -40;
01194 ////  int   Sholder_MSJumpSpeed       = 0;
01195 ////  float Sholder_MSJumpDelay       = 1.0;
01196 ////  int   Sholder_MSSeekSpeed       = -20;
01197 //
01198 //  int   Elbow_LimitSeekSpeed      = -40;
01199 ////  int   Elbow_MSJumpSpeed         = 0;
01200 ////  float Elbow_MSJumpDelay         = 1.0;
01201 ////  int   Elbow_MSSeekSpeed         = -25;
01202 //
01203 //  int   WristPitch_LimitSeekSpeed = -40;
01204 ////  int   WristPitch_MSJumpSpeed    = 0;
01205 ////  float WristPitch_MSJumpDelay    = 1.0;
01206 ////  int   WristPitch_MSSeekSpeed    = -25;
01207 //
01208 ////  int   WristRoll_MSJumpSpeed     = 10;
01209 ////  float WristRoll_MSJumpDelay     = .1;
01210 ////  int   WristRoll_MSSeekSpeed     = -25;
01211 //
01212 //  int   Gripper_LimitSeekSpeed    = -40;
01213 //
01214 //  int   EX1_LimitSeekSpeed        = 50;
01215 //
01216 //
01217 //  unsigned int numLimitsHit = 0;
01218 //  std::map<RobotArm::MOTOR, int> limitHit;
01219 //  limitHit[BASE]        = 0;
01220 //  limitHit[SHOLDER]     = 0;
01221 //  limitHit[ELBOW]       = 0;
01222 //  limitHit[WRIST_PITCH] = 0;
01223 //  limitHit[GRIPPER]     = 0;
01224 //  limitHit[EX1]         = 0;
01225 //
01226 //  std::map<RobotArm::MOTOR, int> lastEnc;
01227 //  lastEnc[BASE]        = getEncoder(BASE);
01228 //  lastEnc[SHOLDER]     = getEncoder(SHOLDER);
01229 //  lastEnc[ELBOW]       = getEncoder(ELBOW);
01230 //  lastEnc[WRIST_PITCH] = getEncoder(WRIST_PITCH);
01231 //  lastEnc[GRIPPER]     = getEncoder(GRIPPER);
01232 //  lastEnc[EX1]         = getEncoder(EX1);
01233 //
01234 //
01235 //  setMotor(BASE,         Base_LimitSeekSpeed);
01236 //  setMotor(SHOLDER,      Shoulder_LimitSeekSpeed);
01237 //  setMotor(ELBOW,        Elbow_LimitSeekSpeed);
01238 //  setMotor(WRIST_PITCH,  WristPitch_LimitSeekSpeed);
01239 //  setMotor(GRIPPER,      Gripper_LimitSeekSpeed);
01240 //  setMotor(EX1,          EX1_LimitSeekSpeed);
01241 //  usleep(100000);
01242 //
01243 //  std::map<RobotArm::MOTOR, int>::iterator lastEncIt;
01244 //  while(numLimitsHit < limitHit.size())
01245 //  {
01246 //    for(lastEncIt = lastEnc.begin(); lastEncIt != lastEnc.end(); lastEncIt++)
01247 //    {
01248 //      LINFO("NumLimitsHit: %d/%lu", numLimitsHit, limitHit.size());
01249 //      usleep(10000);
01250 //      int currEncoder = getEncoder((*lastEncIt).first);
01251 //      int lastEncoder = (*lastEncIt).second;
01252 //      RobotArm::MOTOR axis = (*lastEncIt).first;
01253 //
01254 //
01255 //      if(abs(currEncoder - lastEncoder) == 0)
01256 //      {
01257 //        limitHit[axis] = limitHit[axis] + 1;
01258 //        if(limitHit[axis] > 20)
01259 //        {
01260 //          //Stop The Motor
01261 //          setMotor(axis, 0);
01262 //          //Record the limit as hit
01263 //          limitHit[axis] = true;
01264 //          //Increment the hit counter
01265 //          numLimitsHit++;
01266 //        }
01267 //      }
01268 //      else
01269 //      {
01270 //        limitHit[axis] = std::max(0, limitHit[axis]-1);
01271 //
01272 //      }
01273 //      lastEnc[axis] = currEncoder;
01274 //    }
01275 //  }
01276 //
01277 //  stopAllMotors();
01278 //  LINFO("DONE! NumLimitsHit: %d/%lu", numLimitsHit, limitHit.size());
01279 ////
01280 ////  LINFO("Homing Base");
01281 ////  homeMotor(BASE, Base_MSJumpSpeed, Base_MSJumpDelay, Base_MSSeekSpeed, true);
01282 ////
01283 ////  LINFO("Homing Shoulder");
01284 ////  homeMotor(SHOLDER, Sholder_MSJumpSpeed, Sholder_MSJumpDelay, Sholder_MSSeekSpeed, false);
01285 ////
01286 ////  LINFO("Homing Elbow");
01287 ////  homeMotor(ELBOW, Elbow_MSJumpSpeed, Elbow_MSJumpDelay, Elbow_MSSeekSpeed, true);
01288 ////
01289 ////  LINFO("Homing Wrist Pitch");
01290 ////  homeMotor(WRIST_PITCH,  WristPitch_MSJumpSpeed, WristPitch_MSJumpDelay, WristPitch_MSSeekSpeed, true);
01291 ////
01292 ////  LINFO("Homing Wrist Roll");
01293 ////  homeMotor(WRIST_ROLL, WristRoll_MSJumpSpeed, WristRoll_MSJumpDelay, WristRoll_MSSeekSpeed, true);
01294 ////  homeMotor(WRIST_ROLL, WristRoll_MSJumpSpeed, WristRoll_MSJumpDelay, WristRoll_MSSeekSpeed, false);
01295 //
01296 //
01297 //
01298 //
01299 //
01300 //}
01301 
01302 /*
01303  Encoder Offsets:
01304   Base:        -3860
01305   Shoulder:    -13736
01306   Elbow:        6764
01307   Wrist Roll:  -1320
01308   Wrist Pitch: -578
01309 
01310 
01311  */
01312 
01313 
01314 void Scorbot::homeMotors()
01315 {
01316   int   EX1_LimitSeekSpeed        = 40;
01317 
01318   int   Base_LimitSeekSpeed       = -50;
01319   int   Base_MSJumpSpeed          = 100;
01320   float Base_MSJumpDelay          = 3.5;
01321   int   Base_MSSeekSpeed          = 15;
01322 
01323   int   Sholder_LimitSeekSpeed    = -25;
01324   int   Sholder_MSJumpSpeed       = 70;
01325   float Sholder_MSJumpDelay       = 3.6;
01326   int   Sholder_MSSeekSpeed       = 10;
01327 
01328   int   Elbow_LimitSeekSpeed      = -35;
01329   int   Elbow_MSJumpSpeed         = 90;
01330   float Elbow_MSJumpDelay         = 1.8;
01331   int   Elbow_MSSeekSpeed         = 25;
01332 
01333   int   WristPitch_LimitSeekSpeed = 20;
01334   int   WristPitch_MSJumpSpeed    = 0;
01335   float WristPitch_MSJumpDelay    = 0.0;
01336   int   WristPitch_MSSeekSpeed    = 20;
01337 
01338   int   WristRoll1_MSSeekSpeed     = 25;
01339 
01340   int   WristRoll2_MSJumpSpeed     = 15;
01341   float WristRoll2_MSJumpDelay     = .1;
01342   int   WristRoll2_MSSeekSpeed     = 20;
01343 
01344   int   Gripper_LimitSeekSpeed     = 40;
01345 
01346   itsReadEncoders = false;
01347 
01348   LINFO("Homing Slide");
01349   homeMotor(EX1, EX1_LimitSeekSpeed, 0, 0, 0, true, false);
01350 
01351   LINFO("Homing Base");
01352   homeMotor(BASE, Base_LimitSeekSpeed, Base_MSJumpSpeed, Base_MSJumpDelay, Base_MSSeekSpeed, true, true);
01353 
01354   LINFO("Homing Shoulder");
01355   homeMotor(SHOLDER, Sholder_LimitSeekSpeed, Sholder_MSJumpSpeed, Sholder_MSJumpDelay, Sholder_MSSeekSpeed, true, true);
01356 
01357   LINFO("Homing Elbow");
01358   homeMotor(ELBOW, Elbow_LimitSeekSpeed, Elbow_MSJumpSpeed, Elbow_MSJumpDelay, Elbow_MSSeekSpeed, true, true);
01359 
01360   LINFO("Homing Wrist Pitch");
01361   homeMotor(WRIST_PITCH, WristPitch_LimitSeekSpeed, WristPitch_MSJumpSpeed, WristPitch_MSJumpDelay, WristPitch_MSSeekSpeed, true, true);
01362 
01363   LINFO("Homing Wrist Roll");
01364   homeMotor(WRIST_ROLL, 0, 0, 0, WristRoll1_MSSeekSpeed, true, true);
01365   homeMotor(WRIST_ROLL, 0, WristRoll2_MSJumpSpeed, WristRoll2_MSJumpDelay, WristRoll2_MSSeekSpeed, false, true);
01366 
01367   LINFO("Homing Gripper");
01368   homeMotor(GRIPPER, Gripper_LimitSeekSpeed, 0, 0, 0, true, false);
01369 
01370   stopAllMotors();
01371   resetEncoders();
01372   itsReadEncoders = true;
01373   LINFO("Finished Homing");
01374 }
01375 
01376 
01377 
01378 void Scorbot::homeMotor(RobotArm::MOTOR axis, int LimitSeekSpeed, int MSJumpSpeed, float MSJumpDelay, int MSSeekSpeed, bool MSStopCondition, bool checkMS)
01379 {
01380   int lastPos = -1;
01381   int currPos = -1;
01382 
01383 
01384 //  //First move the joint all the way to one side to find the mechanical limit
01385 //  LINFO("Finding Limit");
01386 //  lastPos = getEncoder(axis);
01387 //  setMotor(axis, LimitSeekSpeed);
01388 //  usleep(500000);
01389 //  currPos = getEncoder(axis);
01390 //  while(abs(currPos - lastPos) > 0) //As long as the joint is still moving, then don't stop
01391 //  {
01392 //    lastPos = currPos;
01393 //    currPos = getEncoder(axis);
01394 //    usleep(70000);
01395 //  }
01396 
01397   //Should we look for a microswitch?
01398   if(checkMS)
01399   {
01400 //    //Now, kick the joint over to get it almost to the microswitch
01401 //    LINFO("Jumping to MS");
01402 //    setMotor(axis, MSJumpSpeed);
01403 //    usleep(float(MSJumpDelay) * 1000000);
01404 
01405     //And now move until we hit the microswitch
01406     setMotor(axis, MSSeekSpeed);
01407     LINFO("Looking for MS");
01408     while(getMicroSwitchMotor(axis) != MSStopCondition);
01409   } else {
01410     LINFO("Finding Limit");
01411     lastPos = getEncoder(axis);
01412     setMotor(axis, LimitSeekSpeed);
01413     usleep(500000);
01414     currPos = getEncoder(axis);
01415     while(abs(currPos - lastPos) > 0) //As long as the joint is still moving, then don't stop
01416     {
01417       lastPos = currPos;
01418       currPos = getEncoder(axis);
01419       LINFO("Speed: %i", abs(currPos - lastPos));
01420       usleep(90000);
01421     }
01422   }
01423   //Stop the motor
01424   setMotor(axis,0);
01425 }
01426 
01427 int Scorbot::getJointPos(MOTOR m)
01428 {
01429   switch(m)
01430   {
01431     case BASE: return itsCurrentPos.base;
01432     case SHOLDER: return itsCurrentPos.sholder;
01433     case ELBOW: return itsCurrentPos.elbow;
01434     case GRIPPER: return itsCurrentPos.gripper;
01435     case WRIST_ROLL: return itsCurrentPos.wristRoll;
01436     case WRIST_PITCH: return itsCurrentPos.wristPitch;
01437     case EX1: return itsCurrentPos.ex1;
01438     case EX2: return itsCurrentPos.ex2;
01439     default:
01440               LFATAL("UNKOWN axis: %d", m);
01441               break;
01442   }
01443 
01444   return 999999;
01445 }
01446 
01447 int Scorbot::write(const void* buffer, const int nbytes)
01448 {
01449         return itsSerial->write(buffer, nbytes);
01450 }
01451 
01452 int Scorbot::read(void* buffer, const int nbytes)
01453 {
01454         return itsSerial->read(buffer, nbytes);
01455 }
01456 
01457 // ######################################################################
01458 /* So things look consistent in everyone's emacs... */
01459 /* Local Variables: */
01460 /* indent-tabs-mode: nil */
01461 /* End: */
Generated on Sun May 8 08:40:38 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3