41 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
44 #include <visp3/core/vpConfig.h>
46 #ifndef DOXYGEN_SHOULD_SKIP_THIS
47 #ifdef VISP_HAVE_BICLOPS
50 #include <visp3/robot/vpRobotBiclops.h>
51 #include <visp3/robot/vpRobotBiclopsController.h>
52 #include <visp3/robot/vpRobotException.h>
57 #include <visp3/core/vpDebug.h>
66 vpRobotBiclopsController::vpRobotBiclopsController()
67 : biclops(), axisMask(0), panAxis(NULL), tiltAxis(NULL), vergeAxis(NULL), panProfile(), tiltProfile(), vergeProfile(),
68 shm(), stopControllerThread_(false)
70 axisMask = Biclops::PanMask + Biclops::TiltMask
76 biclops.SetDebugLevel(0);
83 shm.jointLimit[i] =
false;
93 vpRobotBiclopsController::~vpRobotBiclopsController() {}
108 void vpRobotBiclopsController::init(
const std::string &configfile)
112 for (
int i = 0; i < 1; i++) {
114 std::cout <<
"Try to initialize biclops head " << std::endl;
115 binit = biclops.Initialize(configfile.c_str());
119 std::cout <<
"Initialization succeed...\n";
122 std::cout <<
"Initialization failed...\n";
125 std::cout <<
"Initialization failed..." << std::endl;
130 std::cout <<
"Cannot initialize biclops head. " << std::endl;
131 std::cout <<
"Check if the serial cable is connected." << std::endl;
132 std::cout <<
"Check if the robot is powered on." << std::endl;
133 std::cout <<
"Check if you try to open the good serial port." << std::endl;
134 std::cout <<
"Try to power off/on and restart..." << std::endl;
142 panAxis = biclops.GetAxis(Biclops::Pan);
143 tiltAxis = biclops.GetAxis(Biclops::Tilt);
144 if ((axisMask & Biclops::VergeMask) != 0)
145 vergeAxis = biclops.GetAxis(Biclops::Verge);
147 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
148 if (!panAxis->GetHomedState() || !tiltAxis->GetHomedState()) {
152 if (!panAxis->IsAlreadyHomed() || !tiltAxis->IsAlreadyHomed()) {
158 vpDEBUG_TRACE(12,
"Execute the homing sequence for all axes");
159 vpDEBUG_TRACE(12,
"Execute the homing sequence for all axes");
160 if (biclops.HomeAxes(axisMask))
169 panAxis->GetProfile(panProfile);
170 tiltAxis->GetProfile(tiltProfile);
171 if ((axisMask & Biclops::VergeMask) != 0)
172 vergeAxis->GetProfile(vergeProfile);
190 void vpRobotBiclopsController::setPosition(
const vpColVector &q,
const double percentVelocity)
197 panAxis->SetProfileMode(PMDTrapezoidalProfile);
198 tiltAxis->SetProfileMode(PMDTrapezoidalProfile);
201 PMDUtils::AxisList axisList;
202 axisList.push_back(panAxis);
203 axisList.push_back(tiltAxis);
205 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
208 panAxis->GetProfile(panProfile);
209 tiltAxis->GetProfile(tiltProfile);
214 panProfile.pos = PMDUtils::RadsToRevs(q[0]);
217 tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
221 panAxis->SetProfile(panProfile);
222 tiltAxis->SetProfile(tiltProfile);
226 PMDAxisControl::CountsProfile desired_profile;
231 panProfile.pos = PMDUtils::RadsToRevs(q[0]);
236 panAxis->ProfileToCounts(panProfile, desired_profile);
237 vpCDEBUG(12) <<
"desired_profile.pos: " << desired_profile.pos << std::endl;
238 vpCDEBUG(12) <<
"desired_profile.vel: " << desired_profile.vel << std::endl;
240 panAxis->SetProfile(desired_profile);
245 tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
248 tiltAxis->ProfileToCounts(tiltProfile, desired_profile);
249 vpCDEBUG(12) <<
"desired_profile.pos: " << desired_profile.pos << std::endl;
250 vpCDEBUG(12) <<
"desired_profile.vel: " << desired_profile.vel << std::endl;
252 tiltAxis->SetProfile(desired_profile);
256 PMDUtils::Coordinate(axisList);
257 biclops.Move(Biclops::PanMask + Biclops::TiltMask );
269 void vpRobotBiclopsController::setVelocity(
const vpColVector &q_dot)
276 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
279 panAxis->GetProfile(panProfile);
280 tiltAxis->GetProfile(tiltProfile);
285 panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
286 tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
289 panAxis->SetProfile(panProfile);
290 tiltAxis->SetProfile(tiltProfile);
292 panAxis->SetProfileMode(PMDVelocityContouringProfile);
293 tiltAxis->SetProfileMode(PMDVelocityContouringProfile);
295 panAxis->SetProfileMode(PMDVelocityContouringProfile);
296 tiltAxis->SetProfileMode(PMDVelocityContouringProfile);
298 PMDAxisControl::CountsProfile desired_profile;
303 panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
305 panAxis->ProfileToCounts(panProfile, desired_profile);
306 panAxis->SetProfile(desired_profile);
311 tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
313 tiltAxis->ProfileToCounts(tiltProfile, desired_profile);
314 tiltAxis->SetProfile(desired_profile);
317 biclops.Move(Biclops::PanMask + Biclops::TiltMask, 0);
327 vpColVector vpRobotBiclopsController::getPosition()
329 vpDEBUG_TRACE(12,
"Start vpRobotBiclopsController::getPosition() ");
331 PMDint32 panpos, tiltpos;
333 panAxis->GetPosition(panpos);
334 tiltAxis->GetPosition(tiltpos);
336 q[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(panpos));
337 q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
339 vpCDEBUG(11) <<
"++++++++ Mesure : " << q.t();
340 vpDEBUG_TRACE(12,
"End vpRobotBiclopsController::getPosition()");
352 vpColVector vpRobotBiclopsController::getActualPosition()
355 PMDint32 panpos, tiltpos;
357 panAxis->GetActualPosition(panpos);
358 tiltAxis->GetActualPosition(tiltpos);
360 q[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(panpos));
361 q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
373 vpColVector vpRobotBiclopsController::getVelocity()
376 PMDint32 pan_vel, tilt_vel;
378 panAxis->GetVelocity(pan_vel);
379 tiltAxis->GetVelocity(tilt_vel);
381 q_dot[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(pan_vel));
382 q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
394 vpColVector vpRobotBiclopsController::getActualVelocity()
397 PMDint32 pan_vel, tilt_vel;
399 panAxis->GetActualVelocity(pan_vel);
400 tiltAxis->GetActualVelocity(tilt_vel);
402 q_dot[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(pan_vel));
403 q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
414 void vpRobotBiclopsController::writeShm(shmType &shm_)
419 memcpy(&this->shm, &shm_,
sizeof(shmType));
432 vpRobotBiclopsController::shmType vpRobotBiclopsController::readShm()
439 memcpy(&tmp_shm, &this->shm,
sizeof(shmType));
442 vpDEBUG_TRACE(13,
"tmp_shm.q_dot[%d]=%f", i, tmp_shm.q_dot[i]);
448 #elif !defined(VISP_BUILD_SHARED_LIBS)
451 void dummy_vpRobotBiclopsController(){};
454 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS