YouBotArmTestWithoutThread.cpp

Go to the documentation of this file.
00001 #include "YouBotArmTestWithoutThread.hpp"
00002 
00003 using namespace youbot;
00004 
00005 YouBotArmTestWithoutThread::YouBotArmTestWithoutThread():dof(5) {
00006 
00007 
00008 
00009 }
00010 
00011 YouBotArmTestWithoutThread::~YouBotArmTestWithoutThread() {
00012 
00013 }
00014 
00015 void YouBotArmTestWithoutThread::setUp() {
00016   Logger::logginLevel = trace;
00017   updateCycle = 2000;
00018   ethercatMaster = &EthercatMaster::getInstance("youbot-ethercat.cfg", "../config/", false);
00019   if(ethercatMaster->isThreadActive()){
00020     LOG(error) << "Thread Active";
00021     EthercatMaster::destroy();
00022     ethercatMaster = &EthercatMaster::getInstance("youbot-ethercat.cfg", "../config/", false);
00023   }
00024 
00025 }
00026 
00027 void YouBotArmTestWithoutThread::tearDown() {
00028     EthercatMaster::destroy();
00029 }
00030 
00031 void YouBotArmTestWithoutThread::youBotArmTest() {
00032 
00033   LOG(info) << __func__ << "\n";
00034   YouBotManipulator myArm("youbot-manipulator");
00035   myArm.doJointCommutation();
00036   myArm.calibrateManipulator();
00037 
00038   std::stringstream jointNameStream;
00039   boost::ptr_vector<DataTrace> myTrace;
00040   std::vector<JointAngleSetpoint> upstretchedpose;
00041   std::vector<JointAngleSetpoint> foldedpose;
00042   JointAngleSetpoint desiredJointAngle;
00043 
00044   desiredJointAngle.angle = 2.56244 * radian;
00045   upstretchedpose.push_back(desiredJointAngle);
00046   desiredJointAngle.angle = 1.04883 * radian;
00047   upstretchedpose.push_back(desiredJointAngle);
00048   desiredJointAngle.angle = -2.43523 * radian;
00049   upstretchedpose.push_back(desiredJointAngle);
00050   desiredJointAngle.angle = 1.73184 * radian;
00051   upstretchedpose.push_back(desiredJointAngle);
00052   desiredJointAngle.angle = 1.5 * radian;
00053   upstretchedpose.push_back(desiredJointAngle);
00054 
00055   desiredJointAngle.angle = 0.11 * radian;
00056   foldedpose.push_back(desiredJointAngle);
00057   desiredJointAngle.angle = 0.11 * radian;
00058   foldedpose.push_back(desiredJointAngle);
00059   desiredJointAngle.angle = -0.11 * radian;
00060   foldedpose.push_back(desiredJointAngle);
00061   desiredJointAngle.angle = 0.11 * radian;
00062   foldedpose.push_back(desiredJointAngle);
00063   desiredJointAngle.angle = 0.2 * radian;
00064   foldedpose.push_back(desiredJointAngle);
00065 
00066 
00067   for (int i = 1; i <= dof; i++) {
00068     jointNameStream << "Joint_" << i << "_" << __func__;
00069     myTrace.push_back(new DataTrace(myArm.getArmJoint(i), jointNameStream.str(), true));
00070     jointNameStream.str("");
00071   }
00072 
00073 
00074   for (int i = 0; i < dof; i++) {
00075     myTrace[i].startTrace();
00076   }
00077   
00078   ClearMotorControllerTimeoutFlag clearTimeoutFlag;
00079   myArm.getArmJoint(1).setConfigurationParameter(clearTimeoutFlag);
00080   myArm.getArmJoint(2).setConfigurationParameter(clearTimeoutFlag);
00081   myArm.getArmJoint(3).setConfigurationParameter(clearTimeoutFlag);
00082   myArm.getArmJoint(4).setConfigurationParameter(clearTimeoutFlag);
00083   myArm.getArmJoint(5).setConfigurationParameter(clearTimeoutFlag);
00084 
00085 
00086   if (!ethercatMaster->isThreadActive()) {
00087       ethercatMaster->sendProcessData();
00088       ethercatMaster->receiveProcessData();
00089    }
00090   
00091 
00092   // 1 sec no movement
00093   startTime = myTrace[0].getTimeDurationMilliSec();
00094   overallTime = startTime + 1000;  
00095   while (myTrace[0].getTimeDurationMilliSec() < overallTime) {
00096     if (!ethercatMaster->isThreadActive()) {
00097       ethercatMaster->sendProcessData();
00098       ethercatMaster->receiveProcessData();
00099     }
00100     for (int i = 0; i < dof; i++) {
00101       myTrace[i].updateTrace();
00102     }
00103     SLEEP_MICROSEC(updateCycle);
00104   }
00105 
00106   // move to upstretched position
00107   startTime = myTrace[0].getTimeDurationMilliSec();
00108   overallTime = startTime + 5000;
00109   myArm.setJointData(upstretchedpose);
00110   while (myTrace[0].getTimeDurationMilliSec() < overallTime) {
00111     if (!ethercatMaster->isThreadActive()) {
00112       ethercatMaster->sendProcessData();
00113       ethercatMaster->receiveProcessData();
00114     }
00115     for (int i = 0; i < dof; i++) {
00116       myTrace[i].updateTrace();
00117     }
00118     SLEEP_MICROSEC(updateCycle);
00119   }
00120   
00121   // move to folded position
00122   startTime = myTrace[0].getTimeDurationMilliSec();
00123   overallTime = startTime + 5000;
00124   myArm.setJointData(foldedpose);
00125   while (myTrace[0].getTimeDurationMilliSec() < overallTime) {
00126     if (!ethercatMaster->isThreadActive()) {
00127       ethercatMaster->sendProcessData();
00128       ethercatMaster->receiveProcessData();
00129     }
00130     for (int i = 0; i < dof; i++) {
00131       myTrace[i].updateTrace();
00132     }
00133     SLEEP_MICROSEC(updateCycle);
00134   }
00135   
00136   // 1 sec no movement
00137   startTime = myTrace[0].getTimeDurationMilliSec();
00138   overallTime = startTime + 1000;  
00139   while (myTrace[0].getTimeDurationMilliSec() < overallTime) {
00140     if (!ethercatMaster->isThreadActive()) {
00141       ethercatMaster->sendProcessData();
00142       ethercatMaster->receiveProcessData();
00143     }
00144     for (int i = 0; i < dof; i++) {
00145       myTrace[i].updateTrace();
00146     }
00147     SLEEP_MICROSEC(updateCycle);
00148   }
00149   
00150   for (int i = 0; i < dof; i++) {
00151     myTrace[i].stopTrace();
00152     myTrace[i].plotTrace();
00153   }
00154 
00155 }
Generated by  doxygen 1.6.3