50 #include <visp/vpConfig.h>
62 #ifdef VISP_HAVE_CYCAB
63 #include <visp/vpRobotCycab.h>
64 #include <visp/vpMath.h>
65 #include <visp/vpTime.h>
77 printf(
"Received signal %d\n",n);
82 #define MAXV 1.5 // velocity in m/S
83 #define MAXPHI 20.0 // front steering angle in deg
84 #define MAX_ACC_V 6 // m/s^2
85 #define MAX_VEL_PHI 4 // rad/s
98 signal(SIGINT,sighdl);
99 signal(SIGTERM,sighdl);
100 signal(SIGPIPE,sighdl);
113 cycab->getOdometry(vm, phim, timestamp);
115 printf(
"State: t=%.1f s v=%f m/s and phi=%f deg\n\t",
120 v+=kv*0.002;
if (fabs(v)>=MAXV) kv = -1 * kv;
121 phi+=kphi*0.002;
if (fabs(phi)>=
vpMath::rad(MAXPHI)) kphi = -1 * kphi;
131 std::cout <<
"Cycab stop requested" << std::endl;
135 if (std::fabs(vm) > std::numeric_limits<double>::epsilon())
136 sign_v = fabs(vm)/vm;
137 v = vm - MAX_ACC_V*(t1-tprev)/1000*sign_v;
138 if (fabs(v) < 0.1) v = 0;
143 if (std::fabs(phim) > std::numeric_limits<double>::epsilon())
144 sign_phi = fabs(phim)/phim;
145 phi = phim - MAX_VEL_PHI*(t1-tprev)/1000*sign_phi;
155 printf(
"Send : v %f m/s and phi %f deg\n", v,
vpMath::deg(phi));
156 cycab->setCommand(v, phi);
160 if (end && (!ctrc)) { end =
false; ctrc=
true;}
162 std::cout <<
"The end" << std::endl;
166 #else // VISP_HAVE_CYCAB
169 cout <<
"Sorry no acces to the cycab" << endl;
171 #endif //VISP_HAVE_CYCAB
static double measureTimeMs()
static int wait(double t0, double t)
Interface for the car-like Cycab mobile robot.
static double rad(double deg)
static double deg(double rad)