YouBotArmTest.cpp
Go to the documentation of this file.00001 #include "YouBotArmTest.hpp"
00002
00003 using namespace youbot;
00004
00005 YouBotArmTest::YouBotArmTest():dof(5) {
00006
00007 EthercatMaster::getInstance("youbot-ethercat.cfg", "../config/", true);
00008
00009
00010 }
00011
00012 YouBotArmTest::~YouBotArmTest() {
00013
00014 }
00015
00016 void YouBotArmTest::setUp() {
00017 Logger::logginLevel = trace;
00018 updateCycle = 2000;
00019
00020 }
00021
00022 void YouBotArmTest::tearDown() {
00023
00024 }
00025
00026 void YouBotArmTest::youBotArmTest() {
00027
00028 LOG(info) << __func__ << "\n";
00029 YouBotManipulator myArm("youbot-manipulator");
00030 myArm.doJointCommutation();
00031 myArm.calibrateManipulator();
00032
00033 std::stringstream jointNameStream;
00034 boost::ptr_vector<DataTrace> myTrace;
00035 std::vector<JointAngleSetpoint> upstretchedpose;
00036 std::vector<JointAngleSetpoint> foldedpose;
00037 JointAngleSetpoint desiredJointAngle;
00038
00039 desiredJointAngle.angle = 2.56244 * radian;
00040 upstretchedpose.push_back(desiredJointAngle);
00041 desiredJointAngle.angle = 1.04883 * radian;
00042 upstretchedpose.push_back(desiredJointAngle);
00043 desiredJointAngle.angle = -2.43523 * radian;
00044 upstretchedpose.push_back(desiredJointAngle);
00045 desiredJointAngle.angle = 1.73184 * radian;
00046 upstretchedpose.push_back(desiredJointAngle);
00047 desiredJointAngle.angle = 1.5 * radian;
00048 upstretchedpose.push_back(desiredJointAngle);
00049
00050 desiredJointAngle.angle = 0.11 * radian;
00051 foldedpose.push_back(desiredJointAngle);
00052 desiredJointAngle.angle = 0.11 * radian;
00053 foldedpose.push_back(desiredJointAngle);
00054 desiredJointAngle.angle = -0.11 * radian;
00055 foldedpose.push_back(desiredJointAngle);
00056 desiredJointAngle.angle = 0.11 * radian;
00057 foldedpose.push_back(desiredJointAngle);
00058 desiredJointAngle.angle = 0.2 * radian;
00059 foldedpose.push_back(desiredJointAngle);
00060
00061
00062 for (int i = 1; i <= dof; i++) {
00063 jointNameStream << "Joint_" << i << "_" << __func__;
00064 myTrace.push_back(new DataTrace(myArm.getArmJoint(i), jointNameStream.str(), true));
00065 jointNameStream.str("");
00066 }
00067
00068
00069 for (int i = 0; i < dof; i++) {
00070 myTrace[i].startTrace();
00071 }
00072
00073
00074 startTime = myTrace[0].getTimeDurationMilliSec();
00075 overallTime = startTime + 1000;
00076 while (myTrace[0].getTimeDurationMilliSec() < overallTime) {
00077 for (int i = 0; i < dof; i++) {
00078 myTrace[i].updateTrace();
00079 }
00080 SLEEP_MICROSEC(updateCycle);
00081 }
00082
00083
00084 startTime = myTrace[0].getTimeDurationMilliSec();
00085 overallTime = startTime + 5000;
00086 myArm.setJointData(upstretchedpose);
00087 while (myTrace[0].getTimeDurationMilliSec() < overallTime) {
00088 for (int i = 0; i < dof; i++) {
00089 myTrace[i].updateTrace();
00090 }
00091 SLEEP_MICROSEC(updateCycle);
00092 }
00093
00094
00095 startTime = myTrace[0].getTimeDurationMilliSec();
00096 overallTime = startTime + 5000;
00097 myArm.setJointData(foldedpose);
00098 while (myTrace[0].getTimeDurationMilliSec() < overallTime) {
00099 for (int i = 0; i < dof; i++) {
00100 myTrace[i].updateTrace();
00101 }
00102 SLEEP_MICROSEC(updateCycle);
00103 }
00104
00105
00106 startTime = myTrace[0].getTimeDurationMilliSec();
00107 overallTime = startTime + 1000;
00108 while (myTrace[0].getTimeDurationMilliSec() < overallTime) {
00109 for (int i = 0; i < dof; i++) {
00110 myTrace[i].updateTrace();
00111 }
00112 SLEEP_MICROSEC(updateCycle);
00113 }
00114
00115 for (int i = 0; i < dof; i++) {
00116 myTrace[i].stopTrace();
00117 myTrace[i].plotTrace();
00118 }
00119
00120 }