test-Scorbot.C

Go to the documentation of this file.
00001 /*!@file AppDevices/test-Scorbot.C Test the scorbot robot arm controller */
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/AppDevices/test-Scorbot.C $
00035 // $Id: test-Scorbot.C 13961 2010-09-17 17:18:58Z lior $
00036 //
00037 
00038 #include "Component/ModelManager.H"
00039 #include "Devices/Scorbot.H"
00040 #include "Util/MathFunctions.H"
00041 #include "Util/Types.H"
00042 #include "Util/log.H"
00043 #include "Media/FrameSeries.H"
00044 #include "Transport/FrameInfo.H"
00045 #include "Raster/GenericFrame.H"
00046 #include "GUI/XWinManaged.H"
00047 #include "GUI/ImageDisplayStream.H"
00048 #include "GUI/PrefsWindow.H"
00049 #include "Image/DrawOps.H"
00050 #include <unistd.h>
00051 #include <stdio.h>
00052 #include <signal.h>
00053 
00054 #define KEY_UP 98 
00055 #define KEY_DOWN 104
00056 #define KEY_LEFT 100
00057 #define KEY_RIGHT 102
00058 
00059 
00060 void home(nub::ref<OutputFrameSeries> &ofs);
00061 void calMotor(Scorbot::MOTOR m,nub::ref<OutputFrameSeries> &ofs, int speed = 75);
00062 nub::soft_ref<Scorbot> scorbot;
00063 int motorStep = 0;
00064 int motorCmd[10][6];
00065 
00066 std::vector<Scorbot::ArmPos> armPositions;
00067 
00068 void terminate(int s)
00069 {
00070         LINFO("*** INTERRUPT ***");
00071         scorbot->stopAllMotors();
00072         scorbot->motorsOff();
00073         sleep(1);
00074         exit(0);
00075 }
00076 
00077 int getKey(nub::ref<OutputFrameSeries> &ofs)
00078 {
00079         const nub::soft_ref<ImageDisplayStream> ids =
00080                 ofs->findFrameDestType<ImageDisplayStream>();
00081 
00082         const rutz::shared_ptr<XWinManaged> uiwin =
00083                 ids.is_valid()
00084                 ? ids->getWindow("Output")
00085                 : rutz::shared_ptr<XWinManaged>();
00086         return uiwin->getLastKeyPress();
00087 }
00088 
00089 Image<PixRGB<byte> > getDisplay(bool motorsOn)
00090 {
00091         char msg[255];
00092         Image<PixRGB<byte> > disp(512,256,ZEROS);
00093         disp.clear(PixRGB<byte>(255, 255, 255));
00094         writeText(disp, Point2D<int>(0,0), "Scorbot Control");
00095         sprintf(msg, "1-6 & q-y move joints");
00096         writeText(disp, Point2D<int>(0,20), msg);
00097 
00098         sprintf(msg, "p to show encoders, n to show micro switch, m to activate motors");
00099         writeText(disp, Point2D<int>(0,40), msg);
00100 
00101         sprintf(msg, "Motors state %i", motorsOn );
00102         writeText(disp, Point2D<int>(0,60), msg);
00103 
00104   return disp;
00105 }
00106 
00107 
00108 
00109 void moveToPositions()
00110 {
00111 //B:-3992 S:-1006 E:0 WR:121 WP:1595 W1:858 W2:-737 G:-2 E1:64 E2:0
00112 //B:102 S:-1006 E:780 WR:-29 WP:1437 W1:704 W2:-733 G:-2 E1:76752 E2:0
00113 //B:4526 S:-1006 E:780 WR:-17 WP:1765 W1:874 W2:-891 G:-2 E1:157468 E2:0
00114 //B:4526 S:-3115 E:2845 WR:168 WP:1688 W1:928 W2:-760 G:-2 E1:157460 E2:0
00115 //B:250 S:-3115 E:2846 WR:143 WP:1339 W1:741 W2:-598 G:-2 E1:79668 E2:0
00116 //B:-4198 S:-3115 E:2846 WR:-33 WP:1747 W1:857 W2:-890 G:-2 E1:8 E2:0
00117 //B:-4034 S:-4756 E:4318 WR:67 WP:1861 W1:964 W2:-897 G:-2 E1:16 E2:0
00118 //B:-78 S:-4757 E:4320 WR:180 WP:1340 W1:760 W2:-580 G:-2 E1:75764 E2:0
00119 //B:4432 S:-4758 E:4319 WR:257 WP:1847 W1:1052 W2:-795 G:-2 E1:157232 E2:0
00120 //B:4432 S:-4822 E:7986 WR:-198 WP:2902 W1:1352 W2:-1550 G:-2 E1:157236 E2:0
00121 //B:122 S:-4822 E:7986 WR:90 WP:2604 W1:1347 W2:-1257 G:-2 E1:78680 E2:0
00122 //B:-4002 S:-4822 E:7986 WR:185 WP:3049 W1:1617 W2:-1432 G:-2 E1:28 E2:0
00123 
00124 
00125 }
00126 
00127 
00128 int main(int argc, const char **argv)
00129 {
00130         // Instantiate a ModelManager:
00131         ModelManager manager("Test Model for Scorbot robot arm controller");
00132 
00133         nub::ref<OutputFrameSeries> ofs(new OutputFrameSeries(manager));
00134         manager.addSubComponent(ofs);
00135 
00136         scorbot = nub::soft_ref<Scorbot>(new Scorbot(manager));
00137         manager.addSubComponent(scorbot);
00138 
00139 
00140         // catch signals and redirect them to terminate for clean exit:
00141         signal(SIGHUP, terminate); signal(SIGINT, terminate);
00142         signal(SIGQUIT, terminate); signal(SIGTERM, terminate);
00143         signal(SIGALRM, terminate);
00144 
00145         // Parse command-line:
00146         if (manager.parseCommandLine(argc, argv, "<Serial Data>", 0, 80) == false) return(1);
00147 
00148 
00149         // let's get all our ModelComponent instances started:
00150         manager.start();
00151 
00152   //Just send raw serial cmd to controller
00153         if (manager.numExtraArgs() > 0)
00154         {
00155     scorbot->shutdownMotorsOff(false); //Dont turn the motors off on shutdown
00156 
00157     int numChar = manager.numExtraArgs();
00158           unsigned char buff[1024];
00159                 for(int i=0; i<numChar; i++)
00160                         buff[i] = manager.getExtraArgAs<int>(i);
00161 
00162                 printf("Sending %i bytes: ", numChar);
00163                 for(int i=0; i<numChar; i++)
00164                         printf("%i ", buff[i]);
00165                 printf("\n");
00166 
00167 
00168                 int ret = scorbot->write(buff, numChar);
00169                 usleep(50000);
00170 
00171                 ret = scorbot->read(buff, 1024);
00172                 printf("Got %i: ", ret);
00173                 for(int i=0; i<ret; i++)
00174                         printf("%3d(%3x) ", buff[i], buff[i]);
00175                 printf("\n");
00176 
00177                 manager.stop(); //Dont turn the motors off
00178                 return 0;
00179         }
00180 
00181                 
00182                 
00183 
00184         //  //Example FK and IK
00185         //  Point3D<float> efPos = scorbot->getEFPos(-664, -12765);
00186         //  LINFO("EF Pos(2405, -3115) %f %f %f", efPos.x, efPos.y, efPos.z);
00187         //  Scorbot::ArmPos armPos = scorbot->getArmPos(efPos);
00188         //  LINFO("ArmPos: sholder %i elbow %i", armPos.sholder, armPos.elbow);
00189         //  return 0;
00190 
00191 
00192         //Pref win
00193         PrefsWindow pWin("Scorbot Control", SimpleFont::FIXED(8));
00194         pWin.setValueNumChars(16);
00195         pWin.addPrefsForComponent(scorbot.get(), false);
00196 
00197 
00198 
00199         int PGain = 0;
00200         int IGain = 0;
00201         int DGain = 0;
00202 
00203         int speed = 50;
00204 
00205         bool showMS = false;
00206         bool rawWrist = false;
00207 
00208         Scorbot::MOTOR pidMot = Scorbot::EX1;
00209 
00210         Scorbot::ArmPos armPos;
00211         bool motorsOn = false;
00212 
00213   //Initial arm position will be 0-0
00214         armPos.base = 0;
00215         armPos.sholder = 0;
00216         armPos.elbow = 0;
00217         armPos.wristRoll = 0;
00218         armPos.wristPitch = 0;
00219         armPos.ex1 =0;
00220         armPos.ex2 =0;
00221         scorbot->resetEncoders();
00222         scorbot->setArmPos(armPos);
00223 
00224         int currentPos = 0;
00225         while(1)
00226         {
00227                 //sprintf(msg, "Speed = %i", speed);
00228                 //writeText(disp, Point2D<int>(0,80), msg);
00229 
00230                 pWin.update();
00231 
00232                 //float gc = scorbot->gravityComp(0,0);
00233 
00234                 // armPos = scorbot->getIKArmPos(efPos);
00235 
00236 
00237     
00238 
00239 
00240 
00241 
00242                 if (showMS)
00243                 {
00244                         //      char b[9];
00245                         //      scorbot->getMicroSwitchByte();
00246                         //LINFO("MS: %s", scorbot->getMicroSwitchByte());
00247                         //LINFO("MS: %d", scorbot->getMicroSwitch());
00248                         printf("MS: %d | %d%d%d%d%d\n", scorbot->getMicroSwitch(), scorbot->getMicroSwitchMotor(Scorbot::BASE),
00249                                         scorbot->getMicroSwitchMotor(Scorbot::SHOLDER),
00250                                         scorbot->getMicroSwitchMotor(Scorbot::ELBOW),
00251                                         scorbot->getMicroSwitchMotor(Scorbot::WRIST_ROLL),
00252                                         scorbot->getMicroSwitchMotor(Scorbot::WRIST_PITCH));
00253 
00254                 }
00255 
00256 
00257     Image<PixRGB<byte> > disp = getDisplay(motorsOn);
00258                 ofs->writeRGB(disp, "Output", FrameInfo("output", SRC_POS));
00259 
00260 
00261                 int key = getKey(ofs);
00262 
00263                 Point3D<float> efPos;
00264                 if (key != -1)
00265                 {
00266                         switch(key)
00267                         {
00268                                 case 10: //1
00269                                         if (!scorbot->internalPidEn())
00270                                                 scorbot->setMotor(Scorbot::BASE, speed);
00271                                         else
00272                                                 scorbot->setJointPos(Scorbot::BASE, 100, true);
00273                                         break;
00274                                 case 24: //q
00275                                         if (!scorbot->internalPidEn())
00276                                                 scorbot->setMotor(Scorbot::BASE, -1*speed);
00277                                         else
00278                                                 scorbot->setJointPos(Scorbot::BASE, -100, true);
00279                                         break;
00280                                 case 11: //2
00281                                         if (!scorbot->internalPidEn())
00282                                                 scorbot->setMotor(Scorbot::SHOLDER, speed);
00283                                         else
00284                                                 scorbot->setJointPos(Scorbot::SHOLDER, 100, true);
00285                                         break;
00286                                 case 25: //w
00287                                         if (!scorbot->internalPidEn())
00288                                                 scorbot->setMotor(Scorbot::SHOLDER, -1*speed);
00289                                         else
00290                                                 scorbot->setJointPos(Scorbot::SHOLDER, -100, true);
00291                                         break;
00292                                 case 12: //3
00293                                         if (!scorbot->internalPidEn())
00294                                                 scorbot->setMotor(Scorbot::ELBOW, -1*speed);
00295                                         else
00296                                                 scorbot->setJointPos(Scorbot::ELBOW, 100, true);
00297                                         break;
00298                                 case 26: //e
00299                                         if (!scorbot->internalPidEn())
00300                                                 scorbot->setMotor(Scorbot::ELBOW, speed);
00301                                         else
00302                                                 scorbot->setJointPos(Scorbot::ELBOW, -100, true);
00303                                         break;
00304                                 case 13: //4
00305                                         if (!scorbot->internalPidEn())
00306                                                 scorbot->setMotor(rawWrist ? Scorbot::WRIST1 : Scorbot::WRIST_PITCH, speed);
00307                                         else
00308                                                 scorbot->setJointPos(rawWrist ? Scorbot::WRIST1 : Scorbot::WRIST_PITCH, 100, true);
00309                                         break;
00310                                 case 27: //r
00311                                         if (!scorbot->internalPidEn())
00312                                                 scorbot->setMotor(rawWrist ? Scorbot::WRIST1 : Scorbot::WRIST_PITCH, -1*speed);
00313                                         else
00314                                                 scorbot->setJointPos(rawWrist ? Scorbot::WRIST1 : Scorbot::WRIST_PITCH, -100, true);
00315                                         break;
00316                                 case 14: //5
00317                                         if (!scorbot->internalPidEn())
00318                                                 scorbot->setMotor(rawWrist ? Scorbot::WRIST2 : Scorbot::WRIST_ROLL, speed);
00319                                         else
00320                                                 scorbot->setJointPos(rawWrist ? Scorbot::WRIST2 : Scorbot::WRIST_ROLL, 100, true);
00321                                         break;
00322                                 case 28: //t
00323                                         if (!scorbot->internalPidEn())
00324                                                 scorbot->setMotor(rawWrist ? Scorbot::WRIST2 : Scorbot::WRIST_ROLL, -1*speed);
00325                                         else
00326                                                 scorbot->setJointPos(rawWrist ? Scorbot::WRIST2 : Scorbot::WRIST_ROLL, -100, true);
00327                                         break;
00328                                 case 15: //6
00329                                         if (!scorbot->internalPidEn())
00330                                                 scorbot->setMotor(Scorbot::GRIPPER, speed);
00331                                         else
00332                                                 scorbot->setJointPos(Scorbot::GRIPPER, 100, true);
00333                                         break;
00334                                 case 29: //y
00335                                         if (!scorbot->internalPidEn())
00336                                                 scorbot->setMotor(Scorbot::GRIPPER, -1*speed);
00337                                         else
00338                                                 scorbot->setJointPos(Scorbot::GRIPPER, -100, true);
00339                                         break;
00340                                 case 16: //7
00341                                         if (!scorbot->internalPidEn())
00342                                                 scorbot->setMotor(Scorbot::EX1, speed);
00343                                         else
00344                                                 scorbot->setJointPos(Scorbot::EX1, 300, true);
00345                                         break;
00346                                 case 30: //u
00347                                         if (!scorbot->internalPidEn())
00348                                                 scorbot->setMotor(Scorbot::EX1, -1*speed);
00349                                         else
00350                                                 scorbot->setJointPos(Scorbot::EX1, -300, true);
00351                                         break;
00352                                 case 17: //8
00353                                         if (!scorbot->internalPidEn())
00354                                                 scorbot->setMotor(Scorbot::EX2, speed);
00355                                         else
00356                                                 scorbot->setJointPos(Scorbot::EX2, 100, true);
00357                                         break;
00358                                 case 31: //i
00359                                         if (!scorbot->internalPidEn())
00360                                                 scorbot->setMotor(Scorbot::EX2, -1*speed);
00361                                         else
00362                                                 scorbot->setJointPos(Scorbot::EX2, -100, true);
00363                                         break;
00364                                 case 34:
00365                                         LINFO("Resetting Encoders");
00366                                         scorbot->resetEncoders();
00367                                         break;
00368                                 case 65: //space
00369                                         scorbot->stopControl();
00370                                         usleep(10000);
00371                                         scorbot->stopAllMotors();
00372                                         break;
00373                                 case 41:
00374                                 case 33: //p
00375                                         {
00376                                                 Scorbot::ArmPos armPos = scorbot->readArmState();
00377                                                 LINFO("Encoders: B:%d S:%d E:%d WR:%d WP:%d W1:%i W2:%i G:%d E1:%d E2:%d",
00378                                                                 armPos.base,
00379                                                                 armPos.sholder,
00380                                                                 armPos.elbow, 
00381                                                                 armPos.wristRoll,
00382                                                                 armPos.wristPitch,
00383                                                                 armPos.wrist1,
00384                                                                 armPos.wrist2,
00385                                                                 armPos.gripper,
00386                                                                 armPos.ex1,
00387                                                                 armPos.ex2);
00388 
00389                                                 Point3D<float> efPos = scorbot->getEFPos(armPos);
00390                                                 LINFO("EFPos(mm): %0.2f %0.2f %0.2f",efPos.x,efPos.y,efPos.z);
00391                                         }
00392                                         break;
00393                                 case 57:
00394                                         showMS = !showMS;
00395                                         break;
00396                                 case 51:
00397                                 case 58: //m
00398                                         //showMS = !showMS;
00399                                         motorsOn = !motorsOn;
00400                                         if (motorsOn)
00401                                                 scorbot->motorsOn();
00402                                         else
00403                                                 scorbot->motorsOff();
00404                                         break;
00405                                 case 43: //h
00406                                         scorbot->homeMotors();
00407                                         break;
00408                                 case 32://o move motor pos to 0
00409                                         //  scorbot->resetMotorPos();
00410                                         break;
00411                                 case 54://c record current motor pos
00412                                         LINFO("Saved position");
00413                                         {
00414                                                 Scorbot::ArmPos armPos = scorbot->getArmPos();
00415                                                 armPositions.push_back(armPos);
00416                                                 //scorbot->setArmPos(armPos);
00417                                         }
00418                                         break;
00419                                 case 46://l goto the motor pos
00420                                         LINFO("Move to positions");
00421                                         for(uint i=0; i<armPositions.size(); i++)
00422                                         {
00423                                                 LINFO("Going to Pos %i...", currentPos);
00424                                                 scorbot->setInternalPos(armPositions[currentPos]);
00425                                         }
00426 
00427 
00428                                         break;
00429                                 case 56://b clear the record pos
00430                                         LINFO("Clear positions");
00431                                         armPositions.clear();
00432                                         break;
00433                                 case 39://s start control
00434                                         LINFO("Starting controller");
00435                                         scorbot->startControl();
00436                                         break;
00437 
00438                                 case 21: speed += 1; break; //-
00439                                 case 20: speed -= 1; break; //=
00440 
00441                                 case 79:    
00442                                                                  PGain += 1000;
00443                                                                  scorbot->setInternalPIDGain(pidMot, PGain, IGain, DGain);
00444                                                                  LINFO("P: %d I: %d D: %d", PGain, IGain, DGain);
00445                                                                  //scorbot->tunePID(pidMot, 0.001,  0, 0.0, true);
00446                                                                  break;
00447                                 case 87:  
00448                                                                  PGain -= 1000;
00449                                                                  scorbot->setInternalPIDGain(pidMot, PGain, IGain, DGain);
00450                                                                  LINFO("P: %d I: %d D: %d", PGain, IGain, DGain);
00451                                                                  //scorbot->tunePID(pidMot, -0.001, 0, 0.0, true);
00452                                                                  break;
00453                                 case 80: //page up
00454                                                                  IGain += 1000;
00455                                                                  scorbot->setInternalPIDGain(pidMot, PGain, IGain, DGain);
00456                                                                  LINFO("P: %d I: %d D: %d", PGain, IGain, DGain);
00457                                                                  //scorbot->tunePID(pidMot, 0, 0.001, 0, true);
00458                                                                  break; 
00459                                 case 88://page down
00460                                                                  IGain -= 1000;
00461                                                                  scorbot->setInternalPIDGain(pidMot, PGain, IGain, DGain);
00462                                                                  LINFO("P: %d I: %d D: %d", PGain, IGain, DGain);
00463                                                                  //scorbot->tunePID(pidMot, 0, -0.001, 0, true);
00464                                                                  break; 
00465                                 case 81:
00466                                                                  DGain += 1000;
00467                                                                  scorbot->setInternalPIDGain(pidMot, PGain, IGain, DGain);
00468                                                                  LINFO("P: %d I: %d D: %d", PGain, IGain, DGain);
00469                                                                  //scorbot->tunePID(pidMot, 0, 0.0, 0.001, true);
00470                                                                  break;
00471                                 case 89:
00472                                                                  DGain -= 1000;
00473                                                                  scorbot->setInternalPIDGain(pidMot, PGain, IGain, DGain);
00474                                                                  LINFO("P: %d I: %d D: %d", PGain, IGain, DGain);
00475                                                                  //scorbot->tunePID(pidMot, 0, 0.0, -0.001, true);
00476                                                                  break;
00477 
00478                                 case KEY_UP:
00479                                                                  armPos = scorbot->getDesiredArmPos();
00480                                                                  efPos = scorbot->getEFPos(armPos);
00481                                                                  efPos.y += 5;
00482                                                                  armPos = scorbot->getIKArmPos(efPos);
00483                                                                  scorbot->setJointPos(Scorbot::EX1, armPos.ex1);
00484                                                                  break;
00485                                 case KEY_DOWN:
00486                                                                  armPos = scorbot->getDesiredArmPos();
00487                                                                  efPos = scorbot->getEFPos(armPos);
00488                                                                  efPos.y -= 5;
00489                                                                  armPos = scorbot->getIKArmPos(efPos);
00490                                                                  scorbot->setJointPos(Scorbot::EX1, armPos.ex1);
00491                                                                  break;
00492                                 case KEY_LEFT:
00493                                                                  armPos = scorbot->getDesiredArmPos();
00494                                                                  efPos = scorbot->getEFPos(armPos);
00495                                                                  efPos.x -= 5;
00496                                                                  armPos = scorbot->getIKArmPos(efPos);
00497                                                                  scorbot->setJointPos(Scorbot::SHOLDER, armPos.sholder);
00498                                                                  scorbot->setJointPos(Scorbot::ELBOW, armPos.elbow);
00499                                                                  break;
00500                                 case KEY_RIGHT:
00501                                                                  armPos = scorbot->getDesiredArmPos();
00502                                                                  efPos = scorbot->getEFPos(armPos);
00503                                                                  efPos.x += 5;
00504                                                                  armPos = scorbot->getIKArmPos(efPos);
00505                                                                  scorbot->setJointPos(Scorbot::SHOLDER, armPos.sholder);
00506                                                                  scorbot->setJointPos(Scorbot::ELBOW, armPos.elbow);
00507                                                                  break;
00508                                 case 99: //page up
00509                                                                  armPos = scorbot->getDesiredArmPos();
00510                                                                  efPos = scorbot->getEFPos(armPos);
00511                                                                  efPos.z += 5;
00512                                                                  armPos = scorbot->getIKArmPos(efPos);
00513                                                                  scorbot->setJointPos(Scorbot::SHOLDER, armPos.sholder);
00514                                                                  scorbot->setJointPos(Scorbot::ELBOW, armPos.elbow);
00515                                                                  break;
00516                                 case 105: //page down
00517                                                                  armPos = scorbot->getDesiredArmPos();
00518                                                                  efPos = scorbot->getEFPos(armPos);
00519                                                                  efPos.z -= 5;
00520                                                                  armPos = scorbot->getIKArmPos(efPos);
00521                                                                  scorbot->setJointPos(Scorbot::SHOLDER, armPos.sholder);
00522                                                                  scorbot->setJointPos(Scorbot::ELBOW, armPos.elbow);
00523                                                                  break;
00524                                 case 67: //F1
00525                                                         {
00526                                                         }
00527                                                                  break;
00528                                 case 68: //F2
00529                                                                  int joint, position, duration;
00530                                                                  std::cout << "Joint, Position, Duration?";
00531                                                                  std::cin >> joint >> position >> duration;
00532                                                                  scorbot->setJointPos((Scorbot::MOTOR)joint, position, false, duration);
00533                                                                  break;
00534                         }
00535 
00536                         if (speed < 0) speed = 0;
00537                         if (speed > 100) speed = 100;
00538 
00539                         LINFO("Key = %i speed=%i", key, speed);
00540                 }
00541 
00542                 usleep(10000);
00543         }
00544 
00545         // stop all our ModelComponents
00546         manager.stop();
00547 
00548         // all done!
00549         return 0;
00550 }
00551 void home(nub::ref<OutputFrameSeries> &ofs)
00552 {
00553         //scorbot->stopAllMotors();
00554         //LINFO("Start doing homeing..");
00555         //LINFO("Use Left/Right Key to move,S to skip,Space to stop");
00556         //LINFO("Move Base..");
00557         //calMotor(Scorbot::BASE,ofs);
00558         //LINFO("Move Sholder..");
00559         //calMotor(Scorbot::SHOLDER,ofs);
00560         //LINFO("Move Elbow..");
00561         //calMotor(Scorbot::ELBOW,ofs);
00562         //LINFO("Move WRIST_ROLL..");
00563         //calMotor(Scorbot::WRIST_ROLL,ofs);
00564         //LINFO("Move WRIST_PITCH..");
00565         //calMotor(Scorbot::WRIST_PITCH,ofs);
00566         //LINFO("Done, all motor @ home now...");
00567         //scorbot->resetEncoders();
00568 
00569 }
00570 
00571 //void calMotor(Scorbot::MOTOR m,nub::ref<OutputFrameSeries> &ofs, int speed)
00572 //{
00573 //  bool doing = true;
00574 //  int ms = m;
00575 //  LINFO("Move motor %d..",m);
00576 //  while(doing)
00577 //  {
00578 //    int key = getKey(ofs);
00579 //    if (key != -1)
00580 //    {
00581 //      switch(key)
00582 //      {
00583 //        case KEY_LEFT:
00584 //          scorbot->setMotor(m, speed);
00585 //          break;
00586 //        case KEY_RIGHT:
00587 //          scorbot->setMotor(m, -1*speed);
00588 //          break;
00589 //        case 65: //space
00590 //          scorbot->stopAllMotors();
00591 //          break;
00592 //        case 39: //S,skip
00593 //          doing = false;
00594 //          break;
00595 //      }
00596 //      //LINFO("Key = %i", key);
00597 //    }
00598 //    if(ms==6)
00599 //      ms = 4;
00600 //    if(ms==7)
00601 //      ms = 3;
00602 //    //Wrist_ROLL = 7 which map to MS 3 & Wrist_PITCH =6 map to MS 4
00603 //
00604 //    //Once hit the switch,move to one side to release switch
00605 //    if( scorbot->getMicroSwitchMotor(ms) == 0){
00606 //      do
00607 //      {
00608 //        scorbot->setMotor(m, -speed);
00609 //      }while( scorbot->getMicroSwitchMotor(ms) == 0);
00610 //      if(ms == 4){
00611 //        scorbot->setMotor(m, speed);
00612 //        usleep(500000);// 1 sec
00613 //      }
00614 //      doing = false;
00615 //      scorbot->stopAllMotors();
00616 //      LINFO("MS %i is homed,plese press Space or UP/DWON",ms);
00617 //      //if it's last motor,also open the gripper
00618 //      if(ms==4)
00619 //        scorbot->setMotor(Scorbot::GRIPPER, speed);
00620 //    }
00621 //  }
00622 //  int key;
00623 //  do{
00624 //    key =getKey(ofs);
00625 //  }while(key != 65 && key!=KEY_DOWN && key!=KEY_UP);//space
00626 //  scorbot->stopAllMotors();
00627 //
00628 //
00629 //}
00630 // ######################################################################
00631 /* So things look consistent in everyone's emacs... */
00632 /* Local Variables: */
00633 /* indent-tabs-mode: nil */
00634 /* End: */
Generated on Sun May 8 08:40:07 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3