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