ArmController.C

Go to the documentation of this file.
00001 /*!@file ArmControl/ArmController.C  Control motors and pid */
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/ArmControl/ArmController.C $
00035 // $Id: ArmController.C 10794 2009-02-08 06:21:09Z itti $
00036 //
00037 
00038 #include "ArmControl/ArmController.H"
00039 #include "ArmControl/RobotArm.H"
00040 #include "Component/ModelOptionDef.H"
00041 #include "Devices/DeviceOpts.H"
00042 #include "Util/Assert.H"
00043 #include "Image/DrawOps.H"
00044 
00045 namespace
00046 {
00047   class ArmControllerPIDLoop : public JobWithSemaphore
00048   {
00049     public:
00050       ArmControllerPIDLoop(ArmController* armCtrl)
00051         :
00052           itsArmController(armCtrl),
00053           itsPriority(1),
00054           itsJobType("controllerLoop")
00055     {}
00056 
00057       virtual ~ArmControllerPIDLoop() {}
00058 
00059       virtual void run()
00060       {
00061         ASSERT(itsArmController);
00062         while(1)
00063         {
00064           if (itsArmController->isControllerOn())
00065             itsArmController->updatePID();
00066           else
00067             sleep(1);
00068         }
00069       }
00070 
00071       virtual const char* jobType() const
00072       { return itsJobType.c_str(); }
00073 
00074       virtual int priority() const
00075       { return itsPriority; }
00076 
00077     private:
00078       ArmController* itsArmController;
00079       const int itsPriority;
00080       const std::string itsJobType;
00081   };
00082 }
00083 
00084 // ######################################################################
00085 ArmController::ArmController(OptionManager& mgr,
00086     const std::string& descrName,
00087     const std::string& tagName,
00088     nub::soft_ref<RobotArm> robotArm ):
00089 
00090   ModelComponent(mgr, descrName, tagName),
00091   itsDesiredBase(0),
00092   itsDesiredSholder(0),
00093   itsDesiredElbow(0),
00094   itsDesiredWrist1(0),
00095   itsDesiredWrist2(0),
00096   itsDesiredGripper(0),
00097 
00098   itsDesiredSpeed(0),
00099 
00100   itsCurrentBase(0),
00101   itsCurrentSholder(0),
00102   itsCurrentElbow(0),
00103   itsCurrentWrist1(0),
00104   itsCurrentWrist2(0),
00105   itsCurrentGripper(0),
00106 
00107   itsCurrentSpeed(0),
00108 
00109   itsBasePID(0.8f, 0.0, 0.2, -20, 20,
00110       ERR_THRESH,30,-30),
00111   itsSholderPID(1.0f, 0, 0.65, -20, 20,
00112       ERR_THRESH,20,-60),
00113   itsElbowPID(0.7f, 0, 0.6, -20, 20,
00114       ERR_THRESH,44,-20),
00115   itsWrist1PID(0.7f, 0, 0.2, -20, 20,
00116       ERR_THRESH,30,-30),
00117   itsWrist2PID(0.7f, 0, 0.2, -20, 20,
00118       ERR_THRESH,30,-30),
00119 
00120   itsCurrentMotor_Base(0),
00121   itsCurrentMotor_Sholder(0),
00122   itsCurrentMotor_Elbow(0),
00123   itsCurrentMotor_Wrist1(0),
00124   itsCurrentMotor_Wrist2(0),
00125 
00126   setCurrentMotor_Base("MotorBase", this, 0, ALLOW_ONLINE_CHANGES),
00127   setCurrentMotor_Sholder("MotorSholder", this, 0, ALLOW_ONLINE_CHANGES),
00128   setCurrentMotor_Elbow("MotorElbow", this, 0, ALLOW_ONLINE_CHANGES),
00129   setCurrentMotor_Wrist1("MotorWrist1", this, 0, ALLOW_ONLINE_CHANGES),
00130   setCurrentMotor_Wrist2("MotorWrist2", this, 0, ALLOW_ONLINE_CHANGES),
00131 
00132 
00133   baseP("baseP", this, 0.8, ALLOW_ONLINE_CHANGES),
00134   baseI("baseI", this, 0, ALLOW_ONLINE_CHANGES),
00135   baseD("baseD", this, 0.2, ALLOW_ONLINE_CHANGES),
00136 
00137   sholderP("sholderP", this, 1.0, ALLOW_ONLINE_CHANGES),
00138   sholderI("sholderI", this, 0, ALLOW_ONLINE_CHANGES),
00139   sholderD("sholderD", this, 0.65, ALLOW_ONLINE_CHANGES),
00140 
00141   elbowP("elbowP", this, 0.7, ALLOW_ONLINE_CHANGES),
00142   elbowI("elbowI", this, 0, ALLOW_ONLINE_CHANGES),
00143   elbowD("elbowD", this, 0.6, ALLOW_ONLINE_CHANGES),
00144 
00145   wrist1P("wrist1P", this, 0.7, ALLOW_ONLINE_CHANGES),
00146   wrist1I("wrist1I", this, 0, ALLOW_ONLINE_CHANGES),
00147   wrist1D("wrist1D", this, 0.2, ALLOW_ONLINE_CHANGES),
00148 
00149   wrist2P("wrist2P", this, 0.7, ALLOW_ONLINE_CHANGES),
00150   wrist2I("wrist2I", this, 0, ALLOW_ONLINE_CHANGES),
00151   wrist2D("wrist2D", this, 0.2, ALLOW_ONLINE_CHANGES),
00152 
00153   motorsOn("motorsOn", this, false, ALLOW_ONLINE_CHANGES),
00154   pidOn("pidOn", this, false, ALLOW_ONLINE_CHANGES),
00155   guiOn("guiOn", this, true, ALLOW_ONLINE_CHANGES),
00156   controllerOn("controllerOn", this, true, ALLOW_ONLINE_CHANGES),
00157   motorsSpeed("motorsSpeed", this, 100, ALLOW_ONLINE_CHANGES),
00158 
00159   basePIDDisplay("Base PID Disp", this, true, ALLOW_ONLINE_CHANGES),
00160   sholderPIDDisplay("Sholder PID Disp", this, false, ALLOW_ONLINE_CHANGES),
00161   elbowPIDDisplay("Elbow PID Disp", this, false, ALLOW_ONLINE_CHANGES),
00162   wrist1PIDDisplay("Wrist1 PID Disp", this, false, ALLOW_ONLINE_CHANGES),
00163   wrist2PIDDisplay("Wrist2 PID Disp", this, false, ALLOW_ONLINE_CHANGES),
00164 
00165   basePos("BasePos", this, itsDesiredBase, ALLOW_ONLINE_CHANGES),
00166   sholderPos("SholderPos", this, itsDesiredSholder, ALLOW_ONLINE_CHANGES),
00167   elbowPos("ElbowPos", this, itsDesiredElbow, ALLOW_ONLINE_CHANGES),
00168   wrist1Pos("Wrist1Pos", this, itsDesiredWrist1, ALLOW_ONLINE_CHANGES),
00169   wrist2Pos("Wrist2Pos", this, itsDesiredWrist2, ALLOW_ONLINE_CHANGES),
00170 
00171   itsPIDImage(256, 256, ZEROS),
00172   itsRobotArm(robotArm),
00173 
00174   itsAvgn(0),
00175   itsAvgtime(0),
00176   itsLps(0)
00177 
00178 {
00179   //itsRobotArm = nub::soft_ref<Scorbot>(new Scorbot(mgr,"Scorbot", "Scorbot", "/dev/ttyUSB0"));
00180   //itsRobotArm = nub::soft_ref<ArmSim>(new ArmSim(mgr));
00181   addSubComponent(itsRobotArm);
00182 
00183   itsMaxJointPos.base = 0;          itsMinJointPos.base = 0;
00184   itsMaxJointPos.sholder = 0;       itsMinJointPos.sholder = 0;
00185   itsMaxJointPos.elbow = 0;         itsMinJointPos.elbow = 0;
00186   itsMaxJointPos.wrist1 = 0;        itsMinJointPos.wrist1 = 0;
00187   itsMaxJointPos.wrist2 = 0;        itsMinJointPos.wrist2 = 0;
00188   itsMaxJointPos.gripper = 0;       itsMinJointPos.gripper = 0;
00189 
00190   //For scrobt
00191   // Base -5647~7431
00192   // Sholder -1296~3264
00193   // Elbow -1500~4630
00194 }
00195 
00196 void ArmController::start2()
00197 {
00198 
00199   killMotors();
00200   sleep(1);
00201 
00202   itsCurrentBase    = itsRobotArm->getEncoder(RobotArm::BASE);
00203   itsCurrentElbow   = itsRobotArm->getEncoder(RobotArm::ELBOW);
00204   itsCurrentSholder = itsRobotArm->getEncoder(RobotArm::SHOLDER);
00205   itsCurrentWrist1  = itsRobotArm->getEncoder(RobotArm::WRIST1);
00206   itsCurrentWrist2  = itsRobotArm->getEncoder(RobotArm::WRIST2);
00207 
00208   //setup pid loop thread
00209   itsThreadServer.reset(new WorkThreadServer("ArmController",1)); //start a single worker thread
00210   itsThreadServer->setFlushBeforeStopping(false);
00211   rutz::shared_ptr<ArmControllerPIDLoop> j(new ArmControllerPIDLoop(this));
00212   itsThreadServer->enqueueJob(j);
00213 
00214 
00215 }
00216 
00217 // ######################################################################
00218 ArmController::~ArmController()
00219 {
00220   killMotors();
00221 }
00222 
00223 // ######################################################################
00224 bool ArmController::setBasePos(int base, bool rel)
00225 {
00226   if (rel)
00227     itsDesiredBase += base;
00228   else
00229     itsDesiredBase = base;
00230   return true;
00231 }
00232 
00233 // ######################################################################
00234 bool ArmController::setElbowPos(int elbow, bool rel)
00235 {
00236   if (rel)
00237     itsDesiredElbow += elbow;
00238   else
00239     itsDesiredElbow = elbow;
00240 
00241   return true;
00242 }
00243 
00244 // ######################################################################
00245 bool ArmController::setSholderPos(int sholder, bool rel)
00246 {
00247   if (rel)
00248     itsDesiredSholder += sholder;
00249   else
00250     itsDesiredSholder = sholder;
00251 
00252   return true;
00253 }
00254 
00255 // ######################################################################
00256 bool ArmController::setWrist1Pos(int wrist1, bool rel)
00257 {
00258   if (rel)
00259   {
00260     itsDesiredWrist1 += wrist1;
00261   //  itsDesiredWrist2 -= wrist1;
00262   }
00263   else
00264   {
00265     itsDesiredWrist1 = wrist1;
00266     //itsDesiredWrist2 = -1*wrist1;
00267   }
00268   return true;
00269 }
00270 
00271 bool ArmController::setWrist2Pos(int wrist2, bool rel)
00272 {
00273   if (rel)
00274   {
00275     //itsDesiredWrist1 += wrist2;
00276     itsDesiredWrist2 += wrist2;
00277   }
00278   else
00279   {
00280     //itsDesiredWrist1 = wrist2;
00281     itsDesiredWrist2 = wrist2;
00282   }
00283   return true;
00284 }
00285 
00286 // ######################################################################
00287 bool ArmController::setSpeed(int speed)
00288 {
00289   itsBasePID.setSpeed((float)speed/100.0);
00290   itsSholderPID.setSpeed((float)speed/100.0);
00291   itsElbowPID.setSpeed((float)speed/100.0);
00292   itsWrist1PID.setSpeed((float)speed/100.0);
00293   itsWrist2PID.setSpeed((float)speed/100.0);
00294   return true;
00295 }
00296 
00297 
00298 // ######################################################################
00299 
00300 void ArmController::updateBase(int base)
00301 {
00302   itsCurrentBase = base;
00303 }
00304 
00305 void ArmController::updateElbow(unsigned int elbow)
00306 {
00307   itsCurrentElbow = elbow;
00308 }
00309 
00310 void ArmController::updateSholder(int sholder)
00311 {
00312   itsCurrentSholder = sholder;
00313 }
00314 
00315 void ArmController::updateWrist1(unsigned int wrist1)
00316 {
00317   itsCurrentWrist1 = wrist1 ;
00318 }
00319 
00320 void ArmController::updateWrist2(unsigned int wrist2)
00321 {
00322   itsCurrentWrist1 = wrist2 ;
00323 }
00324 
00325 ArmController::JointPos ArmController::getJointPos()
00326 {
00327   JointPos jointPos;
00328   jointPos.base = itsRobotArm->getEncoder(RobotArm::BASE);
00329   jointPos.sholder = itsRobotArm->getEncoder(RobotArm::SHOLDER);
00330   jointPos.elbow = itsRobotArm->getEncoder(RobotArm::ELBOW);
00331   jointPos.wrist1 = itsRobotArm->getEncoder(RobotArm::WRIST1);
00332   jointPos.wrist2 = itsRobotArm->getEncoder(RobotArm::WRIST2);
00333   jointPos.gripper = itsCurrentGripper;
00334 
00335   return jointPos;
00336 }
00337 
00338 bool ArmController::isFinishMove()
00339 {
00340     if(abs(itsDesiredBase - itsCurrentBase) > 40 ||
00341       abs(itsDesiredSholder - itsCurrentSholder) > 40 ||
00342       abs(itsDesiredElbow - itsCurrentElbow) > 40 ||
00343       abs(itsDesiredWrist1 - itsCurrentWrist1) > 40 ||
00344       abs(itsDesiredWrist2 - itsCurrentWrist2) > 40 )
00345       return false;
00346 
00347     return true;
00348 }
00349 
00350 void ArmController::setMinJointPos(const ArmController::JointPos &jointPos)
00351 {
00352 
00353   itsMinJointPos.base = jointPos.base;
00354   itsMinJointPos.sholder = jointPos.sholder;
00355   itsMinJointPos.elbow = jointPos.elbow;
00356   itsMinJointPos.wrist1 = jointPos.wrist1;
00357   itsMinJointPos.wrist2 = jointPos.wrist2;
00358   itsMinJointPos.gripper = jointPos.gripper;
00359 
00360 }
00361 
00362 void ArmController::setMaxJointPos(const ArmController::JointPos &jointPos)
00363 {
00364   itsMaxJointPos.base = jointPos.base;
00365   itsMaxJointPos.sholder = jointPos.sholder;
00366   itsMaxJointPos.elbow = jointPos.elbow;
00367   itsMaxJointPos.wrist1 = jointPos.wrist1;
00368   itsMaxJointPos.wrist2 = jointPos.wrist2;
00369   itsMaxJointPos.gripper = jointPos.gripper;
00370 
00371 
00372 }
00373 
00374 bool ArmController::setJointPos(const ArmController::JointPos &jointPos, bool block)
00375 {
00376   if (!jointPos.reachable)
00377     return true;
00378 
00379   itsDesiredBase = jointPos.base;
00380   itsDesiredSholder = jointPos.sholder;
00381   itsDesiredElbow = jointPos.elbow;
00382   itsDesiredWrist1 = jointPos.wrist2;
00383   itsDesiredWrist2 = jointPos.wrist1;
00384   //itsDesiredGripper = jointPos.gripper;
00385 
00386   if (block) //wait untill move is finished
00387   {
00388     while(!isFinishMove()){
00389       itsRobotArm->simLoop();
00390 //      itsRobotArm->getFrame(-1);
00391     }
00392   }
00393 
00394   //After the move, set the gripper
00395   //if (itsDesiredGripper != itsCurrentGripper)
00396   //setGripper(itsDesiredGripper);
00397 
00398   return isFinishMove();
00399 
00400 }
00401 
00402 void ArmController::resetJointPos(JointPos& jointPos, int val)
00403 {
00404   jointPos.base = val;
00405   jointPos.sholder = val;
00406   jointPos.elbow = val;
00407   jointPos.wrist1 = val;
00408   jointPos.wrist2 = val;
00409   jointPos.gripper = val;
00410 
00411 }
00412 
00413 void ArmController::setGripper(int pos)
00414 {
00415 
00416   switch(pos)
00417   {
00418     case 0:
00419       itsRobotArm->setMotor(RobotArm::GRIPPER, -75);
00420       break;
00421     case 1:
00422       itsRobotArm->setMotor(RobotArm::GRIPPER, 75);
00423       break;
00424   }
00425   itsCurrentGripper = pos;
00426   sleep(1);
00427 }
00428 
00429 // ######################################################################
00430 void ArmController::updatePID()
00431 {
00432         //Showing the running speed
00433         //getLps();
00434 
00435   itsCurrentBase    = itsRobotArm->getEncoder(RobotArm::BASE);
00436   itsCurrentElbow   = itsRobotArm->getEncoder(RobotArm::ELBOW);
00437   itsCurrentSholder = itsRobotArm->getEncoder(RobotArm::SHOLDER);
00438   itsCurrentWrist1  = itsRobotArm->getEncoder(RobotArm::WRIST1);
00439   itsCurrentWrist2  = itsRobotArm->getEncoder(RobotArm::WRIST2);
00440 
00441   //limit the joint positions
00442   if (itsDesiredBase    >= itsMaxJointPos.base)
00443     itsDesiredBase   = itsMaxJointPos.base;
00444   if (itsDesiredSholder >= itsMaxJointPos.sholder)
00445     itsDesiredSholder= itsMaxJointPos.sholder;
00446   if (itsDesiredElbow   >= itsMaxJointPos.elbow)
00447     itsDesiredElbow  = itsMaxJointPos.elbow;
00448   if (itsDesiredWrist1  >= itsMaxJointPos.wrist1)
00449     itsDesiredWrist1 = itsMaxJointPos.wrist1;
00450   if (itsDesiredWrist2  >= itsMaxJointPos.wrist2)
00451     itsDesiredWrist2 = itsMaxJointPos.wrist2;
00452   if (itsDesiredGripper >= itsMaxJointPos.gripper)
00453     itsDesiredGripper= itsMaxJointPos.gripper;
00454 
00455   if (itsDesiredBase    < itsMinJointPos.base)
00456     itsDesiredBase   = itsMinJointPos.base;
00457   if (itsDesiredSholder < itsMinJointPos.sholder)
00458     itsDesiredSholder= itsMinJointPos.sholder;
00459   if (itsDesiredElbow   < itsMinJointPos.elbow)
00460     itsDesiredElbow  = itsMinJointPos.elbow;
00461   if (itsDesiredWrist1  < itsMinJointPos.wrist1)
00462     itsDesiredWrist1 = itsMinJointPos.wrist1;
00463   if (itsDesiredWrist2  < itsMinJointPos.wrist2)
00464     itsDesiredWrist2 = itsMinJointPos.wrist2;
00465   if (itsDesiredGripper < itsMinJointPos.base)
00466     itsDesiredGripper= itsMinJointPos.gripper;
00467 
00468   if(guiOn.getVal())
00469   {
00470     genPIDImage();
00471   }
00472 
00473   if (pidOn.getVal())
00474   {
00475     itsCurrentMotor_Base = (int)itsBasePID.update((float)itsDesiredBase, (float)itsCurrentBase);
00476     if (!itsBasePID.getRunPID())
00477     {
00478       LINFO("Base off");
00479       itsCurrentMotor_Base = 0;
00480       itsDesiredBase = itsCurrentBase;
00481       itsBasePID.setPIDOn(true);
00482     }
00483     itsCurrentMotor_Sholder = (int)itsSholderPID.update((float)itsDesiredSholder, (float)itsCurrentSholder);
00484     if (!itsSholderPID.getRunPID())
00485     {
00486       LINFO("Sholder off");
00487       itsDesiredSholder = itsCurrentSholder;
00488       itsCurrentMotor_Sholder = 0;
00489       itsSholderPID.setPIDOn(true);
00490     }
00491     itsCurrentMotor_Elbow = (int)itsElbowPID.update((float)itsDesiredElbow, (float)itsCurrentElbow);
00492     if (!itsElbowPID.getRunPID())
00493     {
00494       LINFO("Elbow off");
00495       itsDesiredElbow = itsCurrentElbow;
00496       itsCurrentMotor_Elbow = 0;
00497       itsElbowPID.setPIDOn(true);
00498     }
00499     itsCurrentMotor_Wrist1 = (int)itsWrist1PID.update((float)itsDesiredWrist1, (float)itsCurrentWrist1);
00500     if (!itsWrist1PID.getRunPID())
00501     {
00502       itsDesiredWrist1 = itsCurrentWrist1;
00503       itsCurrentMotor_Wrist1 = 0;
00504       itsWrist1PID.setPIDOn(true);
00505     }
00506     itsCurrentMotor_Wrist2 = (int)itsWrist2PID.update((float)itsDesiredWrist2, (float)itsCurrentWrist2);
00507     if (!itsWrist2PID.getRunPID())
00508     {
00509       itsDesiredWrist2 = itsCurrentWrist2;
00510       itsCurrentMotor_Wrist2 = 0;
00511       itsWrist2PID.setPIDOn(true);
00512     }
00513   }
00514 
00515   if (!motorsOn.getVal())
00516   {
00517     itsCurrentMotor_Base =0;
00518     itsCurrentMotor_Sholder =0;
00519     itsCurrentMotor_Elbow =0;
00520     itsCurrentMotor_Wrist1 =0;
00521     itsCurrentMotor_Wrist2 =0;
00522 
00523   }
00524 
00525 
00526 
00527 //  LINFO("Set motor %i", itsCurrentMotor_Base);
00528   itsRobotArm->setMotor(RobotArm::BASE, itsCurrentMotor_Base);
00529   itsRobotArm->setMotor(RobotArm::SHOLDER, itsCurrentMotor_Sholder);
00530   itsRobotArm->setMotor(RobotArm::ELBOW, itsCurrentMotor_Elbow);
00531   itsRobotArm->setMotor(RobotArm::WRIST1, itsCurrentMotor_Wrist1);
00532   itsRobotArm->setMotor(RobotArm::WRIST2, itsCurrentMotor_Wrist2);
00533 
00534 }
00535 
00536 void ArmController::setMotor(int motor, int val)
00537 {
00538   switch(motor)
00539   {
00540     case (RobotArm::BASE):
00541       itsCurrentMotor_Base = val;
00542       break;
00543     case (RobotArm::SHOLDER):
00544       itsCurrentMotor_Sholder = val;
00545       break;
00546     case (RobotArm::ELBOW):
00547       itsCurrentMotor_Elbow = val;
00548       break;
00549     case (RobotArm::WRIST1):
00550       itsCurrentMotor_Wrist1 = val;
00551       break;
00552     case (RobotArm::WRIST2):
00553       itsCurrentMotor_Wrist2 = val;
00554       break;
00555     default: LINFO("Invalid motor %i", motor); break;
00556   }
00557 
00558 }
00559 
00560 
00561 void ArmController::killMotors()
00562 {
00563   itsRobotArm->stopAllMotors();
00564   itsCurrentMotor_Base =0;
00565   itsCurrentMotor_Sholder =0;
00566   itsCurrentMotor_Elbow =0;
00567   itsCurrentMotor_Wrist1 =0;
00568   itsCurrentMotor_Wrist2 =0;
00569   pidOn.setVal(false);
00570 }
00571 
00572 
00573 void ArmController::paramChanged(ModelParamBase* const param, const bool valueChanged, ParamClient::ChangeStatus* status)
00574 {
00575 
00576   //////// Base PID constants/gain change ////////
00577   if (param == &baseP && valueChanged)
00578     itsBasePID.setPIDPgain(baseP.getVal());
00579   else if(param == &baseI && valueChanged)
00580     itsBasePID.setPIDIgain(baseI.getVal());
00581   else if(param == &baseD && valueChanged)
00582     itsBasePID.setPIDDgain(baseD.getVal());
00583 
00584   //////// Elbow PID constants/gain change ////
00585   else if(param == &elbowP && valueChanged)
00586     itsElbowPID.setPIDPgain(elbowP.getVal());
00587   else if(param == &elbowI && valueChanged)
00588     itsElbowPID.setPIDIgain(elbowI.getVal());
00589   else if(param == &elbowD && valueChanged)
00590     itsElbowPID.setPIDDgain(elbowD.getVal());
00591 
00592   //////// Sholder PID constants/gain change ///////
00593   else if(param == &sholderP && valueChanged)
00594     itsSholderPID.setPIDPgain(sholderP.getVal());
00595   else if(param == &sholderI && valueChanged)
00596     itsSholderPID.setPIDIgain(sholderI.getVal());
00597   else if(param == &sholderD && valueChanged)
00598     itsSholderPID.setPIDDgain(sholderD.getVal());
00599 
00600   /////// Wrist1 PID constants/gain change ////
00601   else if(param == &wrist1P && valueChanged)
00602     itsWrist1PID.setPIDPgain(wrist1P.getVal());
00603   else if(param == &wrist1I && valueChanged)
00604     itsWrist1PID.setPIDIgain(wrist1I.getVal());
00605   else if(param == &wrist1D && valueChanged)
00606     itsWrist1PID.setPIDDgain(wrist1D.getVal());
00607 
00608   /////// Wrist2 PID constants/gain change ////
00609   else if(param == &wrist2P && valueChanged)
00610     itsWrist2PID.setPIDPgain(wrist2P.getVal());
00611   else if(param == &wrist2I && valueChanged)
00612     itsWrist2PID.setPIDIgain(wrist2I.getVal());
00613   else if(param == &wrist2D && valueChanged)
00614     itsWrist2PID.setPIDDgain(wrist2D.getVal());
00615 
00616 
00617   //Motor_Settings
00618   else if(param == &basePos && valueChanged)
00619     setBasePos(basePos.getVal());
00620   else if(param == &sholderPos && valueChanged)
00621     setSholderPos(sholderPos.getVal());
00622   else if(param == &elbowPos && valueChanged)
00623     setElbowPos(elbowPos.getVal());
00624   else if(param == &wrist1Pos && valueChanged)
00625     setWrist1Pos(wrist1Pos.getVal());
00626   else if(param == &wrist2Pos && valueChanged)
00627     setWrist2Pos(wrist2Pos.getVal());
00628 
00629   //Position settings
00630   else if(param == &setCurrentMotor_Base && valueChanged)
00631     setMotor(RobotArm::BASE, setCurrentMotor_Base.getVal());
00632   else if(param == &setCurrentMotor_Sholder && valueChanged)
00633     setMotor(RobotArm::SHOLDER, setCurrentMotor_Sholder.getVal());
00634   else if(param == &setCurrentMotor_Elbow && valueChanged)
00635     setMotor(RobotArm::ELBOW, setCurrentMotor_Elbow.getVal());
00636   else if(param == &setCurrentMotor_Wrist2 && valueChanged)
00637     setMotor(RobotArm::WRIST2, setCurrentMotor_Wrist2.getVal());
00638   else if(param == &setCurrentMotor_Wrist1 && valueChanged)
00639     setMotor(RobotArm::WRIST1, setCurrentMotor_Wrist1.getVal());
00640 
00641   else if(param == &motorsSpeed && valueChanged)
00642     setSpeed(motorsSpeed.getVal());
00643 
00644 }
00645 
00646 ////////////////// GUI Related ////////////////////////////////////
00647 
00648 void ArmController::genPIDImage()
00649 {
00650   static int x = 0;
00651 
00652   int wrist1Err, wrist2Err, baseErr, elbowErr, sholderErr;
00653 
00654   baseErr = (itsDesiredBase - itsCurrentBase);
00655   elbowErr = (itsDesiredElbow - itsCurrentElbow);
00656   sholderErr = (itsDesiredSholder - itsCurrentSholder);
00657   wrist1Err = (itsDesiredWrist1 - itsCurrentWrist1);
00658   wrist2Err = (itsDesiredWrist2 - itsCurrentWrist2);
00659 
00660   while (elbowErr <= -180) elbowErr += 360;
00661   while (elbowErr > 180) elbowErr -= 360;
00662 
00663 
00664   int scale = 2;
00665 
00666   int wrist1_y = (256/2) + (wrist1Err/scale);
00667   if (wrist1_y > 255) wrist1_y = 255;
00668   if (wrist1_y < 0) wrist1_y = 0;
00669 
00670   int wrist2_y = (256/2) + (wrist2Err/scale);
00671   if (wrist2_y > 255) wrist2_y = 255;
00672   if (wrist2_y < 0) wrist2_y = 0;
00673 
00674   int base_y = (256/2) + (baseErr/scale);
00675   if (base_y > 255) base_y = 255;
00676   if (base_y < 0) base_y = 0;
00677 
00678   int elbow_y = (256/2) + (elbowErr/scale);
00679   if (elbow_y > 255) elbow_y = 255;
00680   if (elbow_y < 0) elbow_y = 0;
00681 
00682   int sholder_y = (256/2) + (sholderErr/scale);
00683   if (sholder_y > 255) sholder_y = 255;
00684   if (sholder_y < 0) sholder_y = 0;
00685 
00686 
00687   if (!x)
00688   {
00689     itsPIDImage.clear();
00690     drawLine(itsPIDImage, Point2D<int>(0, 256/2), Point2D<int>(256, 256/2), PixRGB<byte>(255,0,0));
00691   }
00692 
00693   if(basePIDDisplay.getVal())
00694     drawCircle(itsPIDImage, Point2D<int>(x,base_y), 2, PixRGB<byte>(255,0,0));
00695   if(sholderPIDDisplay.getVal())
00696     itsPIDImage.setVal(x,sholder_y,PixRGB<byte>(255,255,0));
00697   if(elbowPIDDisplay.getVal())
00698     itsPIDImage.setVal(x,elbow_y,PixRGB<byte>(0,0,255));
00699   if(wrist1PIDDisplay.getVal())
00700     itsPIDImage.setVal(x,wrist1_y,PixRGB<byte>(0,255,0));
00701 
00702   x = (x+1)%256;
00703 
00704 }
00705 
00706 int ArmController::getElbowErr()
00707 {
00708   return (itsDesiredElbow - itsCurrentElbow);
00709 }
00710 
00711 // ######################################################################
00712 /* So things look consistent in everyone's emacs... */
00713 /* Local Variables: */
00714 /* indent-tabs-mode: nil */
00715 /* End: */
Generated on Sun May 8 08:40:11 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3