YouBotBaseTestWithoutThread.cpp

Go to the documentation of this file.
00001 #include "YouBotBaseTestWithoutThread.hpp"
00002 
00003 using namespace youbot;
00004 
00005 YouBotBaseTestWithoutThread::YouBotBaseTestWithoutThread() {
00006   
00007 
00008 }
00009 
00010 YouBotBaseTestWithoutThread::~YouBotBaseTestWithoutThread() {
00011 }
00012 
00013 void YouBotBaseTestWithoutThread::setUp() {
00014 
00015   Logger::logginLevel = trace;
00016   ethercatMaster = &EthercatMaster::getInstance("youbot-ethercat.cfg", "../config/", false);
00017   if(ethercatMaster->isThreadActive()){
00018     LOG(error) << "Thread Active";
00019     EthercatMaster::destroy();
00020     ethercatMaster = &EthercatMaster::getInstance("youbot-ethercat.cfg", "../config/", false);
00021   }
00022   
00023   jointNO = 4;
00024   stepStartTime = 1000;
00025   durationNull = 1000;
00026   overallTime = 0;
00027   startTime = 0;
00028   updateCycle = 2000;
00029   setAngle.angle = 0 * radian;
00030   setVel.angularVelocity = 0 * radian_per_second;
00031   currentSetpoint.current = 0 * ampere;
00032 }
00033 
00034 void YouBotBaseTestWithoutThread::tearDown() {
00035 
00036   EthercatMaster::destroy();
00037 }
00038 
00039 void YouBotBaseTestWithoutThread::YouBotBaseTestWithoutThread_PositionMode() {
00040   LOG(info) <<__func__<< "\n";
00041   YouBotBase myBase("youbot-base");
00042   myBase.doJointCommutation();
00043   DataTrace myTrace(myBase.getBaseJoint(jointNO), __func__, true);
00044   myBase.getBaseJoint(jointNO).setEncoderToZero();
00045   if (!ethercatMaster->isThreadActive()) {
00046     ethercatMaster->sendProcessData();
00047     ethercatMaster->receiveProcessData();
00048   }
00049   myTrace.startTrace();
00050   
00051   ClearMotorControllerTimeoutFlag clearTimeoutFlag;
00052   myBase.getBaseJoint(1).setConfigurationParameter(clearTimeoutFlag);
00053   myBase.getBaseJoint(2).setConfigurationParameter(clearTimeoutFlag);
00054   myBase.getBaseJoint(3).setConfigurationParameter(clearTimeoutFlag);
00055   myBase.getBaseJoint(4).setConfigurationParameter(clearTimeoutFlag);
00056   
00057   startTime = myTrace.getTimeDurationMilliSec();
00058   overallTime = startTime + durationNull + stepStartTime + durationNull;
00059 
00060   while (myTrace.getTimeDurationMilliSec() < overallTime) {
00061     if (myTrace.getTimeDurationMilliSec() > startTime + durationNull) {
00062       setAngle.angle = 0 * radian;
00063     }
00064     if (myTrace.getTimeDurationMilliSec() > startTime + durationNull
00065             && myTrace.getTimeDurationMilliSec() < startTime + durationNull + stepStartTime) {
00066       setAngle.angle = 2 * radian;
00067     }
00068     if (myTrace.getTimeDurationMilliSec() > startTime + durationNull + stepStartTime) {
00069       setAngle.angle = 0 * radian;
00070     }
00071 
00072     myBase.getBaseJoint(jointNO).setData(setAngle);
00073     if (!ethercatMaster->isThreadActive()) {
00074       ethercatMaster->sendProcessData();
00075       ethercatMaster->receiveProcessData();
00076     }
00077     myTrace.updateTrace(setAngle);
00078 
00079     SLEEP_MICROSEC(updateCycle);
00080   }
00081   myTrace.stopTrace();
00082   myTrace.plotTrace();
00083 }
00084 
00085 void YouBotBaseTestWithoutThread::YouBotBaseTestWithoutThread_VelocityMode() {
00086   LOG(info) <<__func__<< "\n";
00087   YouBotBase myBase("youbot-base");
00088   myBase.doJointCommutation();
00089   DataTrace myTrace(myBase.getBaseJoint(jointNO), __func__, true);
00090   myBase.getBaseJoint(jointNO).setEncoderToZero();
00091   if (!ethercatMaster->isThreadActive()) {
00092     ethercatMaster->sendProcessData();
00093     ethercatMaster->receiveProcessData();
00094   }
00095   myTrace.startTrace();
00096   
00097   ClearMotorControllerTimeoutFlag clearTimeoutFlag;
00098   myBase.getBaseJoint(1).setConfigurationParameter(clearTimeoutFlag);
00099   myBase.getBaseJoint(2).setConfigurationParameter(clearTimeoutFlag);
00100   myBase.getBaseJoint(3).setConfigurationParameter(clearTimeoutFlag);
00101   myBase.getBaseJoint(4).setConfigurationParameter(clearTimeoutFlag);
00102   
00103   startTime = myTrace.getTimeDurationMilliSec();
00104   overallTime = startTime + durationNull + stepStartTime + durationNull;
00105 
00106   while (myTrace.getTimeDurationMilliSec() < overallTime) {
00107     if (myTrace.getTimeDurationMilliSec() > startTime + durationNull) {
00108       setVel.angularVelocity = 0 * radian_per_second;
00109     }
00110     if (myTrace.getTimeDurationMilliSec() > startTime + durationNull
00111             && myTrace.getTimeDurationMilliSec() < startTime + durationNull + stepStartTime) {
00112       setVel.angularVelocity = 2 * radian_per_second;
00113     }
00114     if (myTrace.getTimeDurationMilliSec() > startTime + durationNull + stepStartTime) {
00115       setVel.angularVelocity = 0 * radian_per_second;
00116     }
00117 
00118     myBase.getBaseJoint(jointNO).setData(setVel);
00119     if (!ethercatMaster->isThreadActive()) {
00120       ethercatMaster->sendProcessData();
00121       ethercatMaster->receiveProcessData();
00122     }
00123     myTrace.updateTrace(setVel);
00124 
00125     SLEEP_MICROSEC(updateCycle);
00126   }
00127   myTrace.stopTrace();
00128   myTrace.plotTrace();
00129 }
00130 
00131 void YouBotBaseTestWithoutThread::YouBotBaseTestWithoutThread_CurrentMode() {
00132   LOG(info) <<__func__<< "\n";
00133   YouBotBase myBase("youbot-base");
00134   myBase.doJointCommutation();
00135   DataTrace myTrace(myBase.getBaseJoint(jointNO), __func__, true);
00136   myBase.getBaseJoint(jointNO).setEncoderToZero();
00137   if (!ethercatMaster->isThreadActive()) {
00138     ethercatMaster->sendProcessData();
00139     ethercatMaster->receiveProcessData();
00140   }
00141   myTrace.startTrace();
00142   
00143   ClearMotorControllerTimeoutFlag clearTimeoutFlag;
00144   myBase.getBaseJoint(1).setConfigurationParameter(clearTimeoutFlag);
00145   myBase.getBaseJoint(2).setConfigurationParameter(clearTimeoutFlag);
00146   myBase.getBaseJoint(3).setConfigurationParameter(clearTimeoutFlag);
00147   myBase.getBaseJoint(4).setConfigurationParameter(clearTimeoutFlag);
00148   
00149   startTime = myTrace.getTimeDurationMilliSec();
00150   overallTime = startTime + durationNull + stepStartTime + durationNull;
00151 
00152   while (myTrace.getTimeDurationMilliSec() < overallTime) {
00153     if (myTrace.getTimeDurationMilliSec() > startTime + durationNull) {
00154       currentSetpoint.current = 0 * ampere;
00155     }
00156     if (myTrace.getTimeDurationMilliSec() > startTime + durationNull
00157             && myTrace.getTimeDurationMilliSec() < startTime + durationNull + stepStartTime) {
00158       currentSetpoint.current = 0.5 * ampere;
00159     }
00160     if (myTrace.getTimeDurationMilliSec() > startTime + durationNull + stepStartTime) {
00161       currentSetpoint.current = 0 * ampere;
00162     }
00163 
00164     myBase.getBaseJoint(jointNO).setData(currentSetpoint);
00165     if (!ethercatMaster->isThreadActive()) {
00166       ethercatMaster->sendProcessData();
00167       ethercatMaster->receiveProcessData();
00168     }
00169     myTrace.updateTrace(currentSetpoint);
00170 
00171     SLEEP_MICROSEC(updateCycle);
00172   }
00173   myTrace.stopTrace();
00174   myTrace.plotTrace();
00175 }