YouBotGripperTest.cpp

Go to the documentation of this file.
00001 #include "YouBotGripperTest.hpp"
00002 
00003 using namespace youbot;
00004 
00005 YouBotGripperTest::YouBotGripperTest() : dof(5) {
00006 
00007   EthercatMaster::getInstance("youbot-ethercat.cfg", "../config/", true);
00008 
00009 }
00010 
00011 YouBotGripperTest::~YouBotGripperTest() {
00012 
00013 }
00014 
00015 void YouBotGripperTest::setUp() {
00016   Logger::logginLevel = trace;
00017   updateCycle = 2000;
00018 
00019 }
00020 
00021 void YouBotGripperTest::tearDown() {
00022   //  EthercatMaster::destroy();
00023 }
00024 
00025 void YouBotGripperTest::youBotGripperTest() {
00026 
00027   LOG(info) << __func__ << "\n";
00028   YouBotManipulator myArm("youbot-manipulator");
00029 
00030   GripperDataTrace myTrace(myArm.getArmGripper().getGripperBar2(), __func__, true);
00031 
00032   TargetPositionReached bar1TargetReched;
00033   TargetPositionReached bar2TargetReched;
00034   bool targetReachedBar1 = false;
00035   bool targetReachedBar2 = false;
00036 
00037   myArm.calibrateGripper(true);
00038   myTrace.startTrace("Load", "");
00039   
00040   //open gripper
00041   myArm.getArmGripper().open();
00042 
00043   for (int i = 0; i < 40; i++) {
00044     myArm.getArmGripper().getGripperBar1().getConfigurationParameter(bar1TargetReched);
00045     bar1TargetReched.getParameter(targetReachedBar1);
00046     myArm.getArmGripper().getGripperBar2().getConfigurationParameter(bar2TargetReched);
00047     bar2TargetReched.getParameter(targetReachedBar2);
00048     myTrace.updateTrace((double)targetReachedBar1);
00049     if (targetReachedBar1 && targetReachedBar2) {
00050       break;
00051     }
00052   }
00053   targetReachedBar1 = false;
00054   targetReachedBar2 = false;
00055 
00056   //close gripper
00057   myArm.getArmGripper().close();
00058 
00059   for (int i = 0; i < 40; i++) {
00060     myArm.getArmGripper().getGripperBar1().getConfigurationParameter(bar1TargetReched);
00061     bar1TargetReched.getParameter(targetReachedBar1);
00062     myArm.getArmGripper().getGripperBar2().getConfigurationParameter(bar2TargetReched);
00063     bar2TargetReched.getParameter(targetReachedBar2);
00064     myTrace.updateTrace((double)targetReachedBar2);
00065     if (targetReachedBar1 && targetReachedBar2) {
00066       break;
00067     }
00068   }
00069 
00070   myTrace.stopTrace();
00071   myTrace.plotTrace();
00072 
00073 
00074 }