test-ScorbotInterface.C

00001 #include "Robots/Scorbot/ScorbotInterface.H"
00002 #include "Component/ModelManager.H"
00003 #include <vector>
00004 
00005 int main(int argc, char* argv[])
00006 {
00007   ModelManager mgr("Test Scorbot Interface");
00008 
00009   nub::ref<ScorbotInterface> scorbot(new ScorbotInterface(mgr));
00010   mgr.addSubComponent(scorbot);
00011 
00012   if(mgr.parseCommandLine(argc, argv, "", 0, 0) == false) return -1;
00013 
00014   mgr.start();
00015 
00016   scorbot->setEnabled(true);
00017 
00018         std::vector<ScorbotInterface::encoderVals_t> positions;
00019         ScorbotInterface::encoderVals_t encoders;
00020 
00021 encoders[ScorbotInterface::Base] = -3180;
00022 encoders[ScorbotInterface::Shoulder] = 4886;
00023 encoders[ScorbotInterface::Elbow] = -1938;
00024 encoders[ScorbotInterface::Wrist1] = 2106;
00025 encoders[ScorbotInterface::Wrist2] = -2187;
00026 encoders[ScorbotInterface::Gripper] = 0;
00027 encoders[ScorbotInterface::Slider] = -42027;
00028         positions.push_back(encoders);
00029 
00030         
00031 encoders[ScorbotInterface::Base] = 1542;
00032 encoders[ScorbotInterface::Shoulder] = 4178;
00033 encoders[ScorbotInterface::Elbow] = -340;
00034 encoders[ScorbotInterface::Wrist1] = 2327;
00035 encoders[ScorbotInterface::Wrist2] = -2187;
00036 encoders[ScorbotInterface::Gripper] = 0;
00037 encoders[ScorbotInterface::Slider] = -42032;
00038         positions.push_back(encoders);
00039 
00040 encoders[ScorbotInterface::Base] = -5269;
00041 encoders[ScorbotInterface::Shoulder] = 3810;
00042 encoders[ScorbotInterface::Elbow] = 2886;
00043 encoders[ScorbotInterface::Wrist1] = 2187;
00044 encoders[ScorbotInterface::Wrist2] = -2179;
00045 encoders[ScorbotInterface::Gripper] = 0;
00046 encoders[ScorbotInterface::Slider] = -42031;
00047         positions.push_back(encoders);
00048 
00049 
00050 
00051         while(1)
00052         {
00053                 for(size_t posIdx=0; posIdx<positions.size(); posIdx++)
00054                 {
00055                         std::cout << "---------------------------------------" << std::endl;
00056                         std::cout << "Going To Position " << posIdx << std::endl;
00057                         scorbot->setJoints(positions[posIdx], 2000);
00058                         sleep(2);
00059                         std::cout << "Done..." << std::endl;
00060 
00061                         ScorbotInterface::encoderVals_t currPos = scorbot->getEncoders();
00062                         std::cout << "Base:     " << currPos[ScorbotInterface::Base]     << " / " << positions[posIdx][ScorbotInterface::Base] << std::endl;
00063                         std::cout << "Shoulder: " << currPos[ScorbotInterface::Shoulder] << " / " << positions[posIdx][ScorbotInterface::Shoulder] << std::endl;
00064                         std::cout << "Elbow:    " << currPos[ScorbotInterface::Elbow]    << " / " << positions[posIdx][ScorbotInterface::Elbow] << std::endl;
00065                         std::cout << "Wrist1:   " << currPos[ScorbotInterface::Wrist1]   << " / " << positions[posIdx][ScorbotInterface::Wrist1] << std::endl;
00066                         std::cout << "Wrist2:   " << currPos[ScorbotInterface::Wrist2]   << " / " << positions[posIdx][ScorbotInterface::Wrist2] << std::endl;
00067                         std::cout << "Gripper:  " << currPos[ScorbotInterface::Gripper]  << " / " << positions[posIdx][ScorbotInterface::Gripper] << std::endl;
00068                         std::cout << "Slider:   " << currPos[ScorbotInterface::Slider]   << " / " << positions[posIdx][ScorbotInterface::Slider] << std::endl;
00069                         sleep(2);
00070                 }
00071         }
00072 }